一、主要公式
二、源代码注释
三、相关原理解释
一、主要公式
二、源代码注释
该功能的实现在文件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）的简单实现 - 知乎
