Dear all, My question might be related to mujoco_ros_control provided by ShadowRobot, https://github.com/shadow-robot/muj...mujoco_ros_control/src/mujoco_ros_control.cpp but still hope to have some answers from you. I try to change a object's pose on the fly via ROS service call. The object is preloaded as follows: Code: <body name="object" pos="1 0 0.6" euler="0 1.57 0"> <inertial pos="0 0 0" mass="0.179594" diaginertia="8.80012e-05 8.80012e-05 8.80012e-05"/> <joint name="OBJTx" pos="0 0 0" axis="1 0 0" type="slide" limited="false" damping="0" /> <joint name="OBJTy" pos="0 0 0" axis="0 1 0" type="slide" limited="false" damping="0" /> <joint name="OBJTz" pos="0 0 0" axis="0 0 1" type="slide" limited="false" damping="0" /> <joint name="OBJRx" pos="0 0 0" axis="1 0 0" limited="false" damping="0" /> <joint name="OBJRy" pos="0 0 0" axis="0 1 0" limited="false" damping="0" /> <joint name="OBJRz" pos="0 0 0" axis="0 0 1" limited="false" damping="0" /> <geom name="case" type="box" size="0.005 0.025 0.075" condim="4" rgba="1 1 1 1" density="1500" /> <site name="object_top" type="sphere" size="0.006" rgba="0.8 0.2 0.2 0.2" pos="0 0 0.01"/> <site name="object_bottom" type="sphere" size="0.006" rgba="0.2 0.8 0.2 0.2" pos="0 0 -0.01"/> </body> I impemented a callback function in mujoco_ros_control.cpp as below. Sometimes it successfully update the object's position & orientation but sometimes does not. I tried to add "mj_step(mujoco_model, mujoco_data);" at the end but it caused simulation crashed thus it is comment out right now. Code: bool MujocoRosControl::set_object_state_in_scene(mujoco_ros_msgs::SetModelStateRequest& rqt, mujoco_ros_msgs::SetModelStateResponse& rsp) { const int xpos_dim = 3; const int xquat_dim = 4; std::string object_name; for (std::map<int, Object_State>::iterator it = objects_in_scene_.begin(); it != objects_in_scene_.end(); it++) { object_name = mj_id2name(mujoco_model, 1, it->first); if (rqt.model_state.name == object_name) { mujoco_model->body_pos[xpos_dim * it->first] = rqt.model_state.pose.position.x; mujoco_model->body_pos[xpos_dim * it->first + 1] = rqt.model_state.pose.position.y; mujoco_model->body_pos[xpos_dim * it->first + 2] = rqt.model_state.pose.position.z; mujoco_model->body_quat[xquat_dim * it->first + 1] = rqt.model_state.pose.orientation.x; mujoco_model->body_quat[xquat_dim * it->first + 2] = rqt.model_state.pose.orientation.y; mujoco_model->body_quat[xquat_dim * it->first + 3] = rqt.model_state.pose.orientation.z; mujoco_model->body_quat[xquat_dim * it->first] = rqt.model_state.pose.orientation.w; rsp.success = true; break; }else { rsp.success = false; } } //mj_step(mujoco_model, mujoco_data); } I also found out that this callback works well with body which has no "joint" inside, so was wondering maybe some additional assignment should be added (ex. mujoco_model->...) for a successful update. Sorry for the vague expression and thanks for any information in advance.
Changing my code into below solved my problem. Hints are from: http://www.mujoco.org/forum/index.p...a-xpos-not-changing-simulation-position.4035/ http://www.mujoco.org/forum/index.php?threads/cartesian-coordinates-of-body.3376/ Code: mujoco_data->qpos[joint_addr] = rqt.model_state.pose.position.x; mujoco_data->qpos[joint_addr + 1] = rqt.model_state.pose.position.y; mujoco_data->qpos[joint_addr + 2] = rqt.model_state.pose.position.z; mujoco_data->qpos[joint_addr + 3] = rqt.model_state.pose.orientation.w; mujoco_data->qpos[joint_addr + 4] = rqt.model_state.pose.orientation.x; mujoco_data->qpos[joint_addr + 5] = rqt.model_state.pose.orientation.y; mujoco_data->qpos[joint_addr + 6] = rqt.model_state.pose.orientation.z; Thanks anyway!