-
Notifications
You must be signed in to change notification settings - Fork 22
/
Quadruped
65 lines (51 loc) · 3.23 KB
/
Quadruped
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
参数:bodyId:机体-----5
>>>>>>> template <typename T>
bool Quadruped<T>::buildModel(FloatingBaseModel<T>& model)
函数功能:输入参数model为一个FloatingBaseModel类的对象
创建向量bodyDims (3×1),存储机体的长_bodyLength、宽_bodyWidth和高_bodyHeight
调用函数addBase(_bodyInertia),添加机体相关属性
调用函数addGroundContactBoxPoints(5, bodyDims)
>>>>>>> template <typename T>
void FloatingBaseModel<T>::addBase(const SpatialInertia<T> &inertia)
函数功能:eye6 单位阵 (6×6)
zero6 零矩阵 (6×6)
SpatialInertia<T>类的对象zeroInertia实例化,zeroInertia->_inertia = zero6
_nDof 赋值为6,表示机体有六个空间自由度
使用push_back()函数,循环六次 (六个自由度???)向定义的九个向量中压入值:
_parents向量:存储父元件,六个元素均为0
_gearRatios向量:存储传动比,前五个元素均为0,最后一个元素为1
_jointTypes向量:存储关节类型,前五个元素均为Nothing,最后一个元素为FloatingBase。
枚举类型JointType:Prismatic滑动副,Revolute转动副,FloatingBase机体,Nothing不是运动副
_jointAxes向量:存储关节轴线,六个元素均为X。
枚举类型CoordinateAxis:X,Y,Z
_Xtree向量:
_Ibody向量:前五个元素均为zeroInertia,最后一个元素为inertia (addBase函数输入参数)
_Xrot向量:
_Irot向量:
_bodyNames向量:向量中的前五个元素均为N/A,最后一个元素为FloatingBase
调用addDynamicsVars(6)函数,完成动力学变量的配置
>>>>>>> template <typename T>
void FloatingBaseModel<T>::addDynamicsVars(int count)
函数功能:配置动力学变量
>>>>>>> template <typename T>
void FloatingBaseModel<T>::addGroundContactBoxPoints(int bodyId, const Vec3<T> &dims)
函数功能:将机体视作长方体的盒子(box),此函数完成box八个角点在以机体中心点为原点的坐标系框架内的坐标的计算
调用addGroundContactPoint()函数
>>>>>>> template <typename T>
int FloatingBaseModel<T>::addGroundContactPoint(int bodyID, const Vec3<T> &location, bool isFoot)
函数功能:
>>>>>>> template <typename T, typename T2>
Vec3<T> withLegSigns(const Eigen::MatrixBase<T2>& v, int legID)
函数功能:输入参数v是机械结构相对于机器人机体中心点在x、y和z三个方向上的距离,根据此机械结构所属的腿不同,将其转换为相对于机体中心点为原点的位置坐标输出。
>>>>>>> template <typename T>
Mat3<typename T::Scalar> vectorToSkewMat(const Eigen::MatrixBase<T>& v)
函数功能:输入三维向量v,输出v所对应的反对称矩阵
参考:https://blog.csdn.net/u012200261/article/details/56494754/
>>>>>>> template <typename T, typename T2>
auto createSXform(const Eigen::MatrixBase<T>& R,const Eigen::MatrixBase<T2>& r)
函数功能:输出矩阵X (6×6)
X = | R 0 |
| -R*vectorToSkewMat(r) R |
>>>>>>> template <typename T>
Mat3<T> coordinateRotation(CoordinateAxis axis, T theta)
函数功能:根据枚举类型的输入参数axis(X/Y/Z)以及旋转角theta,输出旋转矩阵R(3×3)