Mujoco-二阶单摆建模与控制
2024-05-10 03:51:24
模型文件：
<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>控制实现
核心公式：M*qacc + qfrc_bias = qfrc_applied + ctrl
M：惯性矩阵
qacc:加速度
qfrc_bias :科里奥利矩阵和重力矩阵之和：qfrc_bias =C+G
qfrc_applied ：广义力
ctrl:其他控制输入，可以是电机扭矩
测试准备：
我们将模型文件的euler配置成 0 5 0,这样子在无输入的情况下，模型会因为重力进行摆动，
是一个自由双阶单摆的渐渐衰减过程。
有运动了，我们就可以通过设计控制器来看算法的效果
版本一：直接让双 稳定静止状态的输入：无电机，输入=qfrc_applied
效果：即使模型有角度偏差，但因为输入，可以处于平衡的静止状态
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*qddotlhs[0] = lhs[0] + f[0]; //lhs = M*qddot + flhs[1] = lhs[1] + f[1];d->qfrc_applied[0] = f[0];d->qfrc_applied[1] = f[1];
版本二 比例微分输入 tau=-Kp*(q-q_ref) - Kd*qdot
要求2个关节稳定在一个指定的角度
//controldouble Kp1 = 100, Kp2 = 100;double Kv1 = 10, Kv2 = 10;double qref1 = -0.5, qref2 = -1.6;//这个是两个关节要求的角度//PD controld->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];
测试发现，最后稳定下来是-0.63，1.6左右，差异值应该是PD带来的稳态误差
Mujoco二阶单摆的Kp kp控制
版本三 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的gravity+coriolis+PD控制
版本四 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*tautau[0] += f[0];tau[1] += f[1];d->qfrc_applied[0] = tau[0];d->qfrc_applied[1] = tau[1];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 = 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 filechar 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 simulationif( 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 statebutton_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 positionglfwGetCursorPos(window, &lastx, &lasty);
}// mouse move callback
void mouse_move(GLFWwindow* window, double xpos, double ypos)
{// no buttons down: nothing to doif( !button_left && !button_middle && !button_right )return;// compute mouse displacement, savedouble dx = xpos - lastx;double dy = ypos - lasty;lastx = xpos;lasty = ypos;// get current window sizeint width, height;glfwGetWindowSize(window, &width, &height);// get shift key statebool mod_shift = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT)==GLFW_PRESS ||glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT)==GLFW_PRESS);// determine action based on mouse buttonmjtMouse 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;elseaction = mjMOUSE_ZOOM;// move cameramjv_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 heightmjv_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 newlinefprintf(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 spacefprintf(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 newlinefprintf(fid,"\n");
}//**************************
void mycontroller(const mjModel* m, mjData* d)
{//write control heremj_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 + ctrlconst 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*qddotlhs[0] = lhs[0] + f[0]; //lhs = M*qddot + flhs[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");//controldouble 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*tautau[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 softwaremj_activate("mjkey.txt");// char xmlpath[100]={};// char datapath[100]={};//// strcat(xmlpath,path);// strcat(xmlpath,xmlfile);//// strcat(datapath,path);// strcat(datapath,datafile);// load and compile modelchar error[1000] = "Could not load binary model";// check command-line argumentsif( argc<2 )m = mj_loadXML(xmlpath, 0, error, 1000);elseif( strlen(argv[1])>4 && !strcmp(argv[1]+strlen(argv[1])-4, ".mjb") )m = mj_loadModel(argv[1], 0);elsem = mj_loadXML(argv[1], 0, error, 1000);if( !m )mju_error_s("Load model error: %s", error);// make datad = mj_makeData(m);// init GLFWif( !glfwInit() )mju_error("Could not initialize GLFW");// create window, make OpenGL context current, request v-syncGLFWwindow* window = glfwCreateWindow(1244, 700, "Demo", NULL, NULL);glfwMakeContextCurrent(window);glfwSwapInterval(1);// initialize visualization data structuresmjv_defaultCamera(&cam);mjv_defaultOption(&opt);mjv_defaultScene(&scn);mjr_defaultContext(&con);mjv_makeScene(m, &scn, 2000); // space for 2000 objectsmjr_makeContext(m, &con, mjFONTSCALE_150); // model-specific context// install GLFW mouse and keyboard callbacksglfwSetKeyCallback(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 callbackmjcb_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 viewportmjrRect viewport = {0, 0, 0, 0};glfwGetFramebufferSize(window, &viewport.width, &viewport.height);// update scene and rendermjv_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 callbacksglfwPollEvents();}// free visualization storagemjv_freeScene(&scn);mjr_freeContext(&con);// free MuJoCo model and data, deactivatemj_deleteData(d);mj_deleteModel(m);mj_deactivate();// terminate GLFW (crashes with Linux NVidia drivers)#if defined(__APPLE__) || defined(_WIN32)glfwTerminate();#endifreturn 1;
}Mujoco-二阶单摆建模与控制相关推荐
- 线性系统大作业——2.二阶倒立摆建模与控制系统设计（上）
文章目录 0.简介 1.建立数学模型 1.1.牛顿运动定律分析 欧拉-拉格朗日方程分析 2.Simulink仿真 3.使用SimMechancis仿真 4.在平衡点附近模型线性化 5.系统能控性.能观 ...
- MuJoCo - hybrid system的建模与控制
MuJoCo Lec8 Hybrid系统的建模与控制 如果要定义3D的floating base 模型的头6个dof: <worldbody><geom name='floor' p ...
- 《机器人与数字人：基于MATLAB的建模与控制》——2.3节指数映射和k过程
本节书摘来自华章社区<机器人与数字人:基于MATLAB的建模与控制>一书中的第2章,第2.3节指数映射和k过程,作者［美］顾友谅(Edward Y.L.Gu),更多章节内容可以访问云栖社区 ...
- 《机器人与数字人：基于MATLAB的建模与控制》——2.2节李群和李代数
本节书摘来自华章社区<机器人与数字人:基于MATLAB的建模与控制>一书中的第2章,第2.2节李群和李代数,作者［美］顾友谅(Edward Y.L.Gu),更多章节内容可以访问云栖社区&q ...
- 质子交换膜燃料电池建模与控制研究
1.内容简介 略268 2.内容说明 静态模型公式 静态模型公式 要求:输入变量T.P.I.,输出ENernst.Vact.Vohm.Vcon.Vcell.V.P.η 绘制横轴I,纵轴V的伏安特曲线 ...
- MATLAB优化转向器,汽车电动转向器动力学建模与控制仿真研究(MATLAB仿真)
汽车电动转向器动力学建模与控制仿真研究(MATLAB仿真)(任务书,开题报告,外文翻译,计划进度表,毕业论文12000字,相关框图和参数) 摘 要 汽车电动转向器是一种新型的汽车转向助力系统. 文章 ...
- stm32代码生成，基于模型的设计(MBD) 无刷直流电机MATLAB开发板建模代码生成控制 MBD电机控制资料
stm32代码生成,基于模型的设计(MBD) 无刷直流电机MATLAB开发板建模代码生成控制 MBD电机控制资料 控制算法采用MATLAB建模并生成代码的方式 配套电机 开发板 模型 源代码和视频 P ...
- 使用mocap在mujoco中实现逆运动学控制
使用mocap在mujoco中实现逆运动学控制 代码地址:https://github.com/kevincheng3/mocap-control 视频地址https://www.bilibili.c ...
- 两轮差速移动机器人运动分析、建模和控制
两轮差速运动分析.建模和控制 1 运动学分析建模 1.1 三种运动状态分析 1.2 函数模型 1.3 仿真验证 1.3.1 直线验证 1.3.2 曲线验证 1.3.3 旋转验证 2 运动控制 2.1 ...
- 直线一级倒立摆数学建模与控制仿真
学习目标: 1.推导直线型一级倒立摆的数学建模公式,得到状态空间表达式和传递函数,并分析系统的稳定性 2.采用控制算法将系统从不稳定调整到稳定状态,并用matlab和simulink仿真实现 学习内容 ...
最新文章
- 2-5-PerformingMountsUnmounts
- 《SolidWorks 2017中文版机械设计从入门到精通）》——1.4 操作环境设置
- TotoiseSVN的基本使用方法
- 恶意软件 自动化规则提取工具 yargen 原理分析
- Fedora学习总结
- andriod开发环境配置
- iOS_9_scrollView分页
- KnockoutJS + My97DatePicker
- php的静态变量static在函数内部
- newifi3高恪魔改最新_12.22达达最新球球id账号呆瓜表
- Android获取设备隐私 忽略6.0权限管理
- 程序员刚入职很痛苦_在中国，程序员这行能干一辈子吗？
- 好程序员Java教程分享javaweb框架
- 2014.3 USACO月赛T1 Watering the Fields
- Html中style标签是做什么的
- 读《春秋》有感之十二：偪阳之战
- 华三防火墙旁路部署三种方式之子接口旁路
- rtl8723be无线网卡不稳定
- Orz是什么意思……
- APP自动化原理+环境搭建