只是样例
一、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
:定义了机器人的在世界坐标系下的,位置 ,速度 ,以及机身坐标系下的旋转角 ,均为 矩阵 -
Matrix<fpt,3,4> r_feet
:机身参考系下的足端位置, 矩阵 -
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];
}
以下统一表示为:
下标 表示世界坐标系下,下标 表示机身坐标系下
具体形式如下:
2、四元数
this->q.w() = q_[0];
this->q.x() = q_[1];
this->q.y() = q_[2];
this->q.z() = q_[3];
注意这里的xyz不是坐标,而是四元数的固定表示
具体形式:
3、足端位置
输入为 ,输出为
for(u8 rs = 0; rs < 3; rs++)
for(u8 c = 0; c < 4; c++)
this->r_feet(rs,c) = r_[rs*4 + c];
具体形式:
输入:
输出:
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(),通过四元数求出旋转矩阵。具体转换公式如下:
b、偏航角组成的旋转矩阵
5、惯性矩阵
这里通过对角矩阵创建
Matrix<fpt,3,1> Id;
Id << .07f, 0.26f, 0.242f;
//Id << 0.3f, 2.1f, 2.1f; // DH
I_body.diagonal() = Id;
最终形式: