MuJoCo = Multi-Joint dynamics with Contact

MuJoCo = Modeling + Simulation + Visualization

MuJoCo Documentation (old)

MuJoCo Documentation (new)

  • Overview
  • Computation
  • Modeling -> XML Reference
  • Programming -> API Reference
root [⚓] => /body/
    torso_to_abduct_fr_j [⚙+X] => /abduct_fr/
        abduct_fr_to_thigh_fr_j [⚙-Y] => /thigh_fr/
            thigh_fr_to_knee_fr_j [⚙-Y] => /shank_fr/
                toe_fr_joint [⚓] => /toe_fr/
    torso_to_abduct_fl_j [⚙+X] => /abduct_fl/
        abduct_fl_to_thigh_fl_j [⚙-Y] => /thigh_fl/
            thigh_fl_to_knee_fl_j [⚙-Y] => /shank_fl/
                toe_fl_joint [⚓] => /toe_fl/
    torso_to_abduct_hr_j [⚙+X] => /abduct_hr/
        abduct_hr_to_thigh_hr_j [⚙-Y] => /thigh_hr/
            thigh_hr_to_knee_hr_j [⚙-Y] => /shank_hr/
                toe_hr_joint [⚓] => /toe_hr/
    torso_to_abduct_hl_j [⚙+X] => /abduct_hl/
        abduct_hl_to_thigh_hl_j [⚙-Y] => /thigh_hl/
            thigh_hl_to_knee_hl_j [⚙-Y] => /shank_hl/
                toe_hl_joint [⚓] => /toe_hl/

命名约定

类型前缀 用途
mj 核心数据结构
mjt 主要类型
mjf 回调函数类型
mjv 可视化相关数据结构
mjr OpenGL 渲染相关数据结构
mjui UI 相关数据结构
函数前缀 用途
mj_ 核心函数
mju_ 数学函数
mjv_ 可视化相关函数
mjr_ OpenGL 渲染相关函数
mjui_ UI 相关函数
mjcb_ 全局回调函数指针

全局回调函数

// callbacks extending computation pipeline
MJAPI extern mjfGeneric  mjcb_passive;
MJAPI extern mjfGeneric  mjcb_control;
MJAPI extern mjfConFilt  mjcb_contactfilter;
MJAPI extern mjfSensor   mjcb_sensor;
MJAPI extern mjfTime     mjcb_time;
MJAPI extern mjfAct      mjcb_act_dyn;
MJAPI extern mjfAct      mjcb_act_gain;
MJAPI extern mjfAct      mjcb_act_bias;

mjModel 数据结构

具体内容

  • 模型子模块的具体数目
  • 物理引擎参数、可视化参数、模型统计
  • 缓冲区指针

mjData 数据结构

具体内容

  • 头部内容
    • 栈和缓冲区的空间
    • 栈指针
    • 内存使用统计
    • 诊断信息
    • 变量数目
    • 全局属性
    • 缓冲区指针
  • 输入输出
    • 状态量
    • 控制量
    • 动力学
      • qacc
    • 传感器数据
  • 位置依赖型数据
  • 位置、速度依赖型数据
  • 位置、速度、控制量/加速度依赖型数据
    • qfrc_inverse

HOW TO 仿真

初始化 = mjModel + mjData

初始化 mjModel

// option 1: parse and compile XML from file
mjModel* m = mj_loadXML("mymodel.xml", NULL, errstr, errstr_sz);

// option 2: parse and compile XML from virtual file system
mjModel* m = mj_loadXML("mymodel.xml", vfs, errstr_sz);

// option 3: load precompiled model from MJB file
mjModel* m = mj_loadModel("mymodel.mjb", NULL);

// option 4: load precompiled model from virtual file system
mjModel* m = mj_loadModel("mymodel.mjb", vfs);

// option 5: deep copy from existing mjModel
mjModel* m = mj_copyModel(NULL, mexisting);

初始化 mjData

// option 1: create mjDada corresponding to given mjModel
mjData* d = mj_makeData(m);

// option 2: deep copy from existing mjData
mjData* d = mj_copyData(NULL, m, dexisting);

释放资源

// deallocate existing mjModel
mj_deleteModel(m);

// deallocate existing mjData
mj_deleteData(d);

仿真主循环 = 正动力学 + 积分

// simulate until t = 10 seconds
while( d->time<10 )
    mj_step(m, d);

控制量回调函数方式 (官方推荐的方法‼️)

// simple controller applying damping to each dof
void mycontroller(const mjModel* m, mjData* d)
{
    if( m->nu==m->nv )
        mju_scl(d->ctrl, d->qvel, -0.1, m->nv);
}

// install control callback
mjcb_control = mycontroller;

错误的控制量直接赋值方式❌

while( d->time<10 )
{
    // set d->ctrl or d->qfrc_applied or d->xfrc_applied
    mj_step(m, d);
}

正确的控制量直接赋值方式

while( d->time<10 )
{
    mj_step1(m, d);
    // set d->ctrl or d->qfrc_applied or d->xfrc_applied
    mj_step2(m, d);
}

mj_step/mj_step1/mj_step2 实现细节

void mj_step(const mjModel* m, mjData* d)
{
    // common to all integrators
    mj_checkPos(m, d);
    mj_checkVel(m, d);
    mj_forward(m, d);
    mj_checkAcc(m, d);

    // compare forward and inverse solutions if enabled
    if( mjENABLED(mjENBL_FWDINV) )
        mj_compareFwdInv(m, d);

    // use selected integrator
    if( m->opt.integrator==mjINT_RK4 )
        mj_RungeKutta(m, d, 4);
    else
        mj_Euler(m, d);
}

void mj_step1(const mjModel* m, mjData* d)
{
    mj_checkPos(m, d);
    mj_checkVel(m, d);
    mj_fwdPosition(m, d);
    mj_sensorPos(m, d);
    mj_energyPos(m, d);
    mj_fwdVelocity(m, d);
    mj_sensorVel(m, d);
    mj_energyVel(m, d);

    // if we had a callback we would be using mj_step, but call it anyway
    if( mjcb_control )
        mjcb_control(m, d);
}

void mj_step2(const mjModel* m, mjData* d)
{
    mj_fwdActuation(m, d);
    mj_fwdAcceleration(m, d);
    mj_fwdConstraint(m, d);
    mj_sensorAcc(m, d);
    mj_checkAcc(m, d);

    // compare forward and inverse solutions if enabled
    if( mjENABLED(mjENBL_FWDINV) )
        mj_compareFwdInv(m, d);

    // integrate with Euler; ignore integrator option
    mj_Euler(m, d);
}

状态量和控制量

x = (mjData.time, mjData.qpos, mjData.qvel, mjData.act)
u = (mjData.ctrl, mjData.qfrc_applied, mjData.xfrc_applied)

在关节空间施加力/力矩 - mjData.qfrc_applied 在欧式空间施加力/力矩 - mjData.xfrc_applied

dx/dt = mj_forward(t,x,u) = (1, mjData.qvel, mjData.qacc, mjData.act_dot)

清楚所有控制量

// clear controls and applied forces
mju_zero(dst->ctrl, m->nu);
mju_zero(dst->qfrc_applied, m->nv);
mju_zero(dst->xfrc_applied, 6*m->nbody);

⚠️Note that MuJoCo will not clear these controls/forces at the end of the time step. This is the responsibility of the user.

正动力学

void mj_forwardSkip(const mjModel* m, mjData* d,
                    int skipstage, int skipsensor)
{
    // position-dependent
    if( skipstage<mjSTAGE_POS )
    {
        mj_fwdPosition(m, d);
        if( !skipsensor )
            mj_sensorPos(m, d);
        if( mjENABLED(mjENBL_ENERGY) )
            mj_energyPos(m, d);
    }

    // velocity-dependent
    if( skipstage<mjSTAGE_VEL )
    {
        mj_fwdVelocity(m, d);
        if( !skipsensor )
            mj_sensorVel(m, d);
        if( mjENABLED(mjENBL_ENERGY) )
            mj_energyVel(m, d);
    }

    // acceleration-dependent
    if( mjcb_control )
        mjcb_control(m, d);
    mj_fwdActuation(m, d);
    mj_fwdAcceleration(m, d);
    mj_fwdConstraint(m, d);
    if( !skipsensor )
        mj_sensorAcc(m, d);
}

逆动力学 RNE (MuJoCo 特有功能) -> state estimation, system identification and optimal control

有解析解，用于正动力学的热启动

输入：

(mjData.qpos, mjData.qvel, mjData.qacc, mjData.mocap_pos, mjData.mocap_quat)

输出：mjData.qfrc_inverse

mjData.qfrc_inverse = mjData.qfrc_applied + Jacobian’*mjData.xfrc_applied + mjData.qfrc_actuator

多线程

略

模型调整

略

数据结构的内存布局和缓冲区分配

矩阵元素按照行的方式 (row-major) 存储

linear memory array a0,a1,...,a5 represents the 2x3 matrix

a0 a1 a2
a3 a4 a5

约束雅可比矩阵 mjData.efc_J 采用稀疏表示 (compressed sparse row)

3D 姿态用单位四元数 q = (w,x,y,z) 表示

MuJoCo also uses 6D spatial vectors internally.

X Macros

内部栈空间

略

错误、警告和内存分配

略

诊断

时间统计

雅可比

Jacobian maps joint velocities to end-effector velocities, while the transpose of the Jacobian maps end-effector forces to joint forces.

mj_jac

碰撞

mjtNum* contactforce = d->efc_force + d->contact[i].efc_address;

坐标系与变换矩阵

  • 关节坐标系
  • 笛卡尔坐标

References