-
Notifications
You must be signed in to change notification settings - Fork 2
/
Tello.hpp
141 lines (100 loc) · 6.46 KB
/
Tello.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
#ifndef GRBDA_ROBOTS_TELLO_H
#define GRBDA_ROBOTS_TELLO_H
#include "grbda/Robots/Robot.h"
namespace grbda
{
class Tello : public Robot<double>
{
public:
Tello() {}
ClusterTreeModel<> buildClusterTreeModel() const override;
protected:
const double grav = -9.81;
const Mat3<double> R_down = (Mat3<double>() << 1., 0., 0., 0., -1., 0., 0., 0., -1.).finished();
const Mat3<double> R_left = (Mat3<double>() << -1., 0., 0., 0., 0., 1., 0., 1., 0.).finished();
const Mat3<double> R_right = (Mat3<double>() << 1., 0., 0., 0., 0., 1., 0., -1., 0.).finished();
// Left leg
const Mat3<double> R_left_hip_clamp = Mat3<double>::Identity();
const Vec3<double> p_left_hip_clamp = Vec3<double>{0., 126e-3, -87e-3};
const Mat3<double> R_left_gimbal = Mat3<double>::Identity();
const Vec3<double> p_left_gimbal = Vec3<double>{0., 0., -142.5e-3};
const Mat3<double> R_left_thigh = Mat3<double>::Identity();
const Vec3<double> p_left_thigh = Vec3<double>::Zero();
const Mat3<double> R_left_shin = Mat3<double>::Identity();
const Vec3<double> p_left_shin = Vec3<double>{0., 0., -226.8e-3};
const Mat3<double> R_left_foot = Mat3<double>::Identity();
const Vec3<double> p_left_foot = Vec3<double>{0., 0., -260e-3};
const Mat3<double> R_left_hip_clamp_rotor = R_down;
const Vec3<double> p_left_hip_clamp_rotor = Vec3<double>{0., 126e-3, -26e-3};
const Mat3<double> R_left_hip_rotor_1 = R_left;
const Vec3<double> p_left_hip_rotor_1 = Vec3<double>{0., 0.04, 0.};
const Mat3<double> R_left_hip_rotor_2 = R_right;
const Vec3<double> p_left_hip_rotor_2 = Vec3<double>{0., -0.04, 0.};
const Mat3<double> R_left_knee_ankle_rotor_1 = R_right;
const Vec3<double> p_left_knee_ankle_rotor_1 = Vec3<double>{0., 26.55e-3, 0.};
const Mat3<double> R_left_knee_ankle_rotor_2 = R_left;
const Vec3<double> p_left_knee_ankle_rotor_2 = Vec3<double>{0., -26.55e-3, 0.};
// Right leg
const Mat3<double> R_right_hip_clamp = Mat3<double>::Identity();
const Vec3<double> p_right_hip_clamp = Vec3<double>{0., -126e-3, -87e-3};
const Mat3<double> R_right_gimbal = Mat3<double>::Identity();
const Vec3<double> p_right_gimbal = Vec3<double>{0., 0., -142.5e-3};
const Mat3<double> R_right_thigh = Mat3<double>::Identity();
const Vec3<double> p_right_thigh = Vec3<double>::Zero();
const Mat3<double> R_right_shin = Mat3<double>::Identity();
const Vec3<double> p_right_shin = Vec3<double>{0., 0., -226.8e-3};
const Mat3<double> R_right_foot = Mat3<double>::Identity();
const Vec3<double> p_right_foot = Vec3<double>{0., 0., -260e-3};
const Mat3<double> R_right_hip_clamp_rotor = R_down;
const Vec3<double> p_right_hip_clamp_rotor = Vec3<double>{0., -126e-3, -26e-3};
const Mat3<double> R_right_hip_rotor_1 = R_left;
const Vec3<double> p_right_hip_rotor_1 = Vec3<double>{0., 0.04, 0.};
const Mat3<double> R_right_hip_rotor_2 = R_right;
const Vec3<double> p_right_hip_rotor_2 = Vec3<double>{0., -0.04, 0.};
const Mat3<double> R_right_knee_ankle_rotor_1 = R_right;
const Vec3<double> p_right_knee_ankle_rotor_1 = Vec3<double>{0., 26.55e-3, 0.};
const Mat3<double> R_right_knee_ankle_rotor_2 = R_left;
const Vec3<double> p_right_knee_ankle_rotor_2 = Vec3<double>{0., -26.55e-3, 0.};
const double torso_mass = 2.3008;
const Vec3<double> torso_CoM = Vec3<double>{0.0073, -0.0013, -0.0023};
const Mat3<double> torso_inertia = (Mat3<double>() << 0.0366, 0., -0.0006, 0., 0.0142, -0.0002, -0.0006, -0.0002, 0.0291).finished();
const double gear_ratio = 6.0;
const double hip_clamp_mass = 1.3289;
const Vec3<double> hip_clamp_CoM = Vec3<double>{-0.0010, 0., -0.0069};
const Mat3<double> hip_clamp_inertia = (Mat3<double>() << 0.0032, 0., 0.0001, 0., 0.0033, 0., 0.0001, 0., 0.0027).finished();
const double gimbal_mass = 0.4433;
const Vec3<double> gimbal_CoM = Vec3<double>{-0.0027, 0., 0.0258};
const Mat3<double> gimbal_inertia = (Mat3<double>() << 0.0018, 0., 0., 0., 0.0017, 0., 0., 0., 0.0015).finished();
const double thigh_mass = 1.5424;
const Vec3<double> thigh_CoM = Vec3<double>{0.003, -0.0001, -0.0323};
const Mat3<double> thigh_inertia = (Mat3<double>() << 0.0103, 0., -0.0005, 0., 0.0097, 0., -0.0005, 0., 0.0027).finished();
const double shin_mass = 0.3072;
const Vec3<double> shin_CoM = Vec3<double>{0.0047, -0.0003, -0.1043};
const Mat3<double> shin_inertia = (Mat3<double>() << 0.0054 , -0., -0.0002, -0., 0.0054, 0., -0.0002, 0., 0.0001).finished();
const double foot_mass = 0.1025;
const Vec3<double> foot_CoM = Vec3<double>{0.0042, -0., -0.0251};
const Mat3<double> foot_inertia = (Mat3<double>() << 0.094e-3, -0., -0.0038e-3, -0., 0.1773e-3, 0., -0.0038e-3, 0., 0.0901e-3).finished();
const double rotor_mass = 0.07;
const Vec3<double> rotor_CoM = Vec3<double>::Zero();
const Mat3<double> rotor_inertia = (Mat3<double>() << 2.5984e-5, 0., 0., 0., 2.5984e-5, 0., 0., 0., 5.1512e-5).finished();
const double hip_clamp_rotor_mass = rotor_mass;
const Vec3<double> hip_clamp_rotor_CoM = rotor_CoM;
const Mat3<double> hip_clamp_rotor_inertia = rotor_inertia;
const double hip_rotor_1_mass = rotor_mass;
const Vec3<double> hip_rotor_1_CoM = rotor_CoM;
const Mat3<double> hip_rotor_1_inertia = rotor_inertia;
const double hip_rotor_2_mass = rotor_mass;
const Vec3<double> hip_rotor_2_CoM = rotor_CoM;
const Mat3<double> hip_rotor_2_inertia = rotor_inertia;
const double knee_ankle_rotor_1_mass = rotor_mass;
const Vec3<double> knee_ankle_rotor_1_CoM = rotor_CoM;
const Mat3<double> knee_ankle_rotor_1_inertia = rotor_inertia;
const double knee_ankle_rotor_2_mass = rotor_mass;
const Vec3<double> knee_ankle_rotor_2_CoM = rotor_CoM;
const Mat3<double> knee_ankle_rotor_2_inertia = rotor_inertia;
const double _footToeLength = 0.1;
const double _footHeelLength = 0.05;
const double _footHeight = 0.041;
};
} // namespace grbda
#endif // GRBDA_ROBOTS_TELLO_H