当前位置：网站首页>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;
}
边栏推荐
- TypeScript增量编译
- Data analysis course notes (III) array shape and calculation, numpy storage / reading data, indexing, slicing and splicing
- 智能运维应用之道，告别企业数字化转型危机
- Leecode brush questions record sword finger offer 43 The number of occurrences of 1 in integers 1 to n
- Typescript incremental compilation
- 三维扫描体数据的VTK体绘制程序设计
- Racher integrates LDAP to realize unified account login
- DAY THREE
- Supersocket 1.6 creates a simple socket server with message length in the header
- 2022/2/11 summary
猜你喜欢
If the college entrance examination goes well, I'm already graying out at the construction site at the moment
Pytest multi process / multi thread execution test case
1000字精选 —— 接口测试基础
Interface master v3.9, API low code development tool, build your interface service platform immediately
ldap创建公司组织、人员
![[boutique] Pinia Persistence Based on the plug-in Pinia plugin persist](/img/53/95ab85bfd99d943f98881596d0aa8c.png)
[boutique] Pinia Persistence Based on the plug-in Pinia plugin persist
一图看懂对程序员的误解：西方程序员眼中的中国程序员
英雄联盟｜王者｜穿越火线 bgm AI配乐大赛分享
什么是响应式对象？响应式对象的创建过程？
Clipboard management tool paste Chinese version
随机推荐
GEO数据挖掘（三）使用DAVID数据库进行GO、KEGG富集分析
Geo data mining (III) enrichment analysis of go and KEGG using David database
C language input / output stream and file operation [II]
【精品】pinia 基于插件pinia-plugin-persist的 持久化
Why should a complete knapsack be traversed in sequence? Briefly explain
2021 SASE integration strategic roadmap (I)
TypeScript增量编译
Operation test of function test basis
Model-Free Prediction
On February 19, 2021ccf award ceremony will be held, "why in Hengdian?"
[daily problem insight] prefix and -- count the number of fertile pyramids in the farm
【vulnhub】presidential1
Clipboard management tool paste Chinese version
@TableId can‘t more than one in Class: “com.example.CloseContactSearcher.entity.Activity“.
Everyone is always talking about EQ, so what is EQ?
DAY ONE
48 page digital government smart government all in one solution
Idea automatically imports and deletes package settings
Random类的那些事
Amazon MemoryDB for Redis 和 Amazon ElastiCache for Redis 的内存优化