Loading a demo from real robot to mujoco

Hi Emo Todorov,

I was trying to load a demonstration from the real Baxter robot to a mujoco simulation based on the baxter.xml file provided in the mujoco resources. Initially I just want to try with position actuators. A demo performed on the real robot is stored and then replayed on the mujoco engine at the same rate as it was sampled.

But the mujoco baxter goes elsewhere, for sanity check I tried the default neutral position of baxter on mujoco.

Attached are the results from Gazebo and Mujoco. Why is there a mismatch? I guess it has to do something with the frame conventions followed in the xml file. What might be a quick fix for this?

If that is the case is there a better way to load from demonstration?




Best,

Michael

 

Thanks for the replies. Yes, it was my mistake, I had torque motors installed instead of position actuators. That fixed, I constantly get following segmentation fault with Baxter XML file. None of the other XML files I have produced this problem. I captured it by running gdb. Could you please suggest something. Strangely the error doesn't occur always, sometimes it runs smoothly with no issues.

Program received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffa9ec6700 (LWP 7796)]
0x00007fffbd10b191 in mj_collideGeoms () from ~/mjpro131/bin/libmujoco131.so

 

It is happening in the middle of the simulation. Not always. But most of the time. I use mujoco_py wrapper for the simulation. But I have never faced this issue before.
This is what I do essentially:

compute inverse_dynamics from a known joint position, velocity and acceleration for the left hand, keeping all other joints same at the existing values.

def inv_dyn(self, qpos, qvel, qacc):
self._model.data.qpos = qpos
self._model.data.qvel = qvel
self._model.data.qacc = qacc
self._model._compute_subtree()
self._model.forward()
self._model.step()
self._model.inverse()
return self._model.data.qfrc_inverse

(this above model is instantiated separately )

Then I apply the computed torque using the following function.

def exec_torque_cmd(self, cmd):
self._model.data.ctrl = cmd
self._model.step()

(this above model is the main model on which controls are applied.)

I have attached the xml file.

 

Thanks for that suggestion. Let me try it out. In relation to the previous post, I switched to Cpp just to see how well it goes. As a start, I wanted to make Baxter reach a point with torque control using internal inverse dynamics solver. This is my controller. Could you please check this and tell me the way I am calling it right? I use Eigen library for matrix operations.

The behaviour I see: The arm goes very fast to some other location.

void quatdiff(mjtNum* quat_diff, mjtNum* quat_curr, mjtNum* quat_des)
{
mjtNum tmp[3];

for (int i=1; i<4; i++)
quat_diff = quat_curr*quat_des[0] - quat_des*quat_curr[0];

mju_cross(tmp, quat_des+1, quat_curr+1);
mju_addTo(quat_diff, tmp, 3);
}

void inverse_dynamics(mjtNum* tau, mjtNum* qpos, mjtNum* qvel, mjtNum* qacc)
{
mjData* d_tmp = 0;
d_tmp = mj_makeData(m);
mju_copy(d->qpos, qpos, m->nq);
mju_copy(d_tmp->qvel, qvel, m->nv);
mju_copy(d->qacc, qacc, m->nv);
mju_copy(tau, d->qfrc_inverse, m->nv);
mj_deleteData(d_tmp);
}

// simple torque controller
void mycontroller(const mjModel* m, mjData* d)
{
mj_step1(m, d);

//all are initialized

//a test site in the xml file
int test_site_id = mj_name2id(m, mjOBJ_SITE, "testSite");
//this is the tip point of the left arm
int l_ee_site_id = mj_name2id(m, mjOBJ_SITE, "left_pad_center");
// this is the tip point of right arm
int r_ee_site_id = mj_name2id(m, mjOBJ_SITE, "right_pad_center");

//test site
mju_copy(test_site_pos.data(), &d->site_xpos[test_site_id], 3);
mju_copy(test_site_ori.data(), &d->site_xmat[test_site_id], 4);

//left arm ee
mju_copy(l_ee_pos.data(), &d->site_xpos[l_ee_site_id], 3);
mju_copy(l_ee_ori.data(), &d->site_xmat[l_ee_site_id], 4);

//right arm ee
mju_copy(r_ee_pos.data(), &d->site_xpos[r_ee_site_id], 3);
mju_copy(r_ee_ori.data(), &d->site_xmat[r_ee_site_id], 4);

//compute jacobian of left and right hand
mj_jacSite(m, d, l_ee_jac_tmp.data(), l_ee_jac_tmp.data()+45, l_ee_site_id);
mj_jacSite(m, d, r_ee_jac_tmp.data(), r_ee_jac_tmp.data()+45, r_ee_site_id);

//reshaping
Matrix<mjtNum, 6, 15> l_ee_jac(l_ee_jac_tmp.data());
Matrix<mjtNum, 6, 15> r_ee_jac(r_ee_jac_tmp.data());

//position error left and right hand
for (int i=0; i<3; i++)
{
err_l_ee(i,0) = -test_site_pos(i,0) + l_ee_pos(i,0);
err_r_ee(i,0) = -test_site_pos(i,0) + r_ee_pos(i,0);
}

//right hand orientation error
quatdiff(err_l_ee.data()+3, l_ee_ori.data(), test_site_ori.data());

//left hand orientation error
quatdiff(err_r_ee.data()+3, r_ee_ori.data(), test_site_ori.data());

l_ee_jac_pinv = l_ee_jac.transpose() * (l_ee_jac * l_ee_jac.transpose()).inverse();
r_ee_jac_pinv = r_ee_jac.transpose() * (r_ee_jac * r_ee_jac.transpose()).inverse();

//required joint velocities
l_joint_vel = l_ee_jac_pinv * err_l_ee;
r_joint_vel = r_ee_jac_pinv * err_r_ee;

//current position, velocity
mju_copy(qpos.data(), d->qpos, 15);
mju_copy(qvel.data(), d->qvel, 15);

//required joint accelerations
l_joint_acc = l_joint_vel - qvel;
r_joint_acc = r_joint_vel - qvel;

mj_fullM(m, Mass_Matrix_tmp.data(), d->qM);

//reshaping
Matrix<mjtNum, 15, 15> Mass_Matrix(Mass_Matrix_tmp.data());

mju_copy(qacc.data()+1, r_joint_acc.data()+1, 7); // copying relevant data for right hand
mju_copy(qacc.data()+9, l_joint_acc.data()+9, 7); // copying relevant data for left hand

/////////////////////////////////THIS DOESN'T WORK/////////////////////////////

//inverse_dynamics(tau, qpos.data(), qvel.data(), qacc.data());

/////////////////////////////////////////////////////////////////////////////////////////////////


/////////////////****************** APPROXIMATELY WORKS****************//////////////////////////

tau = Mass_Matrix*qacc;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//adding bias terms to tau
mju_addTo(tau.data(), d->qfrc_bias, 15);

mju_printMat(tau.data(), 1, 15);

mju_copy(d->ctrl, tau.data(), m->nu);

mj_step2(m, d);

}