Mini Cheetah 代码分析（八）基于零空间的任务分级

一、主要公式

二、源代码注释

三、相关原理解释

一、主要公式

二、源代码注释

该功能的实现在文件KinWBC.cpp中的FindConfiguration函数，主要看注释，与公式是能够对应起来的，由第0个任务，也就是接触任务开始进行迭代，最终求出delta_q和qdot。qddot在另外一个文件WBIC.cpp中

#include "KinWBC.hpp"
// #include <Utilities/Utilities_print.h>
#include "PseudoInverse.h"

template <typename T>
KinWBC<T>::KinWBC(size_t num_qdot)
    : threshold_(0.001), num_qdot_(num_qdot), num_act_joint_(num_qdot - 6)
{
  I_mtx = DMat<T>::Identity(num_qdot_, num_qdot_);
}

template <typename T>
bool KinWBC<T>::FindConfiguration(
    const DVec<T> &curr_config, const std::vector<Task<T> *> &task_list,
    const std::vector<ContactSpec<T> *> &contact_list, DVec<T> &jpos_cmd,
    DVec<T> &jvel_cmd) //
{

  // Contact Jacobian Setup // contact任务的雅可比，堆叠成一个大矩阵
  DMat<T> Nc(num_qdot_, num_qdot_); // dotx=Jc*qdot，接触雅可比矩阵，维度为仿真
  std::cout << "num_qdot_:zhhw \n"
            << num_qdot_ << std::endl;
  Nc.setIdentity();
  if (contact_list.size() > 0)
  {
    DMat<T> Jc, Jc_i;
    contact_list[0]->getContactJacobian(Jc);
    size_t num_rows = Jc.rows();

    for (size_t i(1); i < contact_list.size(); ++i)
    {
      contact_list[i]->getContactJacobian(Jc_i);
      size_t num_new_rows = Jc_i.rows();
      Jc.conservativeResize(num_rows + num_new_rows, num_qdot_);
      Jc.block(num_rows, 0, num_new_rows, num_qdot_) = Jc_i; // 添加这个contact任务的雅可比
      num_rows += num_new_rows;
    }

    // Projection Matrix
    _BuildProjectionMatrix(Jc, Nc); // 对应公式20， 构建Jc雅可比矩阵的零空间矩阵Nc
  }

  // First Task
  DVec<T> delta_q, qdot;
  DMat<T> Jt, JtPre, JtPre_pinv, N_nx, N_pre;

  Task<T> *task = task_list[0];
  task->getTaskJacobian(Jt);
  JtPre = Jt * Nc;                   // 对应公式19，由Nc乘以当前任务的雅可比矩阵Jt
  _PseudoInverse(JtPre, JtPre_pinv); // 对应公式16中的系数矩阵

  delta_q = JtPre_pinv * (task->getPosError()); // 对应公式16，delt{q_0}=0
  qdot = JtPre_pinv * (task->getDesVel());

  DVec<T> prev_delta_q = delta_q; // 计算出delt{q_1}
  DVec<T> prev_qdot = qdot;
  // 当前任务计算完成
  //  计算下一层级任务所需的变量，For the next task
  _BuildProjectionMatrix(JtPre, N_nx); // 对应公式20， 构建JPre雅可比矩阵的零空间矩阵N_nx
  N_pre = Nc * N_nx;                   // 对应公式19，第二步，从N0开始，

  for (size_t i(1); i < task_list.size(); ++i) // 从i=1开始迭代
  {
    task = task_list[i];

    task->getTaskJacobian(Jt); // 当前任务的雅可比矩阵Jt
    JtPre = Jt * N_pre;        // 对应公式19，构建JPre

    _PseudoInverse(JtPre, JtPre_pinv); // JtPre矩阵的伪逆
    delta_q =
        prev_delta_q + JtPre_pinv * (task->getPosError() - Jt * prev_delta_q); // 对应公式16，求出速度增量delta_q
    qdot = prev_qdot + JtPre_pinv * (task->getDesVel() - Jt * prev_qdot);      // 对应 公式17，求出下一次迭代的qdot
    // 当前任务计算完成
    //  计算下一层级任务所需的变量，For the next task
    _BuildProjectionMatrix(JtPre, N_nx); // 当前任务的零空间矩阵N_nx
    N_pre *= N_nx;                       // 由最高层级第0个的零空间乘以当前的Npre得到当前的零空间，开始迭代，
    prev_delta_q = delta_q;              // 当前的delta_q用于下一次迭代
    prev_qdot = qdot;                    // 当前的qdot用于下一次迭代
  }
  for (size_t i(0); i < num_act_joint_; ++i)
  {
    jpos_cmd[i] = curr_config[i + 6] + delta_q[i + 6]; // 将数据发送出去
    jvel_cmd[i] = qdot[i + 6];
  }
  return true;
}

template <typename T>
void KinWBC<T>::_BuildProjectionMatrix(const DMat<T> &J, DMat<T> &N)
{
  DMat<T> J_pinv;
  _PseudoInverse(J, J_pinv);
  N = I_mtx - J_pinv * J; // 对应公式20，N=1-(Jc# * Jc)求解零空间
}

template <typename T>
void KinWBC<T>::_PseudoInverse(const DMat<T> J, DMat<T> &Jinv)
{
  pseudoInverse(J, threshold_, Jinv); // 计算矩阵的伪逆矩阵
}

template class KinWBC<float>;
template class KinWBC<double>;

三、相关原理解释

先通过一个简单的例子进行说明

基于零空间方法（NUB）的全身控制（WBC）的简单实现 - 知乎