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*qdotmju_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*qdotmju_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*drmju_mulMatVec(dq,J_inv,dr,2,2);printf("%f %f \n", dq[0],dq[1]);//6 告诉电机转动到下个角度//q -> q+dq//ctrl = qd->ctrl[0] = d->qpos[0]+dq[0];d->ctrl[2] = d->qpos[1]+dq[1];

模型文件

<mujoco><option 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 pos="0 0.1 1" euler="0 0 0"><joint name="pin2" type="hinge" axis = "0 -1 0" pos="0 0 -0.5"/><geom type="cylinder" size="0.05 0.5" rgba="0 0 .9 1" mass="1"/><site name="endeff" pos="0 0 0.5" size="0.1"/></body></body></worldbody><actuator><position name="pservo1" joint="pin" kp="100" /><velocity name="vservo1" joint="pin" kv="10" /><position name="pservo2" joint="pin2" kp="100" /><velocity name="vservo2" joint="pin2" kv="10" /></actuator><sensor><framepos objtype="site" objname="endeff"/><framelinvel objtype="site" objname="endeff"/></sensor>
</mujoco>

#include<stdbool.h> //for bool
//#include<unistd.h> //for usleep
//#include <math.h>#include "mujoco.h"
#include "glfw3.h"
#include "stdio.h"
#include "stdlib.h"
#include "string.h"//simulation end time
double simend = 20;
double qinit[2] = {0,1.25};
double r = 0.5;
double omega = 0.5;double x_0, y_0;//related to writing data to a file
FILE *fid;
int loop_index = 0;
const int data_frequency = 10; //frequency at which data is written to a file// char xmlpath[] = "../myproject/template_writeData/pendulum.xml";
// char datapath[] = "../myproject/template_writeData/data.csv";//Change the path <template_writeData>
//Change the xml file
char path[] = "../myproject/dbpendulum_ik/";
char xmlfile[] = "doublependulum.xml";char datafile[] = "data.csv";// MuJoCo data structures
mjModel* m = NULL;                  // MuJoCo model
mjData* d = NULL;                   // MuJoCo data
mjvCamera cam;                      // abstract camera
mjvOption opt;                      // visualization options
mjvScene scn;                       // abstract scene
mjrContext con;                     // custom GPU context// mouse interaction
bool button_left = false;
bool button_middle = false;
bool button_right =  false;
double lastx = 0;
double lasty = 0;// holders of one step history of time and position to calculate dertivatives
mjtNum position_history = 0;
mjtNum previous_time = 0;// controller related variables
float_t ctrl_update_freq = 100;
mjtNum last_update = 0.0;
mjtNum ctrl;// keyboard callback
void keyboard(GLFWwindow* window, int key, int scancode, int act, int mods)
{// backspace: reset simulationif( act==GLFW_PRESS && key==GLFW_KEY_BACKSPACE ){mj_resetData(m, d);mj_forward(m, d);}
}// mouse button callback
void mouse_button(GLFWwindow* window, int button, int act, int mods)
{// update button statebutton_left =   (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT)==GLFW_PRESS);button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE)==GLFW_PRESS);button_right =  (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT)==GLFW_PRESS);// update mouse positionglfwGetCursorPos(window, &lastx, &lasty);
}// mouse move callback
void mouse_move(GLFWwindow* window, double xpos, double ypos)
{// no buttons down: nothing to doif( !button_left && !button_middle && !button_right )return;// compute mouse displacement, savedouble dx = xpos - lastx;double dy = ypos - lasty;lastx = xpos;lasty = ypos;// get current window sizeint width, height;glfwGetWindowSize(window, &width, &height);// get shift key statebool mod_shift = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT)==GLFW_PRESS ||glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT)==GLFW_PRESS);// determine action based on mouse buttonmjtMouse action;if( button_right )action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V;else if( button_left )action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V;elseaction = mjMOUSE_ZOOM;// move cameramjv_moveCamera(m, action, dx/height, dy/height, &scn, &cam);
}// scroll callback
void scroll(GLFWwindow* window, double xoffset, double yoffset)
{// emulate vertical mouse motion = 5% of window heightmjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05*yoffset, &scn, &cam);
}//****************************
//This function is called once and is used to get the headers
void init_save_data()
{//write name of the variable here (header)fprintf(fid,"t, ");fprintf(fid,"x, y ");//Don't remove the newlinefprintf(fid,"\n");
}//***************************
//This function is called at a set frequency, put data here
void save_data(const mjModel* m, mjData* d)
{//data here should correspond to headers in init_save_data()//seperate data by a space %f followed by spacefprintf(fid,"%f, ",d->time);fprintf(fid,"%f, %f ",d->sensordata[0],d->sensordata[2]);//Don't remove the newlinefprintf(fid,"\n");
}/******************************/
void set_torque_control(const mjModel* m,int actuator_no,int flag)
{if (flag==0)m->actuator_gainprm[10*actuator_no+0]=0;elsem->actuator_gainprm[10*actuator_no+0]=1;
}
/******************************//******************************/
void set_position_servo(const mjModel* m,int actuator_no,double kp)
{m->actuator_gainprm[10*actuator_no+0]=kp;m->actuator_biasprm[10*actuator_no+1]=-kp;
}
/******************************//******************************/
void set_velocity_servo(const mjModel* m,int actuator_no,double kv)
{m->actuator_gainprm[10*actuator_no+0]=kv;m->actuator_biasprm[10*actuator_no+2]=-kv;
}
/******************************///**************************
void init_controller(const mjModel* m, mjData* d)
{//mj_step(m,d);mj_forward(m,d);printf("position = %f %f \n",d->sensordata[0],d->sensordata[2]);//x0+r = d->sensordata[0];//y0 = d->sensordata[2]x_0 = d->sensordata[0] - r;y_0 = d->sensordata[2];
}//**************************
void mycontroller(const mjModel* m, mjData* d)
{//write control here//printf("position = %f %f %f \n",d->sensordata[0],d->sensordata[1],d->sensordata[2]);//printf("velocity = %f %f %f \n",d->sensordata[3],d->sensordata[4],d->sensordata[5]);//void mj_jac(const mjModel* m, const mjData* d,mjtNum* jacp, mjtNum* jacr, const mjtNum point[3], int body);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);// printf("J = \n");//3x2// printf("%f %f \n", jacp[0],jacp[1]);// printf("%f %f \n", jacp[2],jacp[3]);// printf("%f %f \n", jacp[4],jacp[5]);// printf("*********\n");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*qdotmju_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]);// d->ctrl[0] = qinit[0];// d->ctrl[2] = qinit[1];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;double x,y;x = x_0 + r*cos(omega*d->time);y = y_0 + r*sin(omega*d->time);double dr[] = {x- d->sensordata[0],y - d->sensordata[2]};double dq[2] ={};//dq = Jinv*drmju_mulMatVec(dq,J_inv,dr,2,2);printf("%f %f \n", dq[0],dq[1]);//q -> q+dq//ctrl = qd->ctrl[0] = d->qpos[0]+dq[0];d->ctrl[2] = d->qpos[1]+dq[1];//write data here (dont change/dete this function call; instead write what you need to save in save_data)if ( loop_index%data_frequency==0){save_data(m,d);}loop_index = loop_index + 1;
}//************************
// main function
int main(int argc, const char** argv)
{// activate softwaremj_activate("mjkey.txt");char xmlpath[100]={};char datapath[100]={};strcat(xmlpath,path);strcat(xmlpath,xmlfile);strcat(datapath,path);strcat(datapath,datafile);// load and compile modelchar error[1000] = "Could not load binary model";// check command-line argumentsif( argc<2 )m = mj_loadXML(xmlpath, 0, error, 1000);elseif( strlen(argv[1])>4 && !strcmp(argv[1]+strlen(argv[1])-4, ".mjb") )m = mj_loadModel(argv[1], 0);elsem = mj_loadXML(argv[1], 0, error, 1000);if( !m )mju_error_s("Load model error: %s", error);// make datad = mj_makeData(m);// init GLFWif( !glfwInit() )mju_error("Could not initialize GLFW");// create window, make OpenGL context current, request v-syncGLFWwindow* window = glfwCreateWindow(1244, 700, "Demo", NULL, NULL);glfwMakeContextCurrent(window);glfwSwapInterval(1);// initialize visualization data structuresmjv_defaultCamera(&cam);mjv_defaultOption(&opt);mjv_defaultScene(&scn);mjr_defaultContext(&con);mjv_makeScene(m, &scn, 2000);                // space for 2000 objectsmjr_makeContext(m, &con, mjFONTSCALE_150);   // model-specific context// install GLFW mouse and keyboard callbacksglfwSetKeyCallback(window, keyboard);glfwSetCursorPosCallback(window, mouse_move);glfwSetMouseButtonCallback(window, mouse_button);glfwSetScrollCallback(window, scroll);double arr_view[] = {89.608063, -11.588379, 5, 0.000000, 0.000000, 1.000000};cam.azimuth = arr_view[0];cam.elevation = arr_view[1];cam.distance = arr_view[2];cam.lookat[0] = arr_view[3];cam.lookat[1] = arr_view[4];cam.lookat[2] = arr_view[5];// install control callbackmjcb_control = mycontroller;d->qpos[0] = qinit[0];d->qpos[1] = qinit[1];fid = fopen(datapath,"w");init_save_data();init_controller(m,d);// use the first while condition if you want to simulate for a period.while( !glfwWindowShouldClose(window)){// advance interactive simulation for 1/60 sec//  Assuming MuJoCo can simulate faster than real-time, which it usually can,//  this loop will finish on time for the next frame to be rendered at 60 fps.//  Otherwise add a cpu timer and exit this loop when it is time to render.mjtNum simstart = d->time;while( d->time - simstart < 1.0/60.0 ){mj_step(m, d);}if (d->time>=simend){fclose(fid);break;}// get framebuffer viewportmjrRect viewport = {0, 0, 0, 0};glfwGetFramebufferSize(window, &viewport.width, &viewport.height);// update scene and rendermjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn);mjr_render(viewport, &scn, &con);//printf("{%f, %f, %f, %f, %f, %f};\n",cam.azimuth,cam.elevation, cam.distance,cam.lookat[0],cam.lookat[1],cam.lookat[2]);// swap OpenGL buffers (blocking call due to v-sync)glfwSwapBuffers(window);// process pending GUI events, call GLFW callbacksglfwPollEvents();}// free visualization storagemjv_freeScene(&scn);mjr_freeContext(&con);// free MuJoCo model and data, deactivatemj_deleteData(d);mj_deleteModel(m);mj_deactivate();// terminate GLFW (crashes with Linux NVidia drivers)#if defined(__APPLE__) || defined(_WIN32)glfwTerminate();#endifreturn 1;
}

Mujoco雅克比-逆运动-传感器相关推荐

使用mocap在mujoco中实现逆运动学控制
使用mocap在mujoco中实现逆运动学控制代码地址:https://github.com/kevincheng3/mocap-control 视频地址https://www.bilibili.c ...
android传感器获取运动方向,Android开发者指南-运动传感器Motion Sensor[原创译文]...
Android 平台支持很多监测设备运动的传感器.其中有两个传感器一定是基于硬件的(加速度计和陀螺仪),有三个可能基于硬件或软件(重力计.线性加速计和旋转向量传感器). 比如,某些设备的软传感器利用加 ...
摇一摇事件封装了设备的运动传感器
微信活动页面经常有"摇一摇,拿好礼",还有拼多多摇现金,摇一摇功能也非常常见.HTML5 提供的 devicemotion 事件封装了设备的运动传感器,提供设备的加速度,还提供设备 ...
2020年TI杯大学生电子设计大赛无线运动传感器节点备赛、参赛实录（历时一个月）
转载请标明转载自:https://blog.csdn.net/weixin_44578655/article/details/109020022 9.5 清单已经出了一段时间了.买的少部分元器件也到了 ...
中国运动传感器陀螺仪行业市场供需与战略研究报告
运动传感器陀螺仪市场的企业竞争态势该报告涉及的主要国际市场参与者有Analog Devices Inc..Bosch Sensortec.InvenSense.Maxim Integrated.Mu ...
ISM330DHCXTR IMU-惯性测量单元工业物联网运动传感器
ISM330DHCX iNEMO惯性SiP模块采用系统级封装,包含高性能3D数字加速度计和3D数字陀螺仪,适合用于工业物联网 (IIoT) 和工业4.0应用.由于在同一个硅片上集成了加速度计和陀螺仪的 ...
ICM-42670-P 六轴运动传感器 TDK ICP-10740 气压计实现运动监测
ICM-42670-P 六轴运动传感器 & TDK ICP-10740 气压计,配合 CyweeMotion 算法,实现了运动监测.CyweeMotion 算法不仅支持多种运动模式:如走路.跑 ...
ICM-42605 6轴MEMS加速度计陀螺仪运动传感器数据的读取
ICM42605传感器简介 ICM426056轴MEMS运动传感器是集成了3轴陀螺仪和3轴加速度计的运动跟踪器件.陀螺仪支持8个可编程满量程范围设置(从±15.625dps到±2000dps).加速度 ...
Ardunio开发实例-被动红外(PIR)运动传感器使用
被动红外(PIR)运动传感器使用 PIR运动传感器非常适合检测运动. PIR代表"被动红外". 基本上,PIR运动传感器测量其视场中物体的红外光. 因此,它可以根据环境中红外光的变 ...
html5读取运动传感器,详解五种类型的运动传感器
运动传感器,顾名思义,指检测物体或人运动的装置,包括重力,线性加速度,旋转矢量.振动频率等.运动传感器可用于监视设备移动,如倾斜,摇晃,旋转或摆动. 其中,旋转矢量传感器和重力传感器是用于运动检测和监 ...

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

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

2 雅可比

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

4 逆运动

模型文件

Mujoco雅克比-逆运动-传感器相关推荐

最新文章

热门文章