-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathTelloLeg.hpp
179 lines (159 loc) · 10.2 KB
/
TelloLeg.hpp
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
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
#ifndef GRBDA_ROBOTS_TELLO_LEG_H
#define GRBDA_ROBOTS_TELLO_LEG_H
#include "grbda/Robots/Tello.hpp"
namespace grbda
{
class TelloLeg : public Tello
{
public:
ClusterTreeModel<> buildClusterTreeModel() const override
{
using namespace ClusterJoints;
typedef spatial::Transform<> Xform;
ClusterTreeModel<> model{};
// Hip clamp
const std::string hip_clamp_parent_name = "ground";
const std::string hip_clamp_rotor_parent_name = "ground";
const Mat3<double> R_hip_clamp = R_right_hip_clamp;
const Vec3<double> p_hip_clamp = p_right_hip_clamp;
const Xform hip_clamp_Xtree = spatial::Transform(R_hip_clamp, p_hip_clamp);
const std::string hip_clamp_name = "hip-clamp";
const SpatialInertia<double> hip_clamp_spatial_inertia =
SpatialInertia<double>{hip_clamp_mass, hip_clamp_CoM, hip_clamp_inertia};
auto hip_clamp = model.registerBody(hip_clamp_name, hip_clamp_spatial_inertia,
hip_clamp_parent_name, hip_clamp_Xtree);
// Hip clamp rotor
const Mat3<double> R_hip_clamp_rotor = R_right_hip_clamp_rotor;
const Vec3<double> p_hip_clamp_rotor = p_right_hip_clamp_rotor;
const Xform hip_clamp_rotor_Xtree = spatial::Transform(R_hip_clamp_rotor,
p_hip_clamp_rotor);
const std::string hip_clamp_rotor_name = "hip-clamp-rotor";
const SpatialInertia<double> hip_clamp_rotor_spatial_inertia =
SpatialInertia<double>{hip_clamp_rotor_mass, hip_clamp_rotor_CoM,
hip_clamp_rotor_inertia};
auto hip_clamp_rotor = model.registerBody(hip_clamp_rotor_name,
hip_clamp_rotor_spatial_inertia,
hip_clamp_rotor_parent_name,
hip_clamp_rotor_Xtree);
// Hip clamp cluster
const std::string hip_clamp_cluster_name = "hip-clamp";
GearedTransmissionModule<> hip_clamp_module{hip_clamp, hip_clamp_rotor,
ori::CoordinateAxis::Z,
ori::CoordinateAxis::Z,
gear_ratio};
model.appendRegisteredBodiesAsCluster<RevoluteWithRotor<>>(hip_clamp_cluster_name,
hip_clamp_module);
// Hip differential rotor 1
const Mat3<double> R_hip_rotor_1 = R_right_hip_rotor_1;
const Vec3<double> p_hip_rotor_1 = p_right_hip_rotor_1;
const Xform hip_rotor_1_Xtree = spatial::Transform(R_hip_rotor_1, p_hip_rotor_1);
const std::string hip_rotor_1_name = "hip-rotor-1";
const std::string hip_rotor_1_parent_name = "hip-clamp";
const SpatialInertia<double> hip_rotor_1_spatial_inertia =
SpatialInertia<double>{hip_rotor_1_mass, hip_rotor_1_CoM, hip_rotor_1_inertia};
auto hip_rotor_1 = model.registerBody(hip_rotor_1_name, hip_rotor_1_spatial_inertia,
hip_rotor_1_parent_name, hip_rotor_1_Xtree);
// Hip differential rotor 2
const Mat3<double> R_hip_rotor_2 = R_right_hip_rotor_2;
const Vec3<double> p_hip_rotor_2 = p_right_hip_rotor_2;
const Xform hip_rotor_2_Xtree = spatial::Transform(R_hip_rotor_2, p_hip_rotor_2);
const std::string hip_rotor_2_name = "hip-rotor-2";
const std::string hip_rotor_2_parent_name = "hip-clamp";
const SpatialInertia<double> hip_rotor_2_spatial_inertia =
SpatialInertia<double>{hip_rotor_2_mass, hip_rotor_2_CoM, hip_rotor_2_inertia};
auto hip_rotor_2 = model.registerBody(hip_rotor_2_name, hip_rotor_2_spatial_inertia,
hip_rotor_2_parent_name, hip_rotor_2_Xtree);
// Gimbal
const Mat3<double> R_gimbal = R_right_gimbal;
const Vec3<double> p_gimbal = p_right_gimbal;
const Xform gimbal_Xtree = spatial::Transform(R_gimbal, p_gimbal);
const std::string gimbal_name = "gimbal";
const std::string gimbal_parent_name = "hip-clamp";
const SpatialInertia<double> gimbal_spatial_inertia =
SpatialInertia<double>{gimbal_mass, gimbal_CoM, gimbal_inertia};
auto gimbal = model.registerBody(gimbal_name, gimbal_spatial_inertia,
gimbal_parent_name, gimbal_Xtree);
//
const Mat3<double> R_thigh = R_right_thigh;
const Vec3<double> p_thigh = p_right_thigh;
const Xform thigh_Xtree = spatial::Transform(R_thigh, p_thigh);
const std::string thigh_name = "thigh";
const std::string thigh_parent_name = "gimbal";
const SpatialInertia<double> thigh_spatial_inertia =
SpatialInertia<double>{thigh_mass, thigh_CoM, thigh_inertia};
auto thigh = model.registerBody(thigh_name, thigh_spatial_inertia,
thigh_parent_name, thigh_Xtree);
// Hip differential cluster
const std::string hip_differential_cluster_name = "hip-differential";
TelloDifferentialModule<> hip_differential_module{
hip_rotor_1, hip_rotor_2, gimbal, thigh,
ori::CoordinateAxis::Z, ori::CoordinateAxis::Z,
ori::CoordinateAxis::X, ori::CoordinateAxis::Y, gear_ratio};
model.appendRegisteredBodiesAsCluster<TelloHipDifferential<double>>(
hip_differential_cluster_name, hip_differential_module);
// Knee-ankle differential rotor 1
const Mat3<double> R_knee_ankle_rotor_1 = R_right_knee_ankle_rotor_1;
const Vec3<double> p_knee_ankle_rotor_1 = p_right_knee_ankle_rotor_1;
const Xform knee_ankle_rotor_1_Xtree =
spatial::Transform(R_knee_ankle_rotor_1, p_knee_ankle_rotor_1);
const std::string knee_ankle_rotor_1_name = "knee-ankle-rotor-1";
const std::string knee_ankle_rotor_1_parent_name = "thigh";
const SpatialInertia<double> knee_ankle_rotor_1_spatial_inertia = SpatialInertia<double>{
knee_ankle_rotor_1_mass, knee_ankle_rotor_1_CoM, knee_ankle_rotor_1_inertia};
auto knee_ankle_rotor_1 = model.registerBody(knee_ankle_rotor_1_name,
knee_ankle_rotor_1_spatial_inertia,
knee_ankle_rotor_1_parent_name,
knee_ankle_rotor_1_Xtree);
// Knee-ankle differential rotor 2
const Mat3<double> R_knee_ankle_rotor_2 = R_right_knee_ankle_rotor_2;
const Vec3<double> p_knee_ankle_rotor_2 = p_right_knee_ankle_rotor_2;
const Xform knee_ankle_rotor_2_Xtree =
spatial::Transform(R_knee_ankle_rotor_2, p_knee_ankle_rotor_2);
const std::string knee_ankle_rotor_2_name = "knee-ankle-rotor-2";
const std::string knee_ankle_rotor_2_parent_name = "thigh";
const SpatialInertia<double> knee_ankle_rotor_2_spatial_inertia = SpatialInertia<double>{
knee_ankle_rotor_2_mass, knee_ankle_rotor_2_CoM, knee_ankle_rotor_2_inertia};
auto knee_ankle_rotor_2 = model.registerBody(knee_ankle_rotor_2_name,
knee_ankle_rotor_2_spatial_inertia,
knee_ankle_rotor_2_parent_name,
knee_ankle_rotor_2_Xtree);
// Shin
const Mat3<double> R_shin = R_right_shin;
const Vec3<double> p_shin = p_right_shin;
const Xform shin_Xtree = spatial::Transform(R_shin, p_shin);
const std::string shin_name = "shin";
const std::string shin_parent_name = "thigh";
const SpatialInertia<double> shin_spatial_inertia =
SpatialInertia<double>{shin_mass, shin_CoM, shin_inertia};
auto shin = model.registerBody(shin_name, shin_spatial_inertia,
shin_parent_name, shin_Xtree);
// Foot
const Mat3<double> R_foot = R_right_foot;
const Vec3<double> p_foot = p_right_foot;
const Xform foot_Xtree = spatial::Transform(R_foot, p_foot);
const std::string foot_name = "foot";
const std::string foot_parent_name = "shin";
const SpatialInertia<double> foot_spatial_inertia =
SpatialInertia<double>{foot_mass, foot_CoM, foot_inertia};
auto foot = model.registerBody(foot_name, foot_spatial_inertia,
foot_parent_name, foot_Xtree);
// Knee-ankle differential cluster
const std::string knee_ankle_differential_cluster_name = "knee-ankle-differential";
TelloDifferentialModule<> knee_ankle_module{
knee_ankle_rotor_1, knee_ankle_rotor_2, shin, foot,
ori::CoordinateAxis::Z, ori::CoordinateAxis::Z,
ori::CoordinateAxis::Y, ori::CoordinateAxis::Y, gear_ratio};
model.appendRegisteredBodiesAsCluster<TelloKneeAnkleDifferential<double>>(
knee_ankle_differential_cluster_name, knee_ankle_module);
// Append contact points for the feet
const std::string toe_contact_name = "toe_contact";
const std::string heel_contact_name = "heel_contact";
model.appendContactPoint(foot_name, Vec3<double>(-_footToeLength, 0, -_footHeight),
toe_contact_name);
model.appendContactPoint(foot_name, Vec3<double>(-_footHeelLength, 0, -_footHeight),
heel_contact_name);
return model;
}
};
} // namespace grbda
#endif // GRBDA_ROBOTS_TELLO_LEG_H