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
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255 | /**
* @file FootStepPlanner.cpp
*
* @author <a href="mailto:xu@informatik.hu-berlin.de">Xu, Yuan</a>
* plan the foot step according to motion request
*/
#include "ZMPPlanner2018.h"
using namespace InverseKinematic;
void ZMPPlanner2018::init(size_t initial_number_of_cycles, Vector3d initialZMP, Vector3d targetZMP){
for (size_t i = 0; i+1 < initial_number_of_cycles; i++) {
double t = static_cast<double>(i) / static_cast<double>(initial_number_of_cycles - 1); // why? it should be only initial_number_of_cycles
Vector3d zmp = initialZMP*(1.0-t) + targetZMP*t;
getZMPReferenceBuffer().push(zmp);
}
}
void ZMPPlanner2018::execute(){
Step& planningStep = getStepBuffer().last();
Vector3d zmp;
Vector3d other_zmp;
if(planningStep.footStep.liftingFoot() == FootStep::NONE)
{
if(parameters.newZMP_ON) {
zmp = calculateStableCoMByFeet(planningStep.footStep.end(), parameters.bezierZMP.offsetX);
} else {
zmp = calculateStableCoMByFeet(planningStep.footStep.end(), parameters.simpleZMP.offsetX);
}
} else {
if(planningStep.planningCycle == 0){ // only need to be done once per step
Pose3D startFoot, targetFoot;
InverseKinematic::FeetPose begin = planningStep.footStep.begin();
InverseKinematic::FeetPose end = planningStep.footStep.end();
if(planningStep.footStep.liftingFoot() == FootStep::LEFT){
begin.localInRightFoot();
end.localInRightFoot();
startFoot = begin.left;
targetFoot = end.left;
} else {
begin.localInLeftFoot();
end.localInLeftFoot();
startFoot = begin.right;
targetFoot = end.right;
}
Vector2d currentStepLength = targetFoot.projectXY().translation - startFoot.projectXY().translation;
// HACK: hard coded maximal step lengths
// TODO: replace maxHipOffsetBasedOnStepLength by a scaling factor
// alternative: remove it at all, it isn't really used and its effectiveness is questionable
double maxStepLength = 50;
double maxCtrlLength = 80;<--- The scope of the variable 'maxCtrlLength' can be reduced. [+]The scope of the variable 'maxCtrlLength' can be reduced. Warning: Be careful when fixing this message, especially when there are inner loops. Here is an example where cppcheck will write that the scope for 'i' can be reduced:
void f(int x)
{
int i = 0;
if (x) {
// it's safe to move 'int i = 0;' here
for (int n = 0; n < 10; ++n) {
// it is possible but not safe to move 'int i = 0;' here
do_something(&i);
}
}
}
When you see this message it is always safe to reduce the variable scope 1 level.
PLOT("Walk:hipOffsetBasedOnStepLength.x", parameters.stabilization.maxHipOffsetBasedOnStepLength.x * std::abs(currentStepLength.x)/maxStepLength);
// TODO: should it be a part of the Step?
// TODO: hipOffsetBasedOnStepLength.y?
if (planningStep.type == Step::STEP_CONTROL && planningStep.walkRequest.stepControl.type == WalkRequest::StepControlRequest::KICKSTEP) {
zmpOffsetX = parameters.simpleZMP.offsetX
+ parameters.stabilization.maxHipOffsetBasedOnStepLengthForKicks.x * ((currentStepLength.x > 0) ? currentStepLength.x / maxCtrlLength : 0);
zmpOffsetY = parameters.simpleZMP.kickOffsetY
+ parameters.ZMPOffsetYByCharacter * (1-planningStep.walkRequest.character);
newZMPOffsetX = parameters.bezierZMP.offsetXForKicks
+ parameters.stabilization.maxHipOffsetBasedOnStepLengthForKicks.x * ((currentStepLength.x > 0) ? currentStepLength.x / maxCtrlLength : 0);
newZMPOffsetY = parameters.bezierZMP.offsetYForKicks
+ parameters.ZMPOffsetYByCharacter * (1-planningStep.walkRequest.character);
} else {
zmpOffsetX = parameters.simpleZMP.offsetX
+ parameters.stabilization.maxHipOffsetBasedOnStepLength.x * ((currentStepLength.x > 0) ? currentStepLength.x / maxStepLength : 0);
zmpOffsetY = parameters.simpleZMP.offsetY
+ parameters.ZMPOffsetYByCharacter * (1-planningStep.walkRequest.character);
newZMPOffsetX = parameters.bezierZMP.offsetX
+ parameters.stabilization.maxHipOffsetBasedOnStepLength.x * ((currentStepLength.x > 0) ? currentStepLength.x / maxStepLength : 0);
newZMPOffsetY = parameters.bezierZMP.offsetY
+ parameters.ZMPOffsetYByCharacter * (1-planningStep.walkRequest.character);
}
}
Vector2d zmp_new;
zmp_new = bezierBased(
planningStep.footStep,
planningStep.planningCycle,
planningStep.samplesDoubleSupport,
planningStep.samplesSingleSupport,
newZMPOffsetX,
newZMPOffsetY,
parameters.bezierZMP.inFootScalingY,
parameters.bezierZMP.inFootSpacing,
parameters.bezierZMP.transitionScaling);
// old zmp
Vector2d zmp_simple = simplest(planningStep.footStep, zmpOffsetX, zmpOffsetY);
if(parameters.newZMP_ON)
{
other_zmp = Vector3d(zmp_simple.x, zmp_simple.y, parameters.comHeight);
zmp = Vector3d(zmp_new.x, zmp_new.y, parameters.comHeight);
// HACK:
if (parameters.useZMPHackForKicks && planningStep.type == Step::STEP_CONTROL && planningStep.walkRequest.stepControl.type == WalkRequest::StepControlRequest::KICKSTEP) {
zmp.x = other_zmp.x;
}
} else {
zmp = Vector3d(zmp_simple.x, zmp_simple.y, parameters.comHeight);
other_zmp = Vector3d(zmp_new.x, zmp_new.y, parameters.comHeight);
}
}
Vector2d zmp_in_local = planningStep.footStep.supFoot().projectXY()/Vector2d(zmp.x,zmp.y);
PLOT("Walk:zmp:x", zmp_in_local.x);
PLOT("Walk:zmp:y", zmp_in_local.y);
Vector2d other_zmp_in_local = planningStep.footStep.supFoot().projectXY()/Vector2d(other_zmp.x,other_zmp.y);
PLOT("Walk:other_zmp:x", other_zmp_in_local.x);
PLOT("Walk:other_zmp:y", other_zmp_in_local.y);
//PLOT_GENERIC("Walk:zmp:xy", zmp.x, zmp.y);
//zmp.z = parameters().hip.comHeight;
getZMPReferenceBuffer().push(zmp);
DEBUG_REQUEST("Walk:draw_step_plan_geometry",
FIELD_DRAWING_CONTEXT;
getDebugDrawings().pen(Color::BLUE, 5.0);
getDebugDrawings().drawCircle(zmp.x, zmp.y, 10);
);
PLOT("Walk:DRAW_ZMP_x", zmp.x);
PLOT("Walk:DRAW_ZMP_y", zmp.y);
planningStep.planningCycle++;
}
Vector2d ZMPPlanner2018::simplest(const FootStep& step, double offsetX, double offsetY)
{
Pose3D supFoot = step.supFoot();
supFoot.translate(offsetX, offsetY * step.liftingFoot(), 0);
return Vector2d(supFoot.translation.x, supFoot.translation.y);
}//end simplest
Vector2d ZMPPlanner2018::bezierBased(
const FootStep step,
double cycle,
double samplesDoubleSupport, double samplesSingleSupport,
double offsetX, double offsetY,
double inFootScalingY,
double inFootSpacingY,
double transitionScaling)
{
Pose3D supFoot, startFoot, targetFoot;
// poses in support foot coordinates
if(step.liftingFoot() == FootStep::LEFT){
InverseKinematic::FeetPose begin = step.begin();
begin.localInRightFoot();
InverseKinematic::FeetPose end = step.end();
end.localInRightFoot();
supFoot = begin.right;
startFoot = begin.left;
targetFoot = end.left;
inFootSpacingY *= -1;
} else {
InverseKinematic::FeetPose begin = step.begin();
begin.localInLeftFoot();
InverseKinematic::FeetPose end = step.end();
end.localInLeftFoot();
supFoot = begin.left;
startFoot = begin.right;
targetFoot = end.right;
}
// Don't apply offset befor determining start and target for y.
// Applying offsets first will add discontinuities in y (start of step i won't be target of step i-1)
// because the y offset will be applied in different directions depending on the lifting foot
double start_y = (supFoot.translation.y + startFoot.translation.y)/2;
double target_y = (supFoot.translation.y + targetFoot.translation.y)/2;
double start_x = (supFoot.translation.x + startFoot.translation.x)/2 + offsetX;
double target_x = (supFoot.translation.x + targetFoot.translation.x)/2 + offsetX;
double supFootY = supFoot.translation.y + offsetY * step.liftingFoot();
static std::vector<Vector2d> trajectory;
static unsigned int idx;
if(cycle < 0.000001 || trajectory.empty()){
idx = 0;
trajectory.clear();
static std::vector<Vector2d> temp;
// trajectory from start to foot as (time, value) pairs
std::vector<Vector2d> start_to_foot = {Vector2d(0.0,start_y),
Vector2d(transitionScaling/4,(start_y+supFootY-inFootSpacingY)/2),
Vector2d(transitionScaling/4,(start_y+supFootY-inFootSpacingY)/2),
Vector2d(transitionScaling/2, supFootY-inFootSpacingY)};
temp = FourPointBezier2D(start_to_foot, 200);
trajectory.insert(trajectory.end(),temp.begin(),temp.end());
// trajectory in foot as (time, value) pairs
Vector2d inFootStart = Vector2d(transitionScaling/2,supFootY-inFootSpacingY);
Vector2d inDirection = Vector2d(transitionScaling/2+transitionScaling/4,supFootY-inFootSpacingY+(supFootY-inFootSpacingY-start_y)/2) - inFootStart;
Vector2d inFootEnd = Vector2d(1-transitionScaling/2, supFootY-inFootSpacingY);
Vector2d outDirection = inFootEnd - Vector2d(1-transitionScaling/2-transitionScaling/4,supFootY-inFootSpacingY+(supFootY-inFootSpacingY-target_y)/2);
//scaling directions to let the control points be on supFootY+inFootSpacingY
inDirection *= (supFootY+inFootSpacingY)/inDirection.y;
outDirection *= -(supFootY+inFootSpacingY)/outDirection.y;
std::vector<Vector2d> in_foot = {inFootStart,
inFootStart + (inDirection * inFootScalingY),
inFootEnd - (outDirection * inFootScalingY),
inFootEnd};
temp = FourPointBezier2D(in_foot, 200);
trajectory.insert(trajectory.end(),temp.begin(),temp.end());
// trajectory from foot to target as (time, value) pairs
std::vector<Vector2d> foot_to_target = {Vector2d(1-transitionScaling/2,supFootY-inFootSpacingY),
Vector2d(1-transitionScaling/4,(target_y+supFootY-inFootSpacingY)/2),
Vector2d(1-transitionScaling/4,(target_y+supFootY-inFootSpacingY)/2),
Vector2d(1.0, target_y)};
temp = FourPointBezier2D(foot_to_target, 200);
trajectory.insert(trajectory.end(),temp.begin(),temp.end());
}
double t = cycle / (samplesSingleSupport+samplesDoubleSupport);
for(; trajectory[idx].x < t && idx+1 < trajectory.size(); ++idx);
double y = trajectory[idx].y;
double x = start_x*(1.0-t) + t*target_x;
return step.supFoot().projectXY()*Vector2d(x, y);
}//end bezierBased
Vector3d ZMPPlanner2018::calculateStableCoMByFeet(FeetPose feet, double offsetX) const
{
feet.left.translate(offsetX, 0, 0);
feet.right.translate(offsetX, 0, 0);
Vector3d com;
com = (feet.left.translation + feet.right.translation) * 0.5;
com.z = parameters.comHeight;
return com;
}
|