Mujoco 控制器接口（四）

啥也不是的py人

已于 2023-06-08 17:03:37 修改

阅读量1.7k

点赞数 3

分类专栏： # mujoco 文章标签：人工智能

于 2023-06-07 21:54:22 首次发布

本文链接：https://blog.csdn.net/weixin_50920579/article/details/131096252

版权

mujoco 专栏收录该内容

4 篇文章 3 订阅

订阅专栏

.1 简介

control看到下面的图中就是mjtNum类型

mjtNum实际上就是浮点数

ctrl是底层的输入

外界扰动就是通过上图的qfrc和xfrc来添加的

1.1 控制器实现

实际上加上控制器就是xml里添加actuator

joint就是对应由哪个电机控制

motor代表电机

ctrllimited代表的是电机的转动范围有限

这里说明了mujoco的逻辑是siso就是单输入得到单输出

需要注意的是输入都被保存在变量mjData.ctrl的变量里

force outputs保存在另一个变量里

.2 sim.step()

这个函数做了什么

先是检查参数是否在范围内

然后进行正向动力学的计算（位置速度来计算加速度）

然后检查加速度是不是合理的状态

然后进行差分迭代

最后是一个积分器，欧拉积分器

在一个while循环中添加每一步的控制量，再写一个step就可以开始循环这个函数很方便

.3 实例

import mujoco_py as mp

model = mp.load_model_from_path('ur5.xml')
sim = mp.MjSim(model)
viewer = mp.MjViewer(sim)

for i in range(3000):
    # sim.data.ctrl[:6] = 1
    sim.step()
    viewer.render()

先是读取了ur5的model

ur5的model里添加了相应的actuator

然后建立Mjsim的仿真并传入viewer中显示

sim = mp.MjSim(model)
viewer = mp.MjViewer(sim)

然后开启循环

for i in range(3000):
    # sim.data.ctrl[:6] = 1
    sim.step()
    viewer.render()

把每个关节的控制输入给到1

sim.data.ctrl[:6] = 1

然后step开始输出

输出完之后进行render

完整代码：

import mujoco_py as mp

model = mp.load_model_from_path('ur5.xml')
sim = mp.MjSim(model)
viewer = mp.MjViewer(sim)

for i in range(3000):
    sim.data.ctrl[:6] = 1
    sim.step()
    viewer.render()

运行

.4 连杆模型编写

先是link1

<mujoco>
    <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 0" euler="0 0 0">
            <joint type="free"/>
            <geom type="box" size=".1 .1 .3" rgba="0 .9 0 1"/>
        </body>
        
    </worldbody>
</mujoco>

先是写一个带joint的双连杆

<mujoco>
    <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 0" >
            <inertial pos="0 0 0" mass="3.7" diaginertia="0.0102675 0.0102675 0.00666" />
            <joint type="free"/>
            <geom type="box" contype="0" conaffinity="0" group="1" size=".1 .1 .3" rgba="0 .9 0 1"/>
            <geom type='box' size=".1 .1 .3" rgba="0 .9 0 1"/>
            <body pos="0 0 .6" quat="1 0 0 0">
                <inertial pos="0 0 0" mass="3.7" diaginertia="0.0102675 0.0102675 0.00666" />
                <joint name="joint1" pos="0 0 .3" axis="0 1 0" limited="true" range="-3.14159 3.14159" />
                <geom type="box" contype="0" conaffinity="0" group="1" size=".1 .1 .3" rgba="0 .9 0 1"/>
                <geom type="box" size=".1 .1 .3" rgba="1 .9 0 1"/>
            </body>
        </body>        
    </worldbody>
</mujoco>

注意joint的默认参考坐标是物体也就是model的中心

所以按照以下编码

<mujoco>
    <compiler angle="radian" />
    <worldbody>
        <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
        <geom type="plane" size="10 10 0.1" rgba=".9 0 0 1"/>
        <body pos="0 0 .25" >
            <inertial pos="0 0 0" mass="10" diaginertia="0.0102675 0.0102675 0.00666" />
            <joint type="free"/>
            <geom type="box" contype="0" conaffinity="0" group="1" size=".1 .1 .25" rgba="0 .9 0 1"/>
            <geom type='box' size=".1 .1 .25" rgba="0 .9 0 1"/>
            <body pos="0 0 .5" quat="1 0 0 0">
                <inertial pos="0 0 0" mass="3.7" diaginertia="0.0102675 0.0102675 0.00666" />
                <joint name="joint1" pos="0 0 -0.3" axis="0 1 0" limited="true" range="-3.14159 3.14159" />
                <geom type="box" contype="0" conaffinity="0" group="1" size=".1 .1 .2" rgba="0 .9 0 1"/>
                <geom type="box" size=".1 .1 .2" rgba="1 .9 0 1"/>
            </body>
        </body>        
    </worldbody>
    <actuator>
        <motor name='joint1' ctrllimited="true" ctrlrange="-0.5 0.5" joint='joint1' gear="101"/> <!--range -150 150-->
    </actuator>
</mujoco>