只是样例

一、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$，速度 $\dot{p}$，以及机身坐标系下的旋转角 $\omega$,均为 $3\times 1$矩阵

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

• 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$表示世界坐标系下，下标 $_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}$

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]$

3、足端位置

输入为 $12\times1$,输出为 $3\times4$

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}$

输出：

$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}$

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}$

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

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

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}$

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