【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 p,v,w：定义了机器人的在世界坐标系下的，位置$p$，速度$\dot{p}$，以及机身坐标系下的旋转角$\omega$,均为$3\times 1$矩阵

• Matrix r_feet：机身参考系下的足端位置，$3\times 4$矩阵

• Matrix R：机身坐标系到世界坐标系的旋转矩阵

• Matrix R_yaw;：偏航角旋转矩阵

• Matrix I_body：机身坐标系下的惯量矩阵

• Matrix 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{array}{c}p=\left[\begin{array}{ccc}{x}_G& y_G& z_G\end{array}\right]\\ \\ v=\left[\begin{array}{ccc}\dot{x_G}& \dot{y_G}& \dot{z_G}\end{array}\right]\\ \\ \omega =\left[\begin{array}{ccc}\dot{{\varphi }_B}& \dot{{\theta }_B}& \dot{{\psi }_B}\end{array}\right]\end{array}$$

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\times 1$,输出为$3\times 4$

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

具体形式：

输入：$$\left[\begin{array}{cccccccccccc}{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{array}\right]$$

输出：

$$r_{feet}=\left[\begin{array}{cccc}{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{array}\right]$$

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()，通过四元数求出旋转矩阵。具体转换公式如下：

$$\left[\begin{array}{ccc}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{array}\right]$$

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

$$R_{yaw}=\left[\begin{array}{ccc}cos({\psi }_B)& -sin({\psi }_B)& 0\\ sin({\psi }_B)& cos({\psi }_B)& 0\\ 0& 0& 0\end{array}\right]$$

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=\left[\begin{array}{ccc}0.07& 0& 0\\ 0& 0.26& 0\\ 0& 0& 0.242\end{array}\right]$$

---

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