一、主要公式
二、源代码注释
三、相关原理解释
一、主要公式
二、源代码注释
该功能的实现在文件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 TaskDVec<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}=0qdot = 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_nxN_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); // 当前任务的雅可比矩阵JtJtPre = 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_qqdot = prev_qdot + JtPre_pinv * (task->getDesVel() - Jt * prev_qdot); // 对应 公式17，求出下一次迭代的qdot// 当前任务计算完成// 计算下一层级任务所需的变量，For the next task_BuildProjectionMatrix(JtPre, N_nx); // 当前任务的零空间矩阵N_nxN_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）的简单实现 - 知乎
