Skip to content

Commit

Permalink
Merge pull request #36 from Multiverse-Framework/FixWeld
Browse files Browse the repository at this point in the history 
Fix weld
  • Loading branch information
@HoangGiang93
HoangGiang93 authored Aug 6, 2024
2 parents 308585c + 8da42f1 commit 182ed6c
Show file tree
Hide file tree
Showing 77 changed files with 198,355 additions and 60 deletions.
2 changes: 2 additions & 0 deletions multiverse/modules/multiverse_connectors/src/mujoco/include/mj_multiverse_client.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,8 @@ class MjMultiverseClient final : public MultiverseClientJson

std::vector<std::string> get_exist_response(const Json::Value &arguments) const;

std::string get_set_control_value_response(const Json::Value &arguments) const;

public:
bool communicate(const bool resend_meta_data = false) override;

Expand Down
167 changes: 124 additions & 43 deletions multiverse/modules/multiverse_connectors/src/mujoco/src/mj_multiverse_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1065,23 +1065,23 @@ void MjMultiverseClient::bind_response_meta_data()

std::string mocap_pos_str;
std::string mocap_quat_str;
for (int mocap_pos_id = 0; mocap_pos_id < m->nmocap; mocap_pos_id++)
{
mocap_pos_str += std::to_string(d->mocap_pos[3 * mocap_pos_id]) + " ";
mocap_pos_str += std::to_string(d->mocap_pos[3 * mocap_pos_id + 1]) + " ";
mocap_pos_str += std::to_string(d->mocap_pos[3 * mocap_pos_id + 2]) + " ";
m->key_mpos[3 * mocap_pos_id] = d->mocap_pos[3 * mocap_pos_id];
m->key_mpos[3 * mocap_pos_id + 1] = d->mocap_pos[3 * mocap_pos_id + 1];
m->key_mpos[3 * mocap_pos_id + 2] = d->mocap_pos[3 * mocap_pos_id + 2];

mocap_quat_str += std::to_string(d->mocap_quat[4 * mocap_pos_id]) + " ";
mocap_quat_str += std::to_string(d->mocap_quat[4 * mocap_pos_id + 1]) + " ";
mocap_quat_str += std::to_string(d->mocap_quat[4 * mocap_pos_id + 2]) + " ";
mocap_quat_str += std::to_string(d->mocap_quat[4 * mocap_pos_id + 3]) + " ";
m->key_mquat[4 * mocap_pos_id] = d->mocap_quat[4 * mocap_pos_id];
m->key_mquat[4 * mocap_pos_id + 1] = d->mocap_quat[4 * mocap_pos_id + 1];
m->key_mquat[4 * mocap_pos_id + 2] = d->mocap_quat[4 * mocap_pos_id + 2];
m->key_mquat[4 * mocap_pos_id + 3] = d->mocap_quat[4 * mocap_pos_id + 3];
for (int mocap_id = 0; mocap_id < m->nmocap; mocap_id++)
{
mocap_pos_str += std::to_string(d->mocap_pos[3 * mocap_id]) + " ";
mocap_pos_str += std::to_string(d->mocap_pos[3 * mocap_id + 1]) + " ";
mocap_pos_str += std::to_string(d->mocap_pos[3 * mocap_id + 2]) + " ";
m->key_mpos[3 * mocap_id] = d->mocap_pos[3 * mocap_id];
m->key_mpos[3 * mocap_id + 1] = d->mocap_pos[3 * mocap_id + 1];
m->key_mpos[3 * mocap_id + 2] = d->mocap_pos[3 * mocap_id + 2];

mocap_quat_str += std::to_string(d->mocap_quat[4 * mocap_id]) + " ";
mocap_quat_str += std::to_string(d->mocap_quat[4 * mocap_id + 1]) + " ";
mocap_quat_str += std::to_string(d->mocap_quat[4 * mocap_id + 2]) + " ";
mocap_quat_str += std::to_string(d->mocap_quat[4 * mocap_id + 3]) + " ";
m->key_mquat[4 * mocap_id] = d->mocap_quat[4 * mocap_id];
m->key_mquat[4 * mocap_id + 1] = d->mocap_quat[4 * mocap_id + 1];
m->key_mquat[4 * mocap_id + 2] = d->mocap_quat[4 * mocap_id + 2];
m->key_mquat[4 * mocap_id + 3] = d->mocap_quat[4 * mocap_id + 3];
}
if (mocap_pos_str.size() > 0)
{
Expand Down Expand Up @@ -1120,7 +1120,26 @@ void MjMultiverseClient::weld(const Json::Value &arguments)
const std::string object_1_name = arguments[0].asString();
const std::string object_2_name = arguments[1].asString();

std::string relative_pose = "0.0 0.0 0.0 1.0 0.0 0.0 0.0";
const int body_1_id = mj_name2id(m, mjtObj::mjOBJ_BODY, object_2_name.c_str());
const int body_2_id = mj_name2id(m, mjtObj::mjOBJ_BODY, object_1_name.c_str());

mjtNum *body_1_pos = d->xpos + 3 * body_1_id;
mjtNum *body_1_quat = d->xquat + 4 * body_1_id;
mjtNum body_1_neg_quat[4];
mju_negQuat(body_1_neg_quat, body_1_quat);

mjtNum *body_2_pos = d->xpos + 3 * body_2_id;
mjtNum *body_2_quat = d->xquat + 4 * body_2_id;

mjtNum body_2_in_1_pos[3];
mjtNum body_2_in_1_quat[4];

mju_sub3(body_2_in_1_pos, body_2_pos, body_1_pos);
mju_rotVecQuat(body_2_in_1_pos, body_2_in_1_pos, body_1_neg_quat);

mju_mulQuat(body_2_in_1_quat, body_1_neg_quat, body_2_quat);

std::string relative_pose = std::to_string(body_2_in_1_pos[0]) + " " + std::to_string(body_2_in_1_pos[1]) + " " + std::to_string(body_2_in_1_pos[2]) + " " + std::to_string(body_2_in_1_quat[0]) + " " + std::to_string(body_2_in_1_quat[1]) + " " + std::to_string(body_2_in_1_quat[2]) + " " + std::to_string(body_2_in_1_quat[3]);
if (arguments.size() == 3)
{
relative_pose = arguments[2].asString();
Expand Down Expand Up @@ -1196,16 +1215,19 @@ std::string MjMultiverseClient::get_weld_response(const Json::Value &arguments)
const std::string object_1_name = arguments[0].asString();
const std::string object_2_name = arguments[1].asString();

if (mj_name2id(m, mjtObj::mjOBJ_BODY, object_1_name.c_str()) == -1)
const int body_1_id = mj_name2id(m, mjtObj::mjOBJ_BODY, object_2_name.c_str());
const int body_2_id = mj_name2id(m, mjtObj::mjOBJ_BODY, object_1_name.c_str());

if (body_2_id == -1)
{
return "failed (Object " + object_1_name + " does not exist.)";
}
if (mj_name2id(m, mjtObj::mjOBJ_BODY, object_2_name.c_str()) == -1)
if (body_1_id == -1)
{
return "failed (Object " + object_2_name + " does not exist.)";
}

std::vector<mjtNum> relative_pose = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0};
std::vector<mjtNum> relative_pose;
if (arguments.size() == 3)
{
std::istringstream iss(arguments[2].asString());
Expand All @@ -1230,11 +1252,14 @@ std::string MjMultiverseClient::get_weld_response(const Json::Value &arguments)
continue;
}

for (int i = 0; i < 7; i++)
if (arguments.size() == 3)
{
if (mju_abs(relative_pose[i] - m->eq_data[mjNEQDATA * equality_id + i + 3]) > mjMINVAL)
for (int i = 0; i < 7; i++)
{
return "failed (relative pose are different)";
if (mju_abs(relative_pose[i] - m->eq_data[mjNEQDATA * equality_id + i + 3]) > 1e-3)
{
return "failed (relative pose are different)";
}
}
}

Expand Down Expand Up @@ -1428,7 +1453,25 @@ void MjMultiverseClient::attach(const Json::Value &arguments)
const std::string object_1_name = arguments[0].asString();
const std::string object_2_name = arguments[1].asString();

std::vector<mjtNum> relative_pose = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0};
const int body_1_id = mj_name2id(m, mjtObj::mjOBJ_BODY, object_2_name.c_str());
const int body_2_id = mj_name2id(m, mjtObj::mjOBJ_BODY, object_1_name.c_str());

mjtNum *body_1_pos = d->xpos + 3 * body_1_id;
mjtNum *body_1_quat = d->xquat + 4 * body_1_id;
mjtNum body_1_neg_quat[4];
mju_negQuat(body_1_neg_quat, body_1_quat);

mjtNum *body_2_pos = d->xpos + 3 * body_2_id;
mjtNum *body_2_quat = d->xquat + 4 * body_2_id;

mjtNum body_2_in_1_pos[3];
mjtNum body_2_in_1_quat[4];

mju_sub3(body_2_in_1_pos, body_2_pos, body_1_pos);
mju_rotVecQuat(body_2_in_1_pos, body_2_in_1_pos, body_1_neg_quat);

mju_mulQuat(body_2_in_1_quat, body_1_neg_quat, body_2_quat);
std::vector<mjtNum> relative_pose = {body_2_in_1_pos[0], body_2_in_1_pos[1], body_2_in_1_pos[2], body_2_in_1_quat[0], body_2_in_1_quat[1], body_2_in_1_quat[2], body_2_in_1_quat[3]};
if (arguments.size() == 3)
{
std::istringstream iss(arguments[2].asString());
Expand Down Expand Up @@ -1575,28 +1618,24 @@ std::string MjMultiverseClient::get_attach_response(const Json::Value &arguments
return "failed (Object " + object_2_name + " does not exist.)";
}

std::vector<mjtNum> relative_pose = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0};
if (arguments.size() == 3)
{
std::istringstream iss(arguments[2].asString());
relative_pose = std::vector<mjtNum>(std::istream_iterator<mjtNum>(iss), std::istream_iterator<mjtNum>());
}
if (m->body_parentid[body_1_id] == body_2_id)
{
if (m->body_pos[3 * body_1_id] == relative_pose[0] &&
m->body_pos[3 * body_1_id + 1] == relative_pose[1] &&
m->body_pos[3 * body_1_id + 2] == relative_pose[2] &&
m->body_quat[4 * body_1_id] == relative_pose[3] &&
m->body_quat[4 * body_1_id + 1] == relative_pose[4] &&
m->body_quat[4 * body_1_id + 2] == relative_pose[5] &&
m->body_quat[4 * body_1_id + 3] == relative_pose[6])
{
return "success";
}
else
if (arguments.size() == 3)
{
return "failed (relative pose are different)";
std::istringstream iss(arguments[2].asString());
std::vector<mjtNum> relative_pose = std::vector<mjtNum>(std::istream_iterator<mjtNum>(iss), std::istream_iterator<mjtNum>());
if (m->body_pos[3 * body_1_id] != relative_pose[0] ||
m->body_pos[3 * body_1_id + 1] != relative_pose[1] ||
m->body_pos[3 * body_1_id + 2] != relative_pose[2] ||
m->body_quat[4 * body_1_id] != relative_pose[3] ||
m->body_quat[4 * body_1_id + 1] != relative_pose[4] ||
m->body_quat[4 * body_1_id + 2] != relative_pose[5] ||
m->body_quat[4 * body_1_id + 3] != relative_pose[6])
{
return "failed (relative pose are different)";
}
}
return "success";
}
else
{
Expand Down Expand Up @@ -2002,6 +2041,44 @@ std::vector<std::string> MjMultiverseClient::get_exist_response(const Json::Valu
return exist_results;
}

std::string MjMultiverseClient::get_set_control_value_response(const Json::Value &arguments) const
{
if (!arguments.isArray())
{
return "failed (Arguments for set_control_value should be an array of strings.)";
}
if (arguments.size() % 2 != 0)
{
return "failed (Arguments for set_control_value should be an array of strings with an even number of elements.)";
}

for (int i = 0; i < arguments.size(); i += 2)
{
if (!arguments[i].isString())
{
return "failed (Control name should be a string.)";
}
if (!arguments[i + 1].isString())
{
return "failed (Control value should be a number.)";
}

const std::string control_name = arguments[i].asString();
const mjtNum control_value = std::stod(arguments[i + 1].asString());

const int control_id = mj_name2id(m, mjtObj::mjOBJ_ACTUATOR, control_name.c_str());
if (control_id == -1)
{
return "failed (Control " + control_name + " does not exist.)";
}

d->ctrl[control_id] = control_value;
m->key_ctrl[control_id] = control_value;
}

return "success";
}

void MjMultiverseClient::bind_api_callbacks()
{
const Json::Value &api_callbacks_json = response_meta_data_json["api_callbacks"];
Expand Down Expand Up @@ -2096,6 +2173,10 @@ void MjMultiverseClient::bind_api_callbacks_response()
api_callback_response[api_callback_name].append(exist_response);
}
}
else if (strcmp(api_callback_name.c_str(), "set_control_value") == 0)
{
api_callback_response[api_callback_name].append(get_set_control_value_response(api_callback_json[api_callback_name]));
}
else if (strcmp(api_callback_name.c_str(), "pause") == 0)
{
pause = true;
Expand Down
29 changes: 22 additions & 7 deletions multiverse/modules/multiverse_connectors/src/mujoco/src/mj_simulate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,15 +91,30 @@ void MjSimulate::load_new_model_and_keep_old_data()
}

// Copy body position
d_new->xpos[body_id_new] = d->xpos[body_id];
d_new->xpos[body_id_new + 1] = d->xpos[body_id + 1];
d_new->xpos[body_id_new + 2] = d->xpos[body_id + 2];
d_new->xpos[3 * body_id_new] = d->xpos[3 * body_id];
d_new->xpos[3 * body_id_new + 1] = d->xpos[3 * body_id + 1];
d_new->xpos[3 * body_id_new + 2] = d->xpos[3 * body_id + 2];

// Copy body rotation
d_new->xquat[body_id_new] = d->xquat[body_id];
d_new->xquat[body_id_new + 1] = d->xquat[body_id + 1];
d_new->xquat[body_id_new + 2] = d->xquat[body_id + 2];
d_new->xquat[body_id_new + 3] = d->xquat[body_id + 3];
d_new->xquat[4 * body_id_new] = d->xquat[4 * body_id];
d_new->xquat[4 * body_id_new + 1] = d->xquat[4 * body_id + 1];
d_new->xquat[4 * body_id_new + 2] = d->xquat[4 * body_id + 2];
d_new->xquat[4 * body_id_new + 3] = d->xquat[4 * body_id + 3];

// Copy mocap position
const int mocap_id = m->body_mocapid[body_id];
const int mocap_id_new = m_new->body_mocapid[body_id_new];
if (mocap_id != -1 && mocap_id_new != -1)
{
d_new->mocap_pos[3 * mocap_id_new] = d->mocap_pos[3 * mocap_id];
d_new->mocap_pos[3 * mocap_id_new + 1] = d->mocap_pos[3 * mocap_id + 1];
d_new->mocap_pos[3 * mocap_id_new + 2] = d->mocap_pos[3 * mocap_id + 2];

d_new->mocap_quat[4 * mocap_id_new] = d->mocap_quat[4 * mocap_id];
d_new->mocap_quat[4 * mocap_id_new + 1] = d->mocap_quat[4 * mocap_id + 1];
d_new->mocap_quat[4 * mocap_id_new + 2] = d->mocap_quat[4 * mocap_id + 2];
d_new->mocap_quat[4 * mocap_id_new + 3] = d->mocap_quat[4 * mocap_id + 3];
}

// Copy body states
for (int body_nr = 0; body_nr < 6; body_nr++)
Expand Down
4 changes: 1 addition & 3 deletions ...modules/multiverse_connectors/src/multiverse_client_py/tests/test_mulitverse_client_py.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -591,8 +591,6 @@ def test_multiverse_client_callapi_weld(self):
# Spawn panda and milk box
self.test_multiverse_client_spawn()

sleep(2)

# Weld milk box to hand at (0 0 0) (1 0 0 0)
multiverse_client_test_callapi = self.create_multiverse_client_callapi("1339", "world",
{
Expand Down Expand Up @@ -749,7 +747,7 @@ def test_multiverse_client_callapi_attach(self):
# Spawn panda and milk box
self.test_multiverse_client_spawn()

# Attach milk box to hand at (0 0 0) (1 0 0 0)
# Attach milk box to hand at relative pose
multiverse_client_test_callapi = self.create_multiverse_client_callapi("1339", "world",
{
"empty_simulation": [
Expand Down
82 changes: 82 additions & 0 deletions multiverse/resources/muv/apartment_reduce_with_food_with_controllers.muv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,82 @@
resources:
- ../robots
- ../worlds
- ../objects
- ../demos

worlds:
world:
rtf_desired: 1

simulations:
apartment_reduce_with_food:
simulator: mujoco
world:
name: world
path: apartment_full/mjcf/ApartmentReduceWithControllers.xml

robots:
panda:
path: mjcf/panda.xml
apply:
body:
panda:
pos: [6.7, -5.2, 0.86]
gravcomp: 1
disable_self_collision: "off"

objects:
jeroen_cup:
path: jeroen_cup/mjcf/jeroen_cup.xml
apply:
body:
jeroen_cup:
pos: [7.25, -6.5, 0.97]

small_bowl:
path: small_bowl/mjcf/small_bowl.xml
apply:
body:
small_bowl:
pos: [7.1, -5.5, 0.90]
garlic:
path: garlic/garlic.xml
apply:
body:
garlic:
pos: [7.1, -5.2, 0.90]
lime:
path: lime/lime.xml
apply:
body:
lime:
pos: [7.1, -5.1, 0.90]
kiwi_1:
path: kiwi_1/kiwi_1.xml
apply:
body:
kiwi1:
pos: [7.0, -5.2, 0.90]

references:
hand:
body1: hand_ref
body2: hand
config:
max_time_step: 0.002
min_time_step: 0.001

multiverse_server:
host: "tcp://127.0.0.1"
port: 7000

multiverse_clients:
apartment_reduce_with_food:
port: 7500
send:
body: ["position", "quaternion"]
joint: ["joint_rvalue", "joint_tvalue", "joint_angular_velocity", "joint_linear_velocity", "joint_force", "joint_torque"]
receive:
hand_ref: ["position", "quaternion"]
RightController_palm_r: ["position", "quaternion"]
LeftController_palm_l: ["position", "quaternion"]
Loading

0 comments on commit 182ed6c

Please sign in to comment.