Mujoco雅克比-逆运动-传感器

原创

已于 2022-02-20 21:56:55 修改 · 2.2k 阅读

7 ·

CC 4.0 BY-SA版权

文章标签：

#mujoco

于 2022-02-12 18:10:46 首次发布

MuJoCo Lec6 - Mujoco中的雅可比矩阵以及逆运动

1 使用传感器来监控末端位置和速度

2 雅可比

在这里插入图片描述

3 通过雅可比计算末端位置和速度，然后跟传感器感应到的比较

在这里插入图片描述

  double jacp[6]={0};
  double point[3]={d->sensordata[0],d->sensordata[1],d->sensordata[2]};
  int body = 2;
  mj_jac(m,d,jacp,NULL,point,body);
 
  double J[4]={ jacp[0],jacp[1],jacp[4],jacp[5]};
  double qdot[2] = {d->qvel[0],d->qvel[1]};
  double xdot[2] ={0};
  //xdot = J*qdot
  mju_mulMatVec(xdot,J,qdot,2,2);
   printf("velocity using jacobian: %f %f \n",xdot[0],xdot[1]);
   printf("velocity using sensordata= %f %f \n",d->sensordata[3],d->sensordata[5]);

在这里插入图片描述

4 逆运动

计算原理
在这里插入图片描述
例子：我们想让末端画圆，方法是

通过公式先求出期望坐标
用期望坐标跟当前传感器感应到的坐标相减，得到差异
根据上述原理公式，用雅可比的逆反算出各个关节的角度
将角度发送给电机


//1 雅可比的计算
  double J[4]={
   
    jacp[0],jacp[1],jacp[4],jacp[5]};
  double qdot[2] = {
   
   d->qvel[0],d->qvel[1]};
  double xdot[2] ={
   
   0};
  //xdot = J*qdot
  mju_mulMatVec(xdot,J,qdot,2,2);
 

  //2 雅可比的逆
  int i;
  double det_J = J[0]*J[3]-J[1]*J[2];
  double J_temp[] = {
   
   J[3],-J[1],-J[2],J[0]};
  double J_inv[4]={
   
   };
  for (i=0;i<4;i++)
    J_inv[i] = J_temp[i]/det_J;

  //3 计算下个位置的坐标
  double x,y;
  x = x_0 + r*cos(omega*d->time);
  y = y_0 + r*sin(omega*d->time);

  //4 下个位置的坐标和当前传感器感应到的位置的差异，作为末端的变化
  double dr[] = {
   
   x- d->sensordata[0],y - d->sensordata[2]};
  double dq[2] ={
   
   };

  //5 根据末端变换反算角度变化
  //dq = Jinv*dr
  mju_mulMatVec(dq,J_inv,dr,2,2);
  printf("%f %f \n", dq[0],dq[1]);

  //6 告诉电机转动到下个角度
  //q -> q+dq
  //ctrl = q
  d->ctrl[0] = d->qpos[0]+dq[0];
  d->ctrl[2] = d->qpos[1]+dq[1];

模型文件

<mujoco>
	<o
	ption timestep="0.0001" integrator="RK4" gravity="0 0 0" >
		<flag sensornoise="enable" energy="enable" contact="disable" />
	</option>
	<worldbody>
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
		<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
		<body pos="0 0 1.25" euler="0 90 0">
			<joint name="pin" type="hinge" axis = "0 -1 0" pos="0 0 -0.5"/>
			<geom type="cylinder" size="0.05 0.5" rgba="0 .9 0 1" mass="1"/>
			<body