-
Notifications
You must be signed in to change notification settings - Fork 25
/
Copy pathLegController
109 lines (97 loc) · 6.39 KB
/
LegController
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
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
>>>>>>> template <typename T>
void LegControllerCommand<T>::zero()
函数功能：将结构体LegControllerCommand中所有的参数置零
向量类型参数：tauFeedForward：输出的力矩（3*1）
forceFeedForward：输出的力（3*1）
qDes：期望的关节位置（3*1）
qdDes：期望的关节速度（3*1）
pDes：期望的足端位置（3*1）
vDes：期望的足端速度（3*1）
矩阵类型参数：kpCartesian：笛卡尔空间比例系数，用来对足端行为进行控制（3*3）
kdCartesian：笛卡尔空间微分系数，用来对足端行为进行控制（3*3）
kpJoint：关节空间比例系数，用来对关节行为进行控制（3*3）
kdJoint：关节空间微分系数，用来对关节行为进行控制（3*3）
>>>>>>> template <typename T>
void LegControllerData<T>::zero()
函数功能：将结构体LegControllerData中所有的参数置零
向量类型参数：q：实际的关节位置（3*1）
qd：实际的关节速度（3*1）
p：实际的足端位置（3*1）
v：实际的足端速度（3*1）
tauEstimate：关节力矩的估计值（3*1）
矩阵类型参数：J：Jacobian矩阵（3*3）
>>>>>>> template <typename T>
void LegController<T>::zeroCommand()
函数功能：commands为定义的4个元素的LegControllerCommand结构体数组
for循环遍历调用4个元素中的zero()函数
将_legsEnabled变量置为false
>>>>>>> template <typename T>
void LegController<T>::edampCommand(RobotType robot, T gain)
函数功能：根据输入参数robot是Cheetah 3还是mini-cheetah，对微分系数矩阵kdCartesian或kdJoint使用输入参数gain进行对角赋值
>>>>>>> template <typename T>
void LegController<T>::updateData(const SpiData* spiData)
函数功能：datas为定义的4个元素的LegControllerData结构体数组
使用输入参数spiData中的成员对datas中的q和qd成员赋值
调用函数computeLegJacobianAndPosition，计算四条腿各自的Jacobian矩阵以及足端位置坐标
通过正运动学方法计算足端的速度
>>>>>>> template <typename T>
void LegController<T>::updateData(const TiBoardData* tiBoardData)
函数功能：datas为定义的4个元素的LegControllerData结构体数组
使用输入参数tiBoardData中的成员对datas中的q、qd、p和v成员赋值
调用函数computeLegJacobianAndPosition，计算四条腿各自的Jacobian矩阵以及足端位置坐标
使用输入参数tiBoardData中的关节力矩成员tau对datas中的tauEstimate成员赋值
>>>>>>> template <typename T>
void LegController<T>::updateCommand(SpiCommand* spiCommand)
函数功能：用commands中的成员更新spiCommand中的成员
>>>>>>> template <typename T>
void LegController<T>::updateCommand(TiBoardCommand* tiBoardCommand)
函数功能：用commands中的成员更新tiBoardCommand中的成员
>>>>>>> template<typename T>
void LegController<T>::setLcm(leg_control_data_lcmt *lcmData, leg_control_command_lcmt *lcmCommand)
函数功能：更新lcmData和lcmCommand中的成员，本质上就是将LegControllerCommand结构体和LegControllerData结构体中的成员分类存储到lcmData结构体和lcmCommand结构体中的对应成员中
>>>>>>> template <typename T>
void computeLegJacobianAndPosition(Quadruped<T>& quad, Vec3<T>& q, Mat3<T>* J, Vec3<T>* p, int leg)
函数功能：将输入参数quad的abadLink长度、hipLink长度、kneeLink长度以及补偿量kneeLinkY分别存储在l1、l2、l3以及l4中
标志位sideSign用以区分左侧的腿和右侧的腿（左正右负），因为设计到腿部正运动学的足端在y方向上的坐标
J为Jacobian矩阵，联系关节空间和足端笛卡尔空间
p为足端在笛卡尔坐标系下的位置坐标，已知连杆长度和关节角度，通过正运动学求解得到足端位置坐标
| l3 * sin(2 + 3) + l2 * sin(2) |
p = | (l1 + l4) * sideSign * cos(1) + [l3 * cos(2 + 3) + l2 * cos(2)] * sin(1) |
| (l1 + l4) * sideSign * sin(1) - [l3 * cos(2 + 3) + l2 * cos(2)] * cos(1) |
有关参考方向：以左前腿为例，从右前腿的位置看去：
对于髋关节，逆时针方向为正方向
对于膝关节，逆时针方向为正方向
从机体头部看去：
对于肩关节，逆时针方向为正方向
###############################################################################################################################
总结：
主要的参数成员：
LegControllerCommand
|___Vec3
| |___tauFeedForward,
| |___forceFeedForward
| |___qDes
| |___qdDes
| |___pDes
| |___vDes
|
|___Mat3
|___kpCartesian
|___kdCartesian
|___kpJoint
|___kdJoint
LegControllerData
|___Vec3
| |___q
| |___qd
| |___p
| |___v
| |___tauEstimate
|
|___Mat3
| |___J
|
|___Quadruped
|___quadruped
此工程主要完成了机器人腿部状态量的数据处理和计算，构建Jacobian矩阵完成正运动学和逆运动学的计算，通过LCM完成数据的打包和通信
END