Hello, is there a posibility to get the position of a specific body with respect to the worldbody between timesteps in simulation? I'm not sure whether I just missed it in the documentation or I'm already on the right way by taking the values of the corresponding fields in d->qpos of the root joint in my model which attaches the base_link (root of kinematics tree) to the worldbody. Thank you for your help!!! Best, Marc
qpos is the joint coordinate vector. You need to apply forward kinematics to get the body positions and orientations in Cartesian coordinates. This is the first thing that mj_step() and mj_forward() do. The results are written in xpos, xmat. These correspond to the body frame -- which is usually centered at the joint. If you want the body center-of-mass frame, use xipos, ximat. Similarly for geom_xpos etc. All the "x" fields of mjData correspond to Cartesian coordinates. Note that mj_step() computes the kinematics for the current qpos, fills in all the intermediate results including the above-mentioned fields, and then advances qpos. So xpos etc. corresponds to the position before mj_step().
Okay, great!! But I'm still not sure how to address xpos the right way. I assume Code: int base = mj_name2id(m, mjOBJ_BODY, "base_link"); d->xpos[base]; conatining the x-value of the position of base_link. Is this correct?
The size of xpos is nbody x 3. So the data corresponding to body n is: xpos[3*n], xpos[3*n+1], xpos[3*n+2]. Same goes for all other indices. Look at the array definition in the header file, and the comments will tell you what the size is. Maybe the "x" is confusing. It refers to Cartesian coordinates and not to the x-coordinate. So it is a 3D vector with x,y,z coordinates.
I need to translate xpos's into world coords. I inrfer (from the discussion above) that the corresponding xmat's are rotation matrices about one joint (I gotta figure out which one for bodies that have more than one joint) and that I can chain translations from some body with a known relation to world origin (and I gotta figure one that might have an anchor). I can see a way to recover world coords, but it seems indirect and a lot of work. I wonder whether there is a more direct way? Is there an SE(3) matrix somewhere I can grab or dig out of the GPU/CPU somehow? EDIT: I gather from the documentation here http://www.mujoco.org/book/programming.html#siCoordinate that the xpos coordinates are already body coordinates in the global frame. I did some scatter plots in matplotlib that lend some plausibility to that hypothesis (i am trying to track fingertips in the robot-hand model in OpenAI gym, so it's a bit hard to give full detail, here). I'd love to put 3D sprites in the glfw visualization at the locations specified by the xpos's to see where they land w.r.t. the model of the hand [the model in XML]. Is there a way to plot tracking markers directly in the glfw visualization?