Hello, I am confused with a simple task. I have a robot over the plane that in XML is defined as a body with two slide joints. I would like to get the robot's position in space and also set it's position when needed. If this robot was a body with a free joint then that would be easy using the qpos array as the x, y and z values are there. I have seen that when a body has a slide joint(s) then the qpos for that body does not contain the position of the body in the world but instead the value of the joint. Code: <body name="object_1" pos="0.2 0.18 3.3e-02" quat="0 0 0 1"> <geom type="cylinder" name="object_1" mass="10" friction="0.4 0.4 0.8" size="0.038 0.06" rgba="0 1 0. 1" /> <joint name="object_1_free" type="free" limited='false' damping="0" armature="0"/> </body> <body name="robot" pos="0. 0.6 -0.05" quat="0 0 0 1" > <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="1.219" diaginertia="0.21942 0.111173 0.111173" /> <joint axis="-1 0 0" damping="0." name="slider_1" pos="0 0 0" type="slide" range="-100 100"/> <joint axis="0 -1 0" damping="0." name="slider_2" pos="0 0 0" type="slide" range="-100 100"/> <geom type="mesh" rgba="0.7 0.7 0.7 1" friction="0.4 0.4 0.4" mesh="robot" /> . . . </body> I tried to print the values of the qpos array like so: Code: for(int i=0; i < model->nq; i++) { printf("%f\n", data->qpos[i]); } Code: 0.200000 0.180000 0.034027 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 As you can see object_1's qpos x position is the one of the body's position in the XML file, where for the robot the qpos x and y are 0 (I think because of the slider_1 and slider_2 joint pos values). I would like to get and set the position of the robot at any point, but I am not sure how to do this currently... I am aware of the xpos array, but as far as I am aware, I cannot use this one to set positions. In general, I have the following generic code to get and set positions of bodies: Code: double getBodyXpos(mjModel* model, mjData* data, std::string name) { int bodyId = mj_name2id(model, mjOBJ_BODY, name.c_str()); const int jointIndex = model->body_jntadr[bodyId]; const int qposIndex = model->jnt_qposadr[jointIndex]; std::lock_guard<std::mutex> lockGuard(mtx); return data->qpos[qposIndex + 0]; } double getBodyYpos(mjModel* model, mjData* data, std::string name) { int bodyId = mj_name2id(model, mjOBJ_BODY, name.c_str()); const int jointIndex = model->body_jntadr[bodyId]; const int qposIndex = model->jnt_qposadr[jointIndex]; std::lock_guard<std::mutex> lockGuard(mtx); return data->qpos[qposIndex + 1]; } void setBodyXYPosition(std::string name, mjModel* model, mjData* data, double x, double y) { int bodyId = mj_name2id(model, mjOBJ_BODY, name.c_str()); const int jointIndex = model->body_jntadr[bodyId]; const int qposIndex = model->jnt_qposadr[jointIndex]; std::lock_guard<std::mutex> lockGuard(mtx); data->qpos[qposIndex + 0] = x; data->qpos[qposIndex + 1] = y; } So what I am expecting from this code is the following: Code: getBodyXpos(m, d, "object_1"); -> To return 0.2 because of XML object_1 body position getBodyYpos(m, d, "object_1"); -> To return 0.18 because of XML object_1 body position setBodyXYPosition(m, d, "object_1", 0.1, 0.0); getBodyXpos(m, d, "object_1"); -> To return 0.1 getBodyYpos(m, d, "object_1"); -> To return 0.0 getBodyXpos(m, d, "robot"); -> To return 0.0 because of XML robot body position getBodyYpos(m, d, "robot"); -> To return 0.6 because of XML robot body position setBodyXYPosition(m, d, "robot", 0.3, 0.0); getBodyXpos(m, d, "robot"); -> To return 0.3 getBodyYpos(m, d, "robot"); -> To return 0.0 Any ideas how to deal with this? Thank you in advance.
Ok I found a function that looks promising for what I want. mj_local2Global What I don't understand is what the parameters are, and where the output is stored? Code: mj_local2Global(data, data->xpos[bodyId * 3], data->xmat[bodyId * 3], data->pos[bodyId * 3], data->quat[bodyId * 3], bodyId); I can't find enough information about this function (including an explanation of its parameters, and the output and where it's stored). Any help, please?
Any resolution to this? I'm wondering the same thing. My guess (without testing it) is that "pos" and "quat" are the position and orientation in the local frame of body "bodyId", and this function populates "xpos" and "xmat" which are the position and orientation (as a rotation matrix) in the global frame.