当前位置：网站首页>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;
}
边栏推荐
- 三维扫描体数据的VTK体绘制程序设计
- Clipboard management tool paste Chinese version
- SQL的一种写法，匹配就更新，否则就是插入
- What can the interactive slide screen demonstration bring to the enterprise exhibition hall
- Introduction to GPIO
- Uniapp uploads and displays avatars locally, and converts avatars into Base64 format and stores them in MySQL database
- Random类的那些事
- PostgreSQL highly available repmgr (1 master 2 slave +1witness) + pgpool II realizes master-slave switching + read-write separation
- DAY FIVE
- 37頁數字鄉村振興智慧農業整體規劃建設方案
猜你喜欢
沉浸式投影在线下展示中的三大应用特点
2022年PMP项目管理考试敏捷知识点（9）
Clipboard management tool paste Chinese version
How engineers treat open source -- the heartfelt words of an old engineer
Data analysis course notes (III) array shape and calculation, numpy storage / reading data, indexing, slicing and splicing
JWT signature does not match locally computed signature. JWT validity cannot be asserted and should
uniapp中redirectTo和navigateTo的区别
Idea automatically imports and deletes package settings
X.509 certificate based on go language
GPIO简介
随机推荐
Why should a complete knapsack be traversed in sequence? Briefly explain
Rails 4 asset pipeline vendor asset images are not precompiled
37 page overall planning and construction plan for digital Village revitalization of smart agriculture
How rider uses nuget package offline
On February 19, 2021ccf award ceremony will be held, "why in Hengdian?"
[CVPR 2022] target detection sota:dino: Detr with improved detecting anchor boxes for end to end object detection
Uniapp uploads and displays avatars locally, and converts avatars into Base64 format and stores them in MySQL database
1000 words selected - interface test basis
2022 PMP project management examination agile knowledge points (9)
TypeScript增量编译
Command line kills window process
基於GO語言實現的X.509證書
Use Yum or up2date to install the postgresql13.3 database
Personal digestion of DDD
MySQL主从之多源复制（3主1从）搭建及同步测试
QT tutorial: creating the first QT program
Devops can help reduce technology debt in ten ways
37 pages Digital Village revitalization intelligent agriculture Comprehensive Planning and Construction Scheme
Quickly use various versions of PostgreSQL database in docker
MIT 6.824 - raft Student Guide