#1 环境
1.1 win11: qt.5.14.2+mujoco
1.2 mujoco版本 :mujoco210
2 qt工程:
我创建的是qt consol application。
工程主要的工作是引入mujoco和glfw3的库
TEMPLATE = app
CONFIG += console c++11
CONFIG -= app_bundle
CONFIG -= qt
SOURCES += \
main.cpp
INCLUDEPATH += H:\mujoco_dev\include
INCLUDEPATH += H:/mujoco_dev/glfw3/include/GLFW
DEPENDPATH +=H:\mujoco_dev\glfw3\include
#.lib
LIBS += -LH:\mujoco_dev\lib -lmujoco
LIBS += -LH:\mujoco_dev\lib -lmujoco_nogl
LIBS += -LH:/mujoco_dev/glfw3/lib-vc2015 -lglfw3
LIBS +=-lgdi32 -lopengl32 -lkernel32 -luser32 -lshell32
3 用来测试的mojoco模型以及控制代码
完整的模型代码
<mujoco>
<option>
<flag sensornoise="enable" />
</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" euler="0 0 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>
</worldbody>
<actuator>
<!-- <motor joint="pin" name="torque" gear="1" ctrllimited="true" ctrlrange="-100 100" /> -->
<position name="position_servo" joint="pin" kp="100" />
<velocity name="velocity_servo" joint="pin" kv="0" />
</actuator>
<sensor>
<jointpos joint="pin" noise="0.2"/>
<jointvel joint="pin" noise="1" />
</sensor>
</mujoco>
#include <QCoreApplication>
#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"
char filename[] = "H:/mujoco_learn/Code/control_pendulum/pendulum.xml";
// 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);
}
//*************************************
void set_torque_control(const mjModel *m, int actuator_no, int flag)
{
if (flag == 0)
m->actuator_gainprm[10 * actuator_no + 0] = 0;
else
m->actuator_gainprm[10 * actuator_no + 0] = 1;
}
//***********************************
//*************************************
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 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 mycontroller(const mjModel *m, mjData *d)
{
int i;
int actuator_no;
//0 = torque actuator
actuator_no = 0;
int flag = 0;
set_torque_control(m, actuator_no, flag);
d->ctrl[0] = -10 * (d->qpos[0] - 0) - 1 * d->qvel[0]; //PD control
//d->ctrl[0] = -10*(d->sensordata[0]-0)-1*d->sensordata[1];
//1=position servo
actuator_no = 1;
double kp = 0;
set_position_servo(m, actuator_no, kp);
//for (i=0;i<10;i++)
// {
// //printf("%f \n", m->actuator_gainprm[10*actuator_no+i]);
// //printf("%f \n", m->actuator_biasprm[10*actuator_no+i]);
// }
//printf("*********** \n");
d->ctrl[1] = 0.5;
//2= velocity servo
actuator_no = 2;
double kv = 0;
set_velocity_servo(m, actuator_no, kv);
d->ctrl[2] = 0.2;
//PD control
actuator_no = 1;
double kp2 = 10;
set_position_servo(m, actuator_no, kp2);
actuator_no = 2;
double kv2 = 1;
set_velocity_servo(m, actuator_no, kv2);
d->ctrl[1] = -0.5;
d->ctrl[2] = 0;
}
// main function
int main(int argc, char **argv)
{
QCoreApplication a(argc, argv);
// load and compile model
char error[1000] = "Could not load binary model";
// check command-line arguments
if (argc < 2)
m = mj_loadXML(filename, 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[] = {90, -5, 5, 0.012768, -0.000000, 1.254336};
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];
d->qpos[0] = 1.57; //pi/2
mjcb_control = mycontroller;
// 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);
}
// 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 a.exec();
}
4 一些错误的解决:
类似 glfw3.lib(win32_init.obj)👎 error: LNK2019: 无法解析的外部符号 __imp_DispatchMessageW,该符号在函数 createHelperWindow 中被引用
这样的错误
这种问题参考[2]:
结果在看到Link problem when compiling from terminal - support - GLFW的时候灵机一动,查了一下,这些函数不是glfw的,是Windows的。
解决方法:打开.pro工程文件,在LIBS +=后面写上-lgdi32 -lopengl32 -lkernel32 -luser32 -lshell32 ----
5 效果
6 依赖的库
7 reference
[1] glfw: https://www.glfw.org/download.html
[2] 一些错误的解决:https://blog.csdn.net/weixin_43873801/article/details/107695323