MuJoCo - hybrid system的建模与控制

MuJoCo Lec8 Hybrid系统的建模与控制

如果要定义3D的floating base 模型的头6个dof:

<worldbody><geom name='floor' pos='0.001 0 0' size='100 10 .125' type='plane' material='plane' condim='3' conaffinity='15'/><light mode='trackcom' pos='0 0 5' dir='0 0 -1'/><light directional='true' pos='0 0 3' dir='0 0 -1' diffuse='0.2 0.2 0.2' specular='0 0 0'/><body name='pelvis' pos='0 0 1.01'><joint type='slide' axis='1 0 0' limited='false'/><joint type='slide' axis='0 1 0' limited='false'/><joint type='slide' axis='0 0 1' limited='false' ref='1.01'/><joint type='ball' limited='false'/><!--上面是头6个的dof的定义，需要在所有dof之前--></body></worldbody>
</mujoco>

在模拟程序中显示世界坐标，体坐标，COM的方法

  opt.frame = mjFRAME_WORLD; //mjFRAME_BODYopt.flags[mjVIS_COM]  = 1 ; //mjVIS_JOINT;opt.flags[mjVIS_JOINT]  = 1 ;

f是科里奥利力，D+C+G=u的G为什么不需要了？

因为我们有约束x,z是不动的，所以还要加上一个约束力输入

contact="enable"的作用是物体掉下去的时候，会被地板接住

<mujoco><option timestep="0.001" integrator="RK4" gravity="0 0 -9.81"><flag sensornoise="disable" contact="enable" energy="enable"/></option><worldbody><light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/><geom type="plane" size="5 5 0.1" rgba=".9 0 0 1"/><body name="pole" pos="0 0 2" euler="0 0 0"><joint name="x" type="slide" pos="0 0 0.5" axis="1 0 0" /><joint name="z" type="slide" pos="0 0 0.5" axis="0 0 1" /><joint name="pin" type="hinge" pos="0 0 0.5" axis="0 -1 0" /><geom  type="cylinder" size=".05 .5" rgba="0 .9 0 0.1" mass="1"/></body><body name="pole2" pos="0 -1 2" euler="0 0 0"><joint name="x2" type="slide" pos="0 0 0.5" axis="1 0 0" /><joint name="z2" type="slide" pos="0 0 0.5" axis="0 0 1" /><joint name="pin2" type="hinge" pos="0 0 0.5" axis="0 -1 0" /><geom  type="cylinder" size=".05 .5" rgba=".9 .9 .9 1" mass="1"/></body></worldbody><equality><connect body1="pole" body2="world" anchor="0 0 0.5" /></equality><actuator><motor name="torque" joint="pin" gear="1" ctrlrange="-100 100" ctrllimited="true"/><position name="position_servo" joint="pin" kp="0"/><velocity name="velocity_servo" joint="pin" kv="0"/></actuator></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;int fsm;
#define fsm_swing 0
#define fsm_free 1//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/hybrid_pendulum/";
char xmlfile[] = "pendulum.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, ");//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);//Don't remove the newlinefprintf(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;elsem->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)
{fsm = fsm_swing;
}//**************************
void mycontroller(const mjModel* m, mjData* d)
{//write control hereconst int nv = 6;const int nv1 = 3;double M[nv*nv]={0};mj_fullM(m,M,d->qM);//original system// M0 M1 M2// M3 M4 M5// M6 M7 M8//two pendulums// M0 M1 M2 M3 M4 M5// M6 M7 M8 M9 M10 M11// M12 M13 M14 M15 M16 M17// ...// ..// ...// printf("%f %f %f \n",M[0],M[1],M[2]);// printf("%f %f %f \n",M[3],M[4],M[5]);// printf("%f %f %f \n",M[6],M[7],M[8]);double M1[nv1*nv1]={0};M1[0] = M[0];  M1[1] = M[1];  M1[2] = M[2];M1[3] = M[6];  M1[4] = M[7];  M1[5] = M[8];M1[6] = M[12];  M1[7] = M[13];  M1[8] = M[14];double qddot[nv]={0};qddot[0] = d->qacc[0];qddot[1] = d->qacc[1];qddot[2] = d->qacc[2];double f[nv]={0};f[0] = d->qfrc_bias[0];f[1] = d->qfrc_bias[1];f[2] = d->qfrc_bias[2];double lhs[3]={0};double M_qddot[3]={0}; //=M*qddotmju_mulMatVec(M_qddot,M1,qddot,3,3);//lhs = M*qddot + flhs[0]=M_qddot[0]+f[0];lhs[1]=M_qddot[1]+f[1];lhs[2]=M_qddot[2]+f[2];//Fx, Fy, tau_ydouble tau[nv] = {0};tau[0] = d->qfrc_applied[0]; //Fxtau[1] = d->qfrc_applied[1]; //Fztau[2] = d->qfrc_applied[2]; //tau_yint i;double J0[3*nv]={0};for (i=0;i<3*nv;i++)J0[i] = d->efc_J[i];double JT_F[3]={0};double J1[nv1*nv1]={0};J1[0] = J0[0];  J1[1] = J0[1];  J1[2] = J0[2];J1[3] = J0[6];  J1[4] = J0[7];  J1[5] = J0[8];J1[6] = J0[12];  J1[7] = J0[13];  J1[8] = J0[14];//J[3*nv] = J[3x6]// J0 J1 J2 J3 J4 J5// J6 J7 J8 J9 J10 J11// J12 J13 J14 J15 J16 J17double F0[3]={0};F0[0] = d->efc_force[0];F0[1] = d->efc_force[1];F0[2] = d->efc_force[2];mju_mulMatTVec(JT_F,J1,F0,3,3);// mju_mulMatTVec(JT_F,J0,F0,3,3);// double F0[3]={0};// F0[0] = d->efc_force[0];// F0[1] = d->efc_force[1];// F0[2] = d->efc_force[2];// mju_mulMatTVec(JT_F,J1,F0,3,3);// double rhs[3] = {0};// rhs[0] = tau[0] + JT_F[0];// rhs[1] = tau[1] + JT_F[1];// rhs[2] = tau[2] + JT_F[2];//verify equations// printf("eqn1: %f %f \n",lhs[0],rhs[0]);// printf("eqn2: %f %f \n",lhs[1],rhs[1]);// printf("eqn3: %f %f \n",lhs[2],rhs[2]);//transitionsif (fsm==fsm_swing && d->qpos[5]>1){fsm = fsm_free;}//actionsif (fsm==fsm_swing){d->qfrc_applied[2] = -1*(d->qvel[2]-5);d->qfrc_applied[3] =  JT_F[0];d->qfrc_applied[4] =  JT_F[1];d->qfrc_applied[5] =  JT_F[2] + d->qfrc_applied[2];}if (fsm==fsm_free){d->qfrc_applied[3] =  0;d->qfrc_applied[4] =  0;d->qfrc_applied[5] =  0;}//printf("*****\n");//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, 7, 0.000000, 0.000000, 2.000000}; //view the left side (for ll, lh, left_side)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();init_controller(m,d);d->qpos[2] = -1.57;d->qpos[5] = d->qpos[2];// 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);//opt.frame = mjFRAME_WORLD; //mjFRAME_BODY//opt.flags[mjVIS_COM]  = 1 ; //mjVIS_JOINT;opt.flags[mjVIS_JOINT]  = 1 ;// 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 - hybrid system的建模与控制相关推荐

MATLAB优化转向器,汽车电动转向器动力学建模与控制仿真研究(MATLAB仿真)
汽车电动转向器动力学建模与控制仿真研究(MATLAB仿真)(任务书,开题报告,外文翻译,计划进度表,毕业论文12000字,相关框图和参数) 摘要汽车电动转向器是一种新型的汽车转向助力系统. 文章 ...
《机器人与数字人：基于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 ...
线性系统大作业——2.二阶倒立摆建模与控制系统设计（上）
文章目录 0.简介 1.建立数学模型 1.1.牛顿运动定律分析欧拉-拉格朗日方程分析 2.Simulink仿真 3.使用SimMechancis仿真 4.在平衡点附近模型线性化 5.系统能控性.能观 ...
质子交换膜燃料电池建模与控制研究
1.内容简介略268 2.内容说明静态模型公式静态模型公式要求:输入变量T.P.I.,输出ENernst.Vact.Vohm.Vcon.Vcell.V.P.η 绘制横轴I,纵轴V的伏安特曲线 ...
stm32代码生成，基于模型的设计(MBD) 无刷直流电机MATLAB开发板建模代码生成控制 MBD电机控制资料
stm32代码生成,基于模型的设计(MBD) 无刷直流电机MATLAB开发板建模代码生成控制 MBD电机控制资料控制算法采用MATLAB建模并生成代码的方式配套电机开发板模型源代码和视频 P ...
《A hybrid system for entity recognition ...》阅读笔记
2019独角兽企业重金招聘Python工程师标准>>> A hybrid system for entity recognition from Chinese clinical te ...
两轮差速移动机器人运动分析、建模和控制
两轮差速运动分析.建模和控制 1 运动学分析建模 1.1 三种运动状态分析 1.2 函数模型 1.3 仿真验证 1.3.1 直线验证 1.3.2 曲线验证 1.3.3 旋转验证 2 运动控制 2.1 ...
直线一级倒立摆数学建模与控制仿真
学习目标: 1.推导直线型一级倒立摆的数学建模公式,得到状态空间表达式和传递函数,并分析系统的稳定性 2.采用控制算法将系统从不稳定调整到稳定状态,并用matlab和simulink仿真实现学习内容 ...

MuJoCo - hybrid system的建模与控制

MuJoCo - hybrid system的建模与控制相关推荐

最新文章

热门文章