只是样例

一、RobotState.h文件

核心代码：

class RobotState { public: void set(flt* p, flt* v, flt* q, flt* w, flt* r, flt yaw); //void compute_rotations(); void print();         Matrix<fpt,3,1> p,v,w;         Matrix<fpt,3,4> r_feet;         Matrix<fpt,3,3> R;         Matrix<fpt,3,3> R_yaw;         Matrix<fpt,3,3> I_body;         Quaternionf q;         fpt yaw;         fpt m = 9; //fpt m = 50.236; //DH //private: }; 

• Matrix<fpt,3,1> p,v,w：定义了机器人的在世界坐标系下的，位置
  $p$

  p，速度
  $dot{p}$

  p˙​，以及机身坐标系下的旋转角
  $omega$

  ω,均为
  $3times 1$

  3×1矩阵

• Matrix<fpt,3,4> r_feet：机身参考系下的足端位置，
  $3times4$

  3×4矩阵

• Matrix<fpt,3,3> R：机身坐标系到世界坐标系的旋转矩阵

• Matrix<fpt,3,3> R_yaw;：偏航角旋转矩阵

• Matrix<fpt,3,3> I_body：机身坐标系下的惯量矩阵

• Matrix<fpt,3,3> I_body：四元素表示的世界坐标系下的旋转

• fpt yaw：偏航角

• fpt m = 9：机器人质量

二、RobotState.cpp文件

RobotState类的完整实现，set函数的输入为当前（或上一）时刻的状态数据，包括位置，速度，旋转角，足端位置，偏航角以及四元数表示的旋转量，具体定义参考上一节。

void RobotState::set(flt* p_, flt* v_, flt* q_, flt* w_, flt* r_,flt yaw_) { //位置，速度，旋转角 for(u8 i = 0; i < 3; i++) { this->p(i) = p_[i]; this->v(i) = v_[i]; this->w(i) = w_[i]; } //四元数 this->q.w() = q_[0]; this->q.x() = q_[1]; this->q.y() = q_[2]; this->q.z() = q_[3]; //偏航角 this->yaw = yaw_; //足端位置 //for(u8 i = 0; i < 12; i++) //    this->r_feet(i) = r[i]; for(u8 rs = 0; rs < 3; rs++) for(u8 c = 0; c < 4; c++) this->r_feet(rs,c) = r_[rs*4 + c]; //旋转矩阵     R = this->q.toRotationMatrix();     fpt yc = cos(yaw_);     fpt ys = sin(yaw_);      R_yaw <<  yc, -ys, 0,              ys,  yc, 0, 0, 0, 1; //惯量矩阵     Matrix<fpt,3,1> Id;     Id << .07f, 0.26f, 0.242f; //Id << 0.3f, 2.1f, 2.1f; // DH     I_body.diagonal() = Id; //TODO: Consider normalizing quaternion?? } 

1、位置，速度，旋转角

这里直接对这三个量赋值：

for(u8 i = 0; i < 3; i++) { this->p(i) = p_[i]; this->v(i) = v_[i]; this->w(i) = w_[i]; } 

以下统一表示为：
下标
$_G$

G​表示世界坐标系下，下标
$_B$

B​表示机身坐标系下

具体形式如下：

$begin{matrix} p = begin{bmatrix} x_G & y_G & z_G end{bmatrix}\ \ v = begin{bmatrix} dot{x_G} & dot{y_G} & dot{z_G} end{bmatrix}\ \ omega = begin{bmatrix} dot{phi_B} & dot{theta_B} & dot{psi_B} end{bmatrix}end{matrix}$

p=[xG​​yG​​zG​​]v=[xG​˙​​yG​˙​​zG​˙​​]ω=[ϕB​˙​​θB​˙​​ψB​˙​​]​

2、四元数

this->q.w() = q_[0]; this->q.x() = q_[1]; this->q.y() = q_[2]; this->q.z() = q_[3]; 

注意这里的xyz不是坐标，而是四元数的固定表示

具体形式：

$q = [w, x, y, z]$

q=[w,x,y,z]

3、足端位置

输入为
$12times1$

12×1,输出为
$3times4$

3×4

for(u8 rs = 0; rs < 3; rs++) for(u8 c = 0; c < 4; c++) this->r_feet(rs,c) = r_[rs*4 + c]; 

具体形式：

输入：
$begin{bmatrix} x_1 & x_2 & x_3 & x_4 |& y_1 & y_2 & y_3 & y_4| & z_1 & z_2 & z_3 & z_4 end{bmatrix}$

[x1​​x2​​x3​​x4​∣​y1​​y2​​y3​​y4​∣​z1​​z2​​z3​​z4​​]

输出：

$r_{feet} = begin{bmatrix} x_1 & x_2 & x_3 & x_4\ y_1 & y_2 & y_3 & y_4\ z_1 & z_2 & z_3 & z_4 end{bmatrix}$

rfeet​=⎣⎡​x1​y1​z1​​x2​y2​z2​​x3​y3​z3​​x4​y4​z4​​⎦⎤​

4、旋转矩阵

R = this->q.toRotationMatrix();     fpt yc = cos(yaw_);     fpt ys = sin(yaw_);  R_yaw <<  yc, -ys, 0,           ys,  yc, 0, 0, 0, 1; 

这里有两个旋转矩阵

a、机身坐标系到世界坐标系的旋转矩阵

利用了eigen的toRotationMatrix()，通过四元数求出旋转矩阵。具体转换公式如下：

$begin{bmatrix} 1-2y^2 – 2z^2 & 2xy+2wz & 2xz-2wy\ 2xy -2wz & 1-2x^2-2z^2 & 2yz+2wx\ 2xz + 2wy & 2yz-2wx & 1-2x^2-2y^2 end{bmatrix}$

⎣⎡​1−2y2−2z22xy−2wz2xz+2wy​2xy+2wz1−2x2−2z22yz−2wx​2xz−2wy2yz+2wx1−2x2−2y2​⎦⎤​

b、偏航角组成的旋转矩阵

$R_{yaw} = begin{bmatrix} cos(psi_B) & -sin(psi_B) & 0\ sin(psi_B) & cos(psi_B) & 0\ 0 & 0 & 0 end{bmatrix}$

Ryaw​=⎣⎡​cos(ψB​)sin(ψB​)0​−sin(ψB​)cos(ψB​)0​000​⎦⎤​

5、惯性矩阵

这里通过对角矩阵创建

Matrix<fpt,3,1> Id; Id << .07f, 0.26f, 0.242f; //Id << 0.3f, 2.1f, 2.1f; // DH I_body.diagonal() = Id; 

最终形式：

$I_B = begin{bmatrix} 0.07 & 0 & 0\ 0 & 0.26 & 0\ 0 & 0 & 0.242 end{bmatrix}$

IB​=⎣⎡​0.0700​00.260​000.242​⎦⎤​

如果觉得ok，点个赞，点个关注，也欢迎给个打赏支持一下编者的工作