-
Notifications
You must be signed in to change notification settings - Fork 17
/
IKWalk.hpp
222 lines (212 loc) · 5.21 KB
/
IKWalk.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
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
#ifndef LEPH_IKWALK_HPP
#define LEPH_IKWALK_HPP
#include "HumanoidModel.hpp"
namespace Rhoban {
/**
* Walk parameters
*/
struct IKWalkParameters {
/**
* Model leg typical length between
* each rotation axis
*/
double distHipToKnee;
double distKneeToAnkle;
double distAnkleToGround;
/**
* Distance between the two feet in lateral
* axis while in zero position
*/
double distFeetLateral;
/**
* Complete (two legs) walk cycle frequency
* in Hertz
*/
double freq;
/**
* Global gain multiplying all time
* dependant movement between 0 and 1.
* Control walk enabled/disabled smoothing.
* 0 is walk disabled.
* 1 is walk fully enabled
*/
double enabledGain;
/**
* Length of double support phase
* in phase time
* (between 0 and 1)
* 0 is null double support and full single support
* 1 is full double support and null single support
*/
double supportPhaseRatio;
/**
* Lateral offset on default foot
* position in meters (foot lateral distance)
* 0 is default
* > 0 is both feet external offset
*/
double footYOffset;
/**
* Forward length of each foot step
* in meters
* >0 goes forward
* <0 goes backward
* (dynamic parameter)
*/
double stepGain;
/**
* Vertical rise height of each foot
* in meters (positive)
*/
double riseGain;
/**
* Angular yaw rotation of each
* foot for each step in radian.
* 0 does not turn
* >0 turns left
* <0 turns right
* (dynamic parameter)
*/
double turnGain;
/**
* Lateral length of each foot step
* in meters.
* >0 goes left
* <0 goes right
* (dynamic parameter)
*/
double lateralGain;
/**
* Vertical foot offset from trunk
* in meters (positive)
* 0 is in init position
* > 0 set the robot lower to the ground
*/
double trunkZOffset;
/**
* Lateral trunk oscillation amplitude
* in meters (positive)
*/
double swingGain;
/**
* Lateral angular oscillation amplitude
* of swing trunkRoll in radian
*/
double swingRollGain;
/**
* Phase shift of lateral trunk oscillation
* between 0 and 1
*/
double swingPhase;
/**
* Foot X-Z spline velocities
* at ground take off and ground landing.
* Step stands for X and rise stands for Z
* velocities.
* Typical values ranges within 0 and 5.
* >0 for DownVel is having the foot touching the
* ground with backward velocity.
* >0 for UpVel is having the foot going back
* forward with non perpendicular tangent.
*/
double stepUpVel;
double stepDownVel;
double riseUpVel;
double riseDownVel;
/**
* Time length in phase time
* where swing lateral oscillation
* remains on the same side
* between 0 and 0.5
*/
double swingPause;
/**
* Swing lateral spline velocity (positive).
* Control the "smoothness" of swing trajectory.
* Typical values are between 0 and 5.
*/
double swingVel;
/**
* Forward trunk-foot offset
* with respect to foot in meters
* >0 moves the trunk forward
* <0 moves the trunk backward
*/
double trunkXOffset;
/**
* Lateral trunk-foot offset
* with respect to foot in meters
* >0 moves the trunk on the left
* <0 moves the trunk on the right
*/
double trunkYOffset;
/**
* Trunk angular rotation
* around Y in radian
* >0 bends the trunk forward
* <0 bends the trunk backward
*/
double trunkPitch;
/**
* Trunk angular rotation
* around X in radian
* >0 bends the trunk on the right
* <0 bends the trunk on the left
*/
double trunkRoll;
/**
* Add extra offset on X, Y and Z
* direction on left and right feet
* in meters
*/
double extraLeftX;
double extraLeftY;
double extraLeftZ;
double extraRightX;
double extraRightY;
double extraRightZ;
/**
* Add extra angular offset on
* Yaw, Pitch and Roll rotation of
* left and right foot in radians
*/
double extraLeftYaw;
double extraLeftPitch;
double extraLeftRoll;
double extraRightYaw;
double extraRightPitch;
double extraRightRoll;
};
/**
* IKWalk
*
* Open loop walk engine based on
* cubic splines in Cartesian space and
* inverse kinematics for "standard"
* small humanoid robots.
*/
class IKWalk
{
public:
/**
* Compute and return target motor reference
* positions using given walk parameters at given
* phase (between 0 and 1).
* Phase is updated according to frequency
* parameter and given time step dt.
* If inverse kinematics fail, false is return and
* neither phase or output is updated.
*/
static bool walk(
const IKWalkParameters& params,
double dt,
double& phase,
IKWalkOutputs& outputs);
private:
/**
* Cycle given phase between 0 and 1
*/
static void boundPhase(double& phase);
};
}
#endif