当前位置：网站首页>Mujoco Jacobi - inverse motion - sensor
Mujoco Jacobi - inverse motion - sensor
2022-07-07 00:28:00 【Feisy】
1 Use sensors to monitor end position and speed
2 Jacobian
3 Calculate the end position and speed through Jacobi , Then compare it with the sensor
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 Inverse motion
Calculation principle 
Example ： We want the end to be rounded , The method is
- Find the desired coordinates first through the formula
- Subtract the desired coordinates from the coordinates sensed by the current sensor , Get the difference
- According to the above principle formula , Use Jacobi's inversion to calculate the angle of each joint
- Send the angle to the motor
//1 Jacobi's calculation
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 Jacobi's inverse
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 Calculate the coordinates of the next position
double x,y;
x = x_0 + r*cos(omega*d->time);
y = y_0 + r*sin(omega*d->time);
//4 The difference between the coordinates of the next position and the position sensed by the current sensor , As a change of the end
double dr[] = {
x- d->sensordata[0],y - d->sensordata[2]};
double dq[2] ={
};
//5 Calculate the angle change according to the end transformation
//dq = Jinv*dr
mju_mulMatVec(dq,J_inv,dr,2,2);
printf("%f %f \n", dq[0],dq[1]);
//6 Tell the motor to rotate to the next angle
//q -> q+dq
//ctrl = q
d->ctrl[0] = d->qpos[0]+dq[0];
d->ctrl[2] = d->qpos[1]+dq[1];
Model file
<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 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 simulation
if( 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 state
button_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 position
glfwGetCursorPos(window, &lastx, &lasty);
}
// mouse move callback
void mouse_move(GLFWwindow* window, double xpos, double ypos)
{
// no buttons down: nothing to do
if( !button_left && !button_middle && !button_right )
return;
// compute mouse displacement, save
double dx = xpos - lastx;
double dy = ypos - lasty;
lastx = xpos;
lasty = ypos;
// get current window size
int width, height;
glfwGetWindowSize(window, &width, &height);
// get shift key state
bool mod_shift = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT)==GLFW_PRESS ||
glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT)==GLFW_PRESS);
// determine action based on mouse button
mjtMouse 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;
else
action = mjMOUSE_ZOOM;
// move camera
mjv_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 height
mjv_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 newline
fprintf(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 space
fprintf(fid,"%f, ",d->time);
fprintf(fid,"%f, %f ",d->sensordata[0],d->sensordata[2]);
//Don't remove the newline
fprintf(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;
else
m->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*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]);
// 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*dr
mju_mulMatVec(dq,J_inv,dr,2,2);
printf("%f %f \n", dq[0],dq[1]);
//q -> q+dq
//ctrl = q
d->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 software
mj_activate("mjkey.txt");
char xmlpath[100]={
};
char datapath[100]={
};
strcat(xmlpath,path);
strcat(xmlpath,xmlfile);
strcat(datapath,path);
strcat(datapath,datafile);
// load and compile model
char error[1000] = "Could not load binary model";
// check command-line arguments
if( argc<2 )
m = mj_loadXML(xmlpath, 0, error, 1000);
else
if( strlen(argv[1])>4 && !strcmp(argv[1]+strlen(argv[1])-4, ".mjb") )
m = mj_loadModel(argv[1], 0);
else
m = mj_loadXML(argv[1], 0, error, 1000);
if( !m )
mju_error_s("Load model error: %s", error);
// make data
d = mj_makeData(m);
// init GLFW
if( !glfwInit() )
mju_error("Could not initialize GLFW");
// create window, make OpenGL context current, request v-sync
GLFWwindow* window = glfwCreateWindow(1244, 700, "Demo", NULL, NULL);
glfwMakeContextCurrent(window);
glfwSwapInterval(1);
// initialize visualization data structures
mjv_defaultCamera(&cam);
mjv_defaultOption(&opt);
mjv_defaultScene(&scn);
mjr_defaultContext(&con);
mjv_makeScene(m, &scn, 2000); // space for 2000 objects
mjr_makeContext(m, &con, mjFONTSCALE_150); // model-specific context
// install GLFW mouse and keyboard callbacks
glfwSetKeyCallback(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 callback
mjcb_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 viewport
mjrRect viewport = {
0, 0, 0, 0};
glfwGetFramebufferSize(window, &viewport.width, &viewport.height);
// update scene and render
mjv_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 callbacks
glfwPollEvents();
}
// free visualization storage
mjv_freeScene(&scn);
mjr_freeContext(&con);
// free MuJoCo model and data, deactivate
mj_deleteData(d);
mj_deleteModel(m);
mj_deactivate();
// terminate GLFW (crashes with Linux NVidia drivers)
#if defined(__APPLE__) || defined(_WIN32)
glfwTerminate();
#endif
return 1;
}
边栏推荐
- Devops can help reduce technology debt in ten ways
- Idea automatically imports and deletes package settings
- GPIO简介
- Supersocket 1.6 creates a simple socket server with message length in the header
- Leecode brush questions record interview questions 32 - I. print binary tree from top to bottom
- ldap创建公司组织、人员
- Leecode brush question record sword finger offer 56 - ii Number of occurrences of numbers in the array II
- Use package FY in Oracle_ Recover_ Data. PCK to recover the table of truncate misoperation
- 微信小程序uploadfile服务器,微信小程序之wx.uploadFile[通俗易懂]
- 量子时代计算机怎么保证数据安全？美国公布四项备选加密算法
猜你喜欢
三维扫描体数据的VTK体绘制程序设计
37页数字乡村振兴智慧农业整体规划建设方案
Liuyongxin report | microbiome data analysis and science communication (7:30 p.m.)
沉浸式投影在线下展示中的三大应用特点
Introduction au GPIO
VTK volume rendering program design of 3D scanned volume data
37 page overall planning and construction plan for digital Village revitalization of smart agriculture
2022/2/12 summary
What can the interactive slide screen demonstration bring to the enterprise exhibition hall
Geo data mining (III) enrichment analysis of go and KEGG using David database
随机推荐
MIT 6.824 - Raft学生指南
Operation test of function test basis
智能运维应用之道，告别企业数字化转型危机
What can the interactive slide screen demonstration bring to the enterprise exhibition hall
基于GO语言实现的X.509证书
[vector retrieval research series] product introduction
Leecode brushes questions to record interview questions 17.16 massagist
准备好在CI/CD中自动化持续部署了吗？
一图看懂对程序员的误解：西方程序员眼中的中国程序员
C语言输入/输出流和文件操作【二】
英雄联盟｜王者｜穿越火线 bgm AI配乐大赛分享
【vulnhub】presidential1
How about the order management of okcc call center
互动滑轨屏演示能为企业展厅带来什么
Cross-entrpy Method
JWT signature does not match locally computed signature. JWT validity cannot be asserted and should
How rider uses nuget package offline
微信小程序uploadfile服务器,微信小程序之wx.uploadFile[通俗易懂]
Three application characteristics of immersive projection in offline display
谷歌百度雅虎都是中国公司开发的通用搜索引擎_百度搜索引擎url