Mujoco平面双足机器人模拟

Feisy

已于 2022-03-13 16:27:22 修改

阅读量2.2k

点赞数 9

CC 4.0 BY-SA版权

分类专栏： Mujoco 文章标签： mujoco

于 2022-03-11 10:47:19 首次发布

本文链接：https://blog.csdn.net/feisy/article/details/123419051

内容

本教程主要讲解了一个3DOF的平面机器人在mujoco
中的模拟

xml模型文件的建立
怎么获取每个关节的位置和速度
怎么获取每个躯干的姿态（四元数表示，以及将其转成欧拉形式）
控制方法是通过设置重力在x方向上的0.1倍作为驱动机器人前行的动力,通知摆动脚前半段缩起，后半段再伸出

视频教程

MuJoCo Lec13

模型代码

因为是2D行走，所以base是有3个自由度的：

			<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" />

这里将base的关节直接就建在Leg上，有点不适合。
正常的情况下，base的自由度是独立于腿的，附在pelvis上，类似下面的的方法：

      <body name="pelvis" pos="0 0 0.355"> 
        <inertial diaginertia="0.0006208 0.000533 0.0000389" mass="0.256" pos="0 0 0"/>
        <!-- <joint type="free" limited='false'  damping="0" stiffness="0" armature="0" pos="0 0 0"/> -->
        <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'/>
        <joint name="rotx" axis="1 0 0" type="hinge" limited="false" armature="0" damping="0"/>
        <joint name="roty" axis="0 1 0" type="hinge" limited="false" armature="0" damping="0"/>
        <joint name="rotz" axis="0 0 1" type="hinge" limited="false" armature="0" damping="0"/>

另外，还有一个比较奇怪的地方，Leg2居然是作为Leg1的子树，其实Leg1,Leg2应该是独立的分支
才对，这样后面做动力学推导时，才比较简单。

完整的模型代码

<mujoco>

	<visual>
			<headlight ambient="0.25 0.25 0.25"/>
	</visual>

	<option timestep="0.001" integrator="RK4" gravity="0 0 0">
		<flag sensornoise="disable" contact="enable" energy="enable"/>
	</option>

	<worldbody>
		<geom type="plane" size="1000 5 0.1" rgba=".9 0 0 1"/>

		<body name="leg1" pos="0 0 0.75" 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 1" mass="1"/>
			<body name="foot1" pos="0 0 -0.75">
				<joint name="knee1" type="slide" pos="0 0 0.25" axis = "0 0 -1" />
				<geom type="sphere" size=".05" rgba=".9 .9 0 1" mass="0.1"/>
			</body>
			<body name="leg2" pos="0 0.25 0" 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=".9 .9 .9 1" mass="1"/>
				<body name="foot2" pos="0 0 -0.75">
					<joint name="knee2" type="slide" pos="0 0 0.25" axis = "0 0 -1" />
					<geom type="sphere" size=".05" rgba=".9 .9 0 1" mass="0.1"/>
				</body>
			</body>
		</body>

	</worldbody>

	<!-- <equality>
			<connect body1='pole' body2='world' anchor='0 0 0.5'/>
	</equality> -->

	<actuator>
		<position name="pservo_hip" joint="hip" kp="4"/>
		<velocity name="vservo_hip" joint="hip" kv="1"/>
		<position name="pservo_knee1" joint="knee1" kp="1000"/>
		<velocity name="vservo_knee1" joint="knee1" kv="100"/>
		<position name="pservo_knee2" joint="knee2" kp="1000"/>
		<velocity name="vservo_knee2" joint="knee2" kv="100"/>
	</actuator>


</mujoco>

控制代码

main.c

#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"

#include "util.h"