当前位置：网站首页>Mujoco second order simple pendulum modeling and control
Mujoco second order simple pendulum modeling and control
2022-07-07 00:28:00 【Feisy】
Model file ：
<mujoco>
<option timestep="0.0001" integrator="RK4" >
<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 2.5" euler="0 5 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"/>
</body>
</body>
</worldbody>
</mujoco>
Control implementation
The core formula ：M*qacc + qfrc_bias = qfrc_applied + ctrl
M： Inertia matrix
qacc: The acceleration
qfrc_bias : The sum of Coriolis matrix and gravity matrix ：qfrc_bias =C+G
qfrc_applied ： Guangyili
ctrl: Other control inputs , It can be motor torque
Test preparation ：
We will... Of the model file euler configure 0 5 0, In this way, without input , The model will swing because of gravity ,
It is a gradual decay process of a free double order simple pendulum .
There is exercise , We can see the effect of the algorithm by designing the controller
Version of a ： Directly let the double Steady state input ： No motor , Input =qfrc_applied
effect ： Even if the model has angular deviation , But because of the input , Can be in a static state of equilibrium
double dense_M[nv*nv] = {
0};
mj_fullM(m,dense_M, d->qM);
double M[nv][nv]={
0};
M[0][0] = dense_M[0];
M[0][1] = dense_M[1];
M[1][0] = dense_M[2];
M[1][1] = dense_M[3];
// printf("%f %f \n",M[0][0],M[0][1]);
// printf("%f %f \n",M[1][0],M[1][1]);
// printf("******** \n");
double qddot[nv]={
0};
qddot[0]=d->qacc[0];
qddot[1]=d->qacc[1];
double f[nv]={
0};
f[0] = d->qfrc_bias[0];
f[1] = d->qfrc_bias[1];
double lhs[nv]={
0};
mju_mulMatVec(lhs,dense_M,qddot,2,2); //lhs = M*qddot
lhs[0] = lhs[0] + f[0]; //lhs = M*qddot + f
lhs[1] = lhs[1] + f[1];
d->qfrc_applied[0] = f[0];
d->qfrc_applied[1] = f[1];
Version 2 Proportional differential input tau=-Kp*(q-q_ref) - Kd*qdot
requirement 2 Joints are stabilized at a specified angle
//control
double Kp1 = 100, Kp2 = 100;
double Kv1 = 10, Kv2 = 10;
double qref1 = -0.5, qref2 = -1.6;// This is the angle required by the two joints
//PD control
d->qfrc_applied[0] = -Kp1*(d->qpos[0]-qref1)-Kv1*d->qvel[0];
d->qfrc_applied[1] = -Kp2*(d->qpos[1]-qref2)-Kv2*d->qvel[1];
Tests found , Finally, it stabilized -0.63,1.6 about , The difference value should be PD Steady state error
Mujoco Second order simple pendulum Kp kp control
Version 3 tau=coriolis + gravity + PD control
d->qfrc_applied[0] = f[0]-Kp1*(d->qpos[0]-qref1)-Kv1*d->qvel[0];
d->qfrc_applied[1] = f[1]-Kp2*(d->qpos[1]-qref2)-Kv2*d->qvel[1];
Mujoco Of gravity+coriolis+PD control
Version 4 Feedback linearization M*(-kp( … ) - kv(…) + f)
double tau[2]={0};
tau[0]=-Kp1*(d->qpos[0]-qref1)-Kv1*d->qvel[0];
tau[1]=-Kp2*(d->qpos[1]-qref2)-Kv2*d->qvel[1];
mju_mulMatVec(tau,dense_M,tau,2,2); //lhs = M*tau
tau[0] += f[0];
tau[1] += f[1];
d->qfrc_applied[0] = tau[0];
d->qfrc_applied[1] = tau[1];
Mujoco Feedback linearization
Complete code
#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 = 5;
//related to writing data to a file
FILE *fid;
int loop_index = 0;
const int data_frequency = 50; //frequency at which data is written to a file
char xmlpath[] = "../myproject/dbpendulum/doublependulum.xml";
char datapath[] = "../myproject/dbpendulum/data.csv";
//Change the path <template_writeData>
//Change the xml file
// char path[] = "../myproject/dbpendulum/";
// 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,"PE, KE, TE, ");
fprintf(fid,"q1, q2, ");
//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, %f, ",d->energy[0],d->energy[1],d->energy[0]+d->energy[1]);
fprintf(fid,"%f, %f ",d->qpos[0],d->qpos[1]);
//Don't remove the newline
fprintf(fid,"\n");
}
//**************************
void mycontroller(const mjModel* m, mjData* d)
{
//write control here
mj_energyPos(m,d);
mj_energyVel(m,d);
//printf("%f %f %f %f \n",d->time,d->energy[0],d->energy[1],d->energy[0]+d->energy[1]);
//check equations
//M*qacc + qfrc_bias = qfrc_applied + ctrl
//M*qddot + f = qfrc_applied + ctrl
const int nv = 2;
double dense_M[nv*nv] = {0};
mj_fullM(m,dense_M, d->qM);
double M[nv][nv]={0};
M[0][0] = dense_M[0];
M[0][1] = dense_M[1];
M[1][0] = dense_M[2];
M[1][1] = dense_M[3];
// printf("%f %f \n",M[0][0],M[0][1]);
// printf("%f %f \n",M[1][0],M[1][1]);
// printf("******** \n");
double qddot[nv]={0};
qddot[0]=d->qacc[0];
qddot[1]=d->qacc[1];
double f[nv]={0};
f[0] = d->qfrc_bias[0];
f[1] = d->qfrc_bias[1];
double lhs[nv]={0};
mju_mulMatVec(lhs,dense_M,qddot,2,2); //lhs = M*qddot
lhs[0] = lhs[0] + f[0]; //lhs = M*qddot + f
lhs[1] = lhs[1] + f[1];
d->qfrc_applied[0] = 0.1*f[0];
d->qfrc_applied[1] = 0.5*f[1];
double rhs[nv]={0};
rhs[0] = d->qfrc_applied[0];
rhs[1] = d->qfrc_applied[1];
// printf("%f %f \n",lhs[0], rhs[0]);
// printf("%f %f \n",lhs[1], rhs[1]);
// printf("******\n");
//control
double Kp1 = 100, Kp2 = 100;
double Kv1 = 10, Kv2 = 10;
double qref1 = -0.5, qref2 = -1.6;
//PD control
// d->qfrc_applied[0] = -Kp1*(d->qpos[0]-qref1)-Kv1*d->qvel[0];
// d->qfrc_applied[1] = -Kp2*(d->qpos[1]-qref2)-Kv2*d->qvel[1];
//coriolis + gravity + PD control
// d->qfrc_applied[0] = f[0]-Kp1*(d->qpos[0]-qref1)-Kv1*d->qvel[0];
// d->qfrc_applied[1] = f[1]-Kp2*(d->qpos[1]-qref2)-Kv2*d->qvel[1];
//Feedback linearization
//M*(-kp( ... ) - kv(...) + f)
double tau[2]={0};
tau[0]=-Kp1*(d->qpos[0]-qref1)-Kv1*d->qvel[0];
tau[1]=-Kp2*(d->qpos[1]-qref2)-Kv2*d->qvel[1];
mju_mulMatVec(tau,dense_M,tau,2,2); //lhs = M*tau
tau[0] += f[0];
tau[1] += f[1];
d->qfrc_applied[0] = tau[0];
d->qfrc_applied[1] = tau[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, 2.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;
fid = fopen(datapath,"w");
init_save_data();
d->qpos[0] = 0.5;
//d->qpos[1] = 0;
// 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;
}
边栏推荐
- How can computers ensure data security in the quantum era? The United States announced four alternative encryption algorithms
- 37 pages Digital Village revitalization intelligent agriculture Comprehensive Planning and Construction Scheme
- [2022 the finest in the whole network] how to test the interface test generally? Process and steps of interface test
- 智能运维应用之道，告别企业数字化转型危机
- [vector retrieval research series] product introduction
- 2022 PMP project management examination agile knowledge points (9)
- 如何判断一个数组中的元素包含一个对象的所有属性值
- Devops can help reduce technology debt in ten ways
- C language input / output stream and file operation [II]
- Amazon MemoryDB for Redis 和 Amazon ElastiCache for Redis 的内存优化
猜你喜欢
pytest多进程/多线程执行测试用例
Liuyongxin report | microbiome data analysis and science communication (7:30 p.m.)
48页数字政府智慧政务一网通办解决方案
2022/2/11 summary
Everyone is always talking about EQ, so what is EQ?
How can computers ensure data security in the quantum era? The United States announced four alternative encryption algorithms
DAY TWO
DAY FIVE
刘永鑫报告|微生物组数据分析与科学传播(晚7点半)
@TableId can‘t more than one in Class: “com.example.CloseContactSearcher.entity.Activity“.
随机推荐
【向量检索研究系列】产品介绍
C language input / output stream and file operation [II]
DAY SIX
web渗透测试是什么_渗透实战
MIT 6.824 - Raft学生指南
量子时代计算机怎么保证数据安全？美国公布四项备选加密算法
How to set encoding in idea
pytest多进程/多线程执行测试用例
1000字精选 —— 接口测试基础
How engineers treat open source -- the heartfelt words of an old engineer
DAY FIVE
互动滑轨屏演示能为企业展厅带来什么
DAY FOUR
openresty ngx_ Lua subrequest
DAY ONE
使用yum来安装PostgreSQL13.3数据库
Three sentences to briefly introduce subnet mask
37页数字乡村振兴智慧农业整体规划建设方案
如何判断一个数组中的元素包含一个对象的所有属性值
What is AVL tree?