当前位置：网站首页>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;
}
边栏推荐
- 【vulnhub】presidential1
- Pinia module division
- GEO数据挖掘（三）使用DAVID数据库进行GO、KEGG富集分析
- 使用yum来安装PostgreSQL13.3数据库
- [daily problem insight] prefix and -- count the number of fertile pyramids in the farm
- JWT signature does not match locally computed signature. JWT validity cannot be asserted and should
- 【向量检索研究系列】产品介绍
- How to set encoding in idea
- 【vulnhub】presidential1
- How can computers ensure data security in the quantum era? The United States announced four alternative encryption algorithms
猜你喜欢
JWT signature does not match locally computed signature. JWT validity cannot be asserted and should
GEO数据挖掘（三）使用DAVID数据库进行GO、KEGG富集分析
Business process testing based on functional testing
![[automated testing framework] what you need to know about unittest](/img/4d/0f0e0a67ec41e41541e0a2b5ca46d9.png)
[automated testing framework] what you need to know about unittest
DAY TWO
一图看懂对程序员的误解：西方程序员眼中的中国程序员
Data analysis course notes (III) array shape and calculation, numpy storage / reading data, indexing, slicing and splicing
互动滑轨屏演示能为企业展厅带来什么
Clipboard management tool paste Chinese version
uniapp中redirectTo和navigateTo的区别
随机推荐
Data operation platform - data collection [easy to understand]
openresty ngx_lua子请求
ldap创建公司组织、人员
On February 19, 2021ccf award ceremony will be held, "why in Hengdian?"
《LaTex》LaTex数学公式简介「建议收藏」
基于GO语言实现的X.509证书
rancher集成ldap，实现统一账号登录
DAY ONE
谷歌百度雅虎都是中国公司开发的通用搜索引擎_百度搜索引擎url
TypeScript增量编译
MIT 6.824 - raft Student Guide
openresty ngx_ Lua subrequest
vector的使用方法_vector指针如何使用
The way of intelligent operation and maintenance application, bid farewell to the crisis of enterprise digital transformation
"Latex" Introduction to latex mathematical formula "suggestions collection"
Amazon MemoryDB for Redis 和 Amazon ElastiCache for Redis 的内存优化
What is a responsive object? How to create a responsive object?
PostgreSQL highly available repmgr (1 master 2 slave +1witness) + pgpool II realizes master-slave switching + read-write separation
DAY SIX
How about the order management of okcc call center