当前位置：网站首页>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;
}
边栏推荐
- MySQL learning notes (mind map)
- Business process testing based on functional testing
- TypeScript增量编译
- 【vulnhub】presidential1
- Typescript incremental compilation
- How to use vector_ How to use vector pointer
- [boutique] Pinia Persistence Based on the plug-in Pinia plugin persist
- Racher integrates LDAP to realize unified account login
- Leecode brushes questions and records interview questions 01.02 Determine whether it is character rearrangement for each other
- How engineers treat open source -- the heartfelt words of an old engineer
猜你喜欢
1000 words selected - interface test basis
Core knowledge of distributed cache
【2022全网最细】接口测试一般怎么测？接口测试的流程和步骤
Devops can help reduce technology debt in ten ways
JWT signature does not match locally computed signature. JWT validity cannot be asserted and should
Designed for decision tree, the National University of Singapore and Tsinghua University jointly proposed a fast and safe federal learning system
rancher集成ldap，实现统一账号登录
![Tourism Management System Based on jsp+servlet+mysql framework [source code + database + report]](/img/41/94488f4c7627a1dfcf80f170101347.png)
Tourism Management System Based on jsp+servlet+mysql framework [source code + database + report]
工程师如何对待开源 --- 一个老工程师的肺腑之言
St table
随机推荐
Leecode brushes questions to record interview questions 17.16 massagist
PostgreSQL uses pgpool II to realize read-write separation + load balancing
37 page overall planning and construction plan for digital Village revitalization of smart agriculture
Things like random
Quickly use various versions of PostgreSQL database in docker
Leecode brush questions record sword finger offer 43 The number of occurrences of 1 in integers 1 to n
DAY FIVE
【精品】pinia 基于插件pinia-plugin-persist的 持久化
VTK volume rendering program design of 3D scanned volume data
Introduction au GPIO
Cross-entrpy Method
DAY THREE
Use package FY in Oracle_ Recover_ Data. PCK to recover the table of truncate misoperation
SuperSocket 1.6 创建一个简易的报文长度在头部的Socket服务器
Leecode brush questions record sword finger offer 44 A digit in a sequence of numbers
openresty ngx_ Lua subrequest
Use Yum or up2date to install the postgresql13.3 database
uniapp中redirectTo和navigateTo的区别
GPIO簡介
C language input / output stream and file operation [II]