Obtaining global pose of site

Hi Emo,

I have been struggling with this quite a long time. My model includes a site sphere as a point for my robot's end-effector. Within the XML file, I have a site sphere near the end-effector to represent that point.

I would like at any given time to know the global (world) pose of that site in the world. In principle, to know where the end-effector is in the world at any given time.

I tried to use the following:

Code:

int siteID = mj_name2id(m, mjOBJ_SITE, siteName.c_str());
double x = d->site_xpos[3 * siteID + 0];
double y = d->site_xpos[3 * siteID + 1];
double z = d->site_xpos[3 * siteID + 2];

double m0 = d->site_xmat[9 * siteID + 0];
double m1 = d->site_xmat[9 * siteID + 1];
double m2 = d->site_xmat[9 * siteID + 2];
double m3 = d->site_xmat[9 * siteID + 3];
double m4 = d->site_xmat[9 * siteID + 4];
double m5 = d->site_xmat[9 * siteID + 5];
double m6 = d->site_xmat[9 * siteID + 6];
double m7 = d->site_xmat[9 * siteID + 7];
double m8 = d->site_xmat[9 * siteID + 8];

Eigen::MatrixXf transform = Eigen::MatrixXf::Identity(4, 4);
transform(0, 3) = x;
transform(1, 3) = y;
transform(2, 3) = z;

transform(0, 0) = m0;
transform(0, 1) = m1;
transform(0, 2) = m2;

transform(1, 0) = m3;
transform(1, 1) = m4;
transform(1, 2) = m5;

transform(2, 0) = m6;
transform(2, 1) = m7;
transform(2, 2) = m8;

I don't think this is the right way to get what I want though? Is site_xmat and site_xpos with respect to the world frame?

I have seen also the function: void mj_local2Global(mjData* d, mjtNum* xpos, mjtNum* xmat,const mjtNum* pos,const mjtNum* quat,int body, mjtByte sameframe) but I am not sure how to use it and if is needed in my case.

Thank you in advance!