MuJoCo Lec9

hopper的状态机控制

pos servo,vel servo 组合的作用

pos的增益有，vel的增益为0时，pos servo表现为在目标位置来回运动，好像弹簧的效果一样

Mujoco的auctor在目标位置震荡的演示

pos的增益和vel的增益都有，就是PD控制，从而快速的到达目标位置并停下来

//all actionsif (fsm == fsm_air1){//落下状态，这里pos servo,vel servo的设置是为了让pos servo迅速停停下来actuator_no = 2; //pservo-kneeset_position_servo(m,actuator_no,100);actuator_no = 3; //vservo-kneeset_velocity_servo(m,actuator_no,10);//d->ctrl[0] = 0;}if (fsm == fsm_stance1){//if (fsm==fsm_air1 && z_foot < 0.05 )//{//fsm = fsm_stance1;//printf("fsm_stance1 \n");//}//vel的参数设0，同时增大pos servo的参数，这样auctor会在目标点附近来回摆动//从而产生抗压缩的作用actuator_no = 2; //pservo-kneeset_position_servo(m,actuator_no,1000);actuator_no = 3; //vservo-kneeset_velocity_servo(m,actuator_no,0);}if (fsm == fsm_stance2){//if (fsm== fsm_stance1 && vz_torso > 0)//{//fsm = fsm_stance2;//printf("fsm_stance2 \n");//}//这里如果跟stance1一样的话，系统的能量会衰减比较厉害，大概30步会停下来//增大一点，相当于给系统注入多一点能量，不要那么快停下来actuator_no = 2; //pservo-kneeset_position_servo(m,actuator_no,1050);actuator_no = 3; //vservo-kneeset_velocity_servo(m,actuator_no,0);这里是顺时针旋转，这样子整个hopper是往前倾,从而底部的auctor产生//产生的力变为斜向上，以达到向前的目的d->ctrl[0] = -0.2;}if (fsm == fsm_air2){actuator_no = 2; //pservo-kneeset_position_servo(m,actuator_no,100);actuator_no = 3; //vservo-kneeset_velocity_servo(m,actuator_no,10);d->ctrl[0] = 0;//将脚的姿态恢复成垂直}

模型xml

<mujoco><visual>
<headlight ambient="0.5 0.5 0.5"/></visual><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"/> --><!-- <light mode="targetbody" target="torso" /> --><geom type="plane" size="100 1 0.1" rgba=".9 0 0 1"/><body name="torso" pos="0 0 2"><joint name="x" type="slide" pos="0 0 0" axis="1 0 0" /><joint name="z" type="slide" pos="0 0 0" axis="0 0 1" /><geom type="sphere" size="0.1" rgba=".9 .9 .9 1" mass="1"/><body name="leg" pos="0 0 -0.5" euler="0 0 0"><joint name="hip" type="hinge" pos="0 0 0.5" axis="0 -1 0" /><geom type="cylinder" size=".05 .5" rgba="0 .9 0 1" mass="1"/><body name="foot" pos="0 0 -0.75"><joint name="knee" type="slide" pos="0 0 0.25" axis="0 0 -1" /><geom type="cylinder" pos="0 0 0.125" size=".01 .125" rgba="0 0 .9 1" mass="0"/><geom type="sphere" size="0.05" rgba=".9 .9 0 1" mass="0.1"/></body></body></body></worldbody><actuator><position name="pservo-hip" joint="hip" kp="0"/><velocity name="vservo-hip" joint="hip" kv="0"/><position name="pservo-knee" joint="knee" kp="0"/><velocity name="vservo-knee" joint="knee" 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 = 50;int fsm;
#define fsm_air1 0
#define fsm_stance1 1
#define fsm_stance2 2
#define fsm_air2 3int step_no;//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/hopper/";
char xmlfile[] = "hopper.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)
{int actuator_no;actuator_no = 0; //pservo-hipset_position_servo(m,actuator_no,100);actuator_no = 1; //vservo-hipset_velocity_servo(m,actuator_no,10);actuator_no = 2; //pservo-kneeset_position_servo(m,actuator_no,1000);actuator_no = 3; //vservo-kneeset_velocity_servo(m,actuator_no,0);fsm = fsm_air1;step_no = 0;
}//**************************
void mycontroller(const mjModel* m, mjData* d)
{//write control here//Get data for transitionsint body_no;int actuator_no;// body_no = 3;// printf("foot pos: %f %f %f \n", d->xpos[3*body_no + 0],d->xpos[3*body_no + 1],d->xpos[3*body_no + 2]);// printf("z velocity torso: %f \n",d->qvel[1]);// printf("********* \n");//all transitionsbody_no = 3;double z_foot = d->xpos[3*body_no + 2];double vz_torso = d->qvel[1];if (fsm==fsm_air1 && z_foot < 0.05 ){fsm = fsm_stance1;//printf("fsm_stance1 \n");}if (fsm== fsm_stance1 && vz_torso > 0){fsm = fsm_stance2;//printf("fsm_stance2 \n");}if (fsm==fsm_stance2 && z_foot>0.05){fsm = fsm_air2;//printf("fsm_air2 \n");}if (fsm==fsm_air2 && vz_torso < 0){fsm = fsm_air1;//printf("fsm_air1 \n");step_no += 1;printf("step no = %d \n",step_no);}//all actionsif (fsm == fsm_air1){actuator_no = 2; //pservo-kneeset_position_servo(m,actuator_no,100);actuator_no = 3; //vservo-kneeset_velocity_servo(m,actuator_no,10);//d->ctrl[0] = 0;}if (fsm == fsm_stance1){actuator_no = 2; //pservo-kneeset_position_servo(m,actuator_no,1000);actuator_no = 3; //vservo-kneeset_velocity_servo(m,actuator_no,0);}if (fsm == fsm_stance2){actuator_no = 2; //pservo-kneeset_position_servo(m,actuator_no,1050);actuator_no = 3; //vservo-kneeset_velocity_servo(m,actuator_no,0);d->ctrl[0] = -0.2;}if (fsm == fsm_air2){actuator_no = 2; //pservo-kneeset_position_servo(m,actuator_no,100);actuator_no = 3; //vservo-kneeset_velocity_servo(m,actuator_no,10);d->ctrl[0] = 0;}//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}; //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);// 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 rendercam.lookat[0] = d->qpos[0];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 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 2D hopper相关推荐

2D池化IPoolingLayer
2D池化IPoolingLayer IPooling层在通道内实现池化.支持的池类型为最大, 平均和最大平均混合. 层描述:二维池化使用张量上的2D滤波器计算池化a tensor A, of d ...
CVPR2019论文解读：单眼提升2D检测到6D姿势和度量形状
CVPR2019论文解读:单眼提升2D检测到6D姿势和度量形状 ROI-10D: Monocular Lifting of 2D Detection to 6D Pose and Metric Sha ...
hnswlib RuntimeError: Cannot return the results in a contigious 2D array. Probably ef or M is to sma
1. 问题现象 index = hnswlib.Index(space = '100', dim = 512) index.init_index(max_elements = 100, ef_cons ...
【radar】毫米波雷达相关开源项目代码汇总（工具箱、仿真、2D毫米波检测、融合、4D毫米波检测、分割、SLAM、跟踪）（6）
[radar]毫米波雷达相关开源项目代码汇总(工具箱.仿真.2D毫米波检测.融合.4D毫米波检测.分割.SLAM.跟踪)(6) Toolbox pymmw https://github.com/m6c ...
【radar】毫米波雷达动态障碍物检测相关论文汇总（聚类、分类、稀疏2D点、4D点、雷达成像、原始数据处理）（4）
[radar]毫米波雷达动态障碍物检测相关论文汇总(聚类.分类.稀疏2D点.4D点.雷达成像.原始数据处理)(4) Detection of Dynamic Objects Clustering 20 ...
从2D到3D的目标检测综述
点云PCL免费知识星球,点云论文速读. 文章:An Overview Of 3D Object Detection 作者:Yilin Wang Jiayi Ye 翻译:分享者本文仅做学术分享,如有 ...
实现2D全景图的中心视野变换
对于同一场景的2D全景图,如果想改变其视野中心位置,比如下图,初始情况下视野的中心位置是蓝框,如果想让红框的灯位于中心位置该怎么做呢? #include "opencv2/highgui/h ...
Vyond制作2D动画学习教程
Vyond为2D动画提供了极其简单的分解视频创建过程. 你会学到什么课程获取:Vyond制作2D动画学习教程-云桥网您将学习如何为2d动画制作画外音您将学习如何使用Vyond轻松创建精彩的动画视 ...
Unity 创建2D平台游戏开发学习教程
了解如何使用C#在Unity中创建您的第一款2D平台游戏你会学到什么使用Unity创建2D奥运会使用可脚本化的对象和单一模式使用良好的编程实践创造武器和射弹使用可脚本化的对象和委托模式创建 ...
学习用C#在Unity中创建一个2D Metroidvania游戏
学习用C#在Unity中创建一个2D Metroidvania游戏你会学到: 构建2D Unity游戏用C#编程玩家统计,水平提升,米尔和远程攻击敌方人工智能系统制定级别和级别选择 Lear ...

Mujoco 2D hopper

hopper的状态机控制

模型xml

控制代码

Mujoco 2D hopper相关推荐

最新文章

热门文章