【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\times 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$表示世界坐标系下，下标${}_B$表示机身坐标系下

具体形式如下：
p = [ x G y G z G ] v = [ x G ˙ y G ˙ z G ˙ ]