【Cheetah仿真環境】源碼解析--robotstate

只是樣例，更多詳情請私信

一、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，點個贊，點個關注，也歡迎給個打賞支持一下編者的工作