【四足机器人--关节初始化时足端位置（速度、加速度）轨迹规划】（4.2）JPosInitializer（B样条曲线计算脚的摆动轨迹）代码解析
丁念
系列文章目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理
前言
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对JPosInitializer（B样条曲线计算脚的摆动轨迹）代码解析做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章
提示：以下是本篇文章正文内容
一、关节初始化构造函数（设置目标时间及间隔）
/*!
* 功能：关节初始化构造函数（设置目标时间及间隔）
*/
template <typename T> //C++模板
JPosInitializer<T>::JPosInitializer(T end_time, float dt)
: _b_first_visit(true),
_end_time(end_time),
_curr_time(0.),
_dt(dt),
_ini_jpos(cheetah::num_act_joint)
{
_UpdateParam();
}
/*!
* 功能：关节初始化，更新控制器参数函数
* 步骤：
* （1）从yaml文件加载关节位置控制初始化参数，更新参数
* （2）获得target_jpos矩阵
* （3）获得mid_jpos矩阵
*/
template <typename T>
void JPosInitializer<T>::_UpdateParam()
{
ParamHandler handler(THIS_COM "config/initial_jpos_ctrl.yaml"); //从yaml文件加载关节位置控制初始化参数
handler.getVector<T>("target_jpos", _target_jpos); //_target_jpos矩阵
handler.getVector<T>("mid_jpos", _mid_jpos); //_mid_jpos矩阵
}
.
.
二、给定并B样条起始点、中间点、落脚点，目标时间点四个参数
/*!
* 功能：给定并B样条起始点、中间点、落脚点，目标时间点四个参数
* 步骤：
* （1）定义变量
* （2）复位每条腿的参数
* （3）设置每条腿的参数：起始点、中间点、落脚点，目标时间点
* （4）根据上面填充的数据，使用B样条的方法，进行更新关节轨迹参数
*/
template <typename T>
void JPosInitializer<T>::_UpdateInitial(const LegController<T>* ctrl)
{
/*（1）定义起始点、中间点、落脚点变量*/
T ini[3 * cheetah::num_act_joint]; //定义起始点
T fin[3 * cheetah::num_act_joint]; //定义落脚点
T** mid = new T*[1]; //定义中间点
mid[0] = new T[cheetah::num_act_joint];
/*（2）复位每条腿起始点和落点的参数为0*/
for (size_t i(cheetah::num_act_joint); i < 3 * cheetah::num_act_joint; ++i) /
{
ini[i] = 0.;
fin[i] = 0.;
}
/*（3）加载每条腿三个关节起始点、中间点和落点位置的参数*/
//起始点
for (int leg(0); leg < 4; ++leg) //四条腿
{
for (int jidx(0); jidx < 3; ++jidx) //每条腿的三个关节
{
ini[3 * leg + jidx] = ctrl->datas[leg].q[jidx]; //加载对应的参数
_ini_jpos[3 * leg + jidx] = ctrl->datas[leg].q[jidx];
}
}
//中间点和落脚点
for (size_t i(0); i < cheetah::num_act_joint; ++i) //加载对应的关节位置参数
{
fin[i] = _target_jpos[i];
mid[0][i] = _mid_jpos[i];
}
/*（4）根据上面填充的数据，使用B样条的方法，进行更新关节轨迹参数*/
_jpos_trj.SetParam(ini, fin, mid, _end_time);
delete[] mid[0];
delete[] mid;
}
.
.
三、按照B样条曲线平稳移动跟踪到匍匐状态的函数（进行关节初始化）
/*!
* 功能：按照B样条曲线平稳移动跟踪到匍匐状态的函数（进行关节初始化）
*/
template <typename T>
bool JPosInitializer<T>::IsInitialized(LegController<T>* ctrl)
{
_curr_time += _dt;
// 初始设置
if (_b_first_visit) //在第一次进来的时候更新控制指令
{
_UpdateInitial(ctrl); //给定并设置B样条起始点、中间点、落脚点，目标时间点四个参数
_b_first_visit = false;
}
// 进行B样条曲线跟踪
if (_curr_time < _end_time)
{
T jpos[cheetah::num_act_joint];
//获取给定时间的B样条线位置,存到jpos
_jpos_trj.getCurvePoint(_curr_time, jpos);
//遍历4条腿的三个电机，进行位置控制B样条曲线跟踪
for (size_t leg(0); leg < cheetah::num_leg; ++leg)
{
for (size_t jidx(0); jidx < cheetah::num_leg_joint; ++jidx)
{
ctrl->commands[leg].tauFeedForward[jidx] = 0.; //设置关节前馈力矩为0
ctrl->commands[leg].qDes[jidx] = jpos[3 * leg + jidx]; //设置关节期望位置
ctrl->commands[leg].qdDes[jidx] = 0.; //设置关节期望速度=0
}
}
return false;
}
return true;
}
.
.
四、B样条实现的库Basic Bspline（看成小黑盒子，会用API就能部署）
我把整个库放上来把，有点多
#ifndef B_SPLINE_BASIC
#define B_SPLINE_BASIC
#include <assert.h>
#include <stdio.h>
#include <iostream>
#define SP_IS_EQUAL(x, y) (((x) - (y)) * ((x) - (y)) < 1.e-10)
#define SP_SAFE_DELETE_AR(p) \
if (p) { \
delete[] p; \
(p) = NULL; \
}
/*!
* Basic Bspline <br/>
* DIM : Dimension of control points <br/>
* DEGREE : Derivation is going to be 0 when it is over DEGREE <br/>
* NUM_MIDDLE : Num middle points (the points except initial and final) <br/>
* <br/>
* CONST_LEVEL_INI/FIN : constraint level <br/>
* 0: position <br/>
* 1: + velocity <br/>
* 2: + acceleration <br/>
* <br/>
* ****************************** WARNING
* *********************************************** <br/> NumKnots(DEGREE +
* NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN) >= 2 * (DEGREE + 1) <br/>
* ****************************** WARNING
* *********************************************** <br/>
*/
template <typename T, int DIM, int DEGREE, int NUM_MIDDLE, int CONST_LEVEL_INI,
int CONST_LEVEL_FIN>
class BS_Basic
{
public:
BS_Basic()
: NumKnots_(DEGREE + NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN +
1),
NumCPs_(NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN)
{
for (int i(0); i < NumKnots_; ++i) Knots_[i] = 0.;
for (int i(0); i < NumCPs_; ++i) {
for (int j(0); j < DIM; ++j) CPoints_[i][j] = 0.;
}
if (NumKnots_ < 2 * (DEGREE + 1)) {
printf("Invalid setup (num_knots, degree): %d, %d\n", NumKnots_, DEGREE);
}
}
~BS_Basic() {}
/*!
* size of T: DIM * CONST_LEVEL_INI (or CONST_LEVEL_FIN) <br/>
* ex) if dim:3, const level ini: 3(pos, vel, acc) <br/>
* ini[0 ~ 2]: pos <br/>
* ini[3 ~ 5]: vel <br/>
* ini[6 ~ 8]: acc <br/>
* @param init : 初始点信息（矢量） <br/>
* @param fin : 终点信息（矢量） <br/>
* @param middle_pt : 中间点信息（矩阵） <br/>
* @param fin_time : 样条曲线的持续时间 <br/>
* @return boolean : success <br/>
*/
bool SetParam(T* init, T* fin, T** middle_pt, T fin_time)
{
_CalcKnot(fin_time); //计算
_CalcConstrainedCPoints(init, fin, fin_time);
_CalcCPoints(middle_pt);
return true;
}
/*!
* 获取给定时间的样条线位置 <br/>
* 如果输入时间在0之前，则返回初始值 <br/>
* 如果输入时间在最后一次之后，则返回最后一次 <br/>
* @param u : time <br/>
* @return ret : position at the given time. <br/>
*/
bool getCurvePoint(T u, T* ret)
{
int _span;
if (u < Knots_[0])
u = Knots_[0];
else if (u > Knots_[NumKnots_ - 1]) {
u = Knots_[NumKnots_ - 1];
}
if (!_findSpan(_span, u)) return false;
T _N[DEGREE + 1];
_BasisFuns(_N, _span, u);
T _C[DIM];
for (int j(0); j < DIM; ++j) {
_C[j] = 0.0;
for (int i(0); i <= DEGREE; ++i) {
_C[j] += _N[i] * CPoints_[_span - DEGREE + i][j];
}
}
for (int i(0); i < DIM; ++i) {
ret[i] = _C[i];
}
return true;
}
/*!
* 得到给定时刻的样条导数信息<br/>
* 如果输入时间在0之前，则返回初始值 <br/>
* 如果输入时间在最后一次之后，则返回最后一次 <br/>
* @param u : time <br/>
* @param d : drivative level (e.g. 1: velocity, 2: acceleration) <br/>
* @return ret : derivative information at the given time. <br/>
*/
bool getCurveDerPoint(T u, int d, T* ret)
{
if (d > DEGREE) return 0.0;
if (u < Knots_[0])
u = Knots_[0];
else if (u > Knots_[NumKnots_ - 1]) {
u = Knots_[NumKnots_ - 1];
}
T** _CK = new T*[d + 1];
for (int i(0); i < d + 1; ++i) {
_CK[i] = new T[DIM];
}
if (_CurveDerivsAlg1V(_CK, u, d)) {
for (int m(0); m < DIM; ++m) ret[m] = _CK[d][m];
for (int p(0); p < d + 1; ++p) delete[] _CK[p];
SP_SAFE_DELETE_AR(_CK);
return true;
} else {
for (int p(0); p < d + 1; ++p) delete[] _CK[p];
SP_SAFE_DELETE_AR(_CK);
}
return false;
}
// protected:
private:
inline void _CalcKnot(T Tf)
{
int _i(0);
int _j(0);
int _NumMidKnot(NumKnots_ - 2 * DEGREE - 2);
T _TimeStep = Tf / (_NumMidKnot + 1);
// 增加初始部分的结序列
// # of order ( degree + 1 )
for (_j = 0; _j < DEGREE + 1; ++_j) Knots_[_i++] = 0.0;
// 中间部分均匀的结序，
// #: NumKnot - degree - degree = NumKnot - order - order + 2
for (_j = 0; _j < _NumMidKnot; ++_j) {
Knots_[_i] = Knots_[_i - 1] + _TimeStep;
++_i;
}
// 为最后一部分增加结序列
// # of order ( degree + 1 )
for (_j = 0; _j < DEGREE + 1; ++_j) Knots_[_i++] = Tf;
// for(int i(0); i< NumKnots_; ++i)
// std::cout<<Knots_[i]<<std::endl;
}
bool _CurveDerivsAlg1V(T** CK, T u, int d)
{
assert(d <= DEGREE);
int _k(0);
int _j(0);
T** _nders = new T*[d + 1];
for (_k = 0; _k < d + 1; ++_k) _nders[_k] = new T[DEGREE + 1];
int _span;
if (!_findSpan(_span, u)) return false;
_BasisFunsDers(_nders, _span, u, d);
for (_k = 0; _k <= d; ++_k) {
// Clean Up Column
for (int m(0); m < DIM; ++m) CK[_k][m] = 0.;
for (_j = 0; _j <= DEGREE; ++_j) {
for (int m(0); m < DIM; ++m) {
CK[_k][m] += _nders[_k][_j] * CPoints_[_span - DEGREE + _j][m];
}
}
}
for (_k = 0; _k < d + 1; ++_k) delete[] _nders[_k];
delete[] _nders;
return true;
}
bool _BasisFunsDers(T** ders, T u, int n)
{
// int _span = FindSpan(u);
int _span;
if (!_findSpan(_span, u)) return false;
_BasisFunsDers(ders, _span, u, n);
return true;
}
bool _BasisFunsDers(T** ders, int span, T u, int n)
{
int _j, _r, _k;
int _s1, _s2;
int _j1, _j2;
int _rk;
int _pk;
T _saved = 0.0;
T _left = 0.0;
T _right = 0.0;
T _temp = 0.0;
T _d = 0.0;
// to store the basis functions and knot differences
T** _ndu = new T*[DEGREE + 1];
for (_j = 0; _j <= DEGREE; ++_j) _ndu[_j] = new T[DEGREE + 1];
// to store (in an alternating fashion) the two most recently computed
// rows a(k,j) and a(k-1,j)
T** _a = new T*[2];
for (_j = 0; _j < 2; ++_j) _a[_j] = new T[DEGREE + 1];
_ndu[0][0] = 1.0;
for (_j = 1; _j <= DEGREE; ++_j) {
_saved = 0.0;
for (_r = 0; _r < _j; ++_r) {
_left = _Left(span, _j - _r, u);
_right = _Right(span, _r + 1, u);
// Lower triangle
_ndu[_j][_r] = _right + _left;
_temp = _ndu[_r][_j - 1] / _ndu[_j][_r];
// Upper triangle
_ndu[_r][_j] = _saved + _right * _temp;
_saved = _left * _temp;
}
_ndu[_j][_j] = _saved;
}
// Load the basis functions
for (_j = 0; _j <= DEGREE; ++_j) ders[0][_j] = _ndu[_j][DEGREE];
// This section computes the derivatives (Eq. [2.9])
for (_r = 0; _r <= DEGREE; ++_r) {
_s1 = 0;
_s2 = 1;
_a[0][0] = 1.0;
// Loop to compute k-th derivative
for (_k = 1; _k <= n; ++_k) {
_d = 0.0;
_rk = _r - _k;
_pk = DEGREE - _k;
if (_r >= _k) {
_a[_s2][0] = _a[_s1][0] / _ndu[_pk + 1][_rk];
_d = _a[_s2][0] * _ndu[_rk][_pk];
}
if (_rk >= -1)
_j1 = 1;
else
_j1 = -_rk;
if (_r - 1 <= _pk)
_j2 = _k - 1;
else
_j2 = DEGREE - _r;
for (_j = _j1; _j <= _j2; ++_j) {
_a[_s2][_j] =
(_a[_s1][_j] - _a[_s1][_j - 1]) / _ndu[_pk + 1][_rk + _j];
_d += _a[_s2][_j] * _ndu[_rk + _j][_pk];
}
if (_r <= _pk) {
_a[_s2][_k] = -_a[_s1][_k - 1] / _ndu[_pk + 1][_r];
_d += _a[_s2][_k] * _ndu[_r][_pk];
}
ders[_k][_r] = _d;
// Switch rows
_j = _s1;
_s1 = _s2;
_s2 = _j;
}
}
// Multiply through by the correct factors
// (Eq. [2.9])
_r = DEGREE;
for (_k = 1; _k <= n; ++_k) {
for (_j = 0; _j <= DEGREE; ++_j) ders[_k][_j] *= _r;
_r *= (DEGREE - _k);
}
// Deallocate
for (_j = 0; _j <= DEGREE; ++_j) delete[] _ndu[_j];
delete[] _ndu;
for (_j = 0; _j < 2; ++_j) delete[] _a[_j];
delete[] _a;
return true;
}
void _BasisFuns(T* N, T u)
{
// Original
/*int _span = FindSpan(u);
BasisFuns(N, _span, u);*/
int _span;
if (_findSpan(_span, u)) {
_BasisFuns(N, _span, u);
}
}
void _BasisFuns(T* N, int span, T u)
{
int _j, _r;
T _left = 0.0;
T _right = 0.0;
T _saved = 0.0;
T _temp = 0.0;
N[0] = 1.0;
for (_j = 1; _j <= DEGREE; ++_j) {
_saved = 0.0;
for (_r = 0; _r < _j; ++_r) {
_left = _Left(span, _j - _r, u);
_right = _Right(span, _r + 1, u);
if ((_right + _left) != 0) {
_temp = N[_r] / (_right + _left);
}
N[_r] = _saved + _right * _temp;
_saved = _left * _temp;
}
N[_j] = _saved;
}
}
inline T _Left(int i, int j, T u)
{ return u - Knots_[i + 1 - j]; }
inline T _Right(int i, int j, T u)
{ return Knots_[i + j] - u; }
bool _findSpan(int& ret, T u)
{
if (u < Knots_[0] || Knots_[NumKnots_ - 1] < u) return false;
if (SP_IS_EQUAL(u, Knots_[NumKnots_ - 1])) {
for (int i(NumKnots_ - 2); i > -1; --i) {
if (Knots_[i] < u && u <= Knots_[i + 1]) {
ret = i;
return true;
}
}
return false;
}
// Binary search
int _low = 0;
int _high = NumKnots_ - 1;
int _mid = (_low + _high) >> 1;
while (u < Knots_[_mid] || u >= Knots_[_mid + 1]) {
if (u < Knots_[_mid])
_high = _mid;
else
_low = _mid;
_mid = (_low + _high) >> 1;
}
ret = _mid;
return true;
}
void _CalcConstrainedCPoints(T* init, T* fin, T Tf)
{
// Position
for (int m(0); m < DIM; ++m) {
CPoints_[0][m] = init[m];
CPoints_[NumCPs_ - 1][m] = fin[m];
}
// Initial Constraints
T** d_mat = new T*[CONST_LEVEL_INI + 1];
for (int i(0); i < CONST_LEVEL_INI + 1; ++i)
d_mat[i] = new T[CONST_LEVEL_INI + 2];
_BasisFunsDers(d_mat, 0., CONST_LEVEL_INI);
T ini_const[DIM];
// Vel, Acc, ...
for (int j(1); j < CONST_LEVEL_INI + 1; ++j) {
for (int k(0); k < DIM; ++k) {
ini_const[k] = init[j * DIM + k];
for (int h(j); h > 0; --h) {
ini_const[k] -= d_mat[j][h - 1] * CPoints_[h - 1][k];
}
CPoints_[j][k] = ini_const[k] / d_mat[j][j];
}
}
for (int p(0); p < CONST_LEVEL_INI + 1; ++p) delete[] d_mat[p];
SP_SAFE_DELETE_AR(d_mat);
// Final Constraints
T** c_mat = new T*[CONST_LEVEL_FIN + 1];
for (int i(0); i < CONST_LEVEL_FIN + 1; ++i)
c_mat[i] = new T[CONST_LEVEL_FIN + 2];
_BasisFunsDers(c_mat, Tf, CONST_LEVEL_FIN);
// Vel, Acc, ...
int idx(1);
for (int j(NumCPs_ - 2); j > NumCPs_ - 2 - CONST_LEVEL_FIN; --j) {
for (int k(0); k < DIM; ++k) {
ini_const[k] = fin[idx * DIM + k];
for (int h(idx); h > 0; --h) {
ini_const[k] -=
c_mat[idx][CONST_LEVEL_FIN + 2 - h] * CPoints_[NumCPs_ - h][k];
}
CPoints_[j][k] = ini_const[k] / c_mat[idx][CONST_LEVEL_FIN + 1 - idx];
}
++idx;
}
for (int p(0); p < CONST_LEVEL_FIN + 1; ++p) delete[] c_mat[p];
SP_SAFE_DELETE_AR(c_mat);
}
void _PrintCP(int i)
{
printf("%i th CP check:\n", i);
for (int m(0); m < DIM; ++m) {
for (int j(0); j < NumCPs_; ++j) {
printf("%f \t", CPoints_[j][m]);
}
printf("\n");
}
printf("\n");
}
void _CalcCPoints(T** middle_pt)
{
for (int i(0); i < NUM_MIDDLE; ++i) {
for (int m(0); m < DIM; ++m) {
CPoints_[CONST_LEVEL_INI + 1 + i][m] = middle_pt[i][m];
}
}
}
T fin_time_;
int NumKnots_;
int NumCPs_;
T Knots_[DEGREE + NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN + 1];
T CPoints_[NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN][DIM];
};
#endif
总结
（1）第一步：给定并设置B样条轨迹起始点、中间点、落脚点，目标时间点四个参数，即可以通过库Basic Bspline插值规划得到连续的足端位置轨迹、速度轨迹、加速度（力矩）轨迹【防盗标记–盒子君hzj】
（2）第二步：仅仅把足端位置轨迹发送给腿部控制器，这就完成轨迹规划的任务了，至于腿部控制器怎么把足端位置轨迹分解到每个关节的转角我再后面的博客再进行论述【防盗标记–盒子君hzj】
.
.
参考资料
如果想看懂库Basic Bspline插值规划原理，可以看看我的这篇博客
【路径生成–插值拟合方法】B样条曲线
类似资料：