1 |
|
|
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 |
|
|
* Copyright Projet JRL-Japan, 2007 |
3 |
|
|
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
4 |
|
|
* |
5 |
|
|
* File: NextStep.h |
6 |
|
|
* Project: SOT |
7 |
|
|
* Author: Nicolas Mansard |
8 |
|
|
* |
9 |
|
|
* Version control |
10 |
|
|
* =============== |
11 |
|
|
* |
12 |
|
|
* $Id$ |
13 |
|
|
* |
14 |
|
|
* Description |
15 |
|
|
* ============ |
16 |
|
|
* |
17 |
|
|
* |
18 |
|
|
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ |
19 |
|
|
|
20 |
|
|
#include <sot/pattern-generator/next-step.h> |
21 |
|
|
#include <time.h> |
22 |
|
|
|
23 |
|
|
#include <cmath> |
24 |
|
|
#include <sot/core/debug.hh> |
25 |
|
|
#ifndef WIN32 |
26 |
|
|
#include <sys/time.h> |
27 |
|
|
#else |
28 |
|
|
#include <Winsock2.h> |
29 |
|
|
|
30 |
|
|
#include <sot/core/utils-windows.hh> |
31 |
|
|
#endif /*WIN32*/ |
32 |
|
|
|
33 |
|
|
#include <dynamic-graph/factory.h> |
34 |
|
|
|
35 |
|
|
#include <sot/core/macros-signal.hh> |
36 |
|
|
|
37 |
|
|
namespace dynamicgraph { |
38 |
|
|
namespace sot { |
39 |
|
|
|
40 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NextStep, "NextStep"); |
41 |
|
|
|
42 |
|
|
/* --- CONSTRUCT -------------------------------------------------------- */ |
43 |
|
|
/* --- CONSTRUCT -------------------------------------------------------- */ |
44 |
|
|
/* --- CONSTRUCT -------------------------------------------------------- */ |
45 |
|
|
|
46 |
|
|
const unsigned int NextStep::PERIOD_DEFAULT = 160; // 160iter=800ms |
47 |
|
|
const double NextStep::ZERO_STEP_POSITION_DEFAULT = 0.19; |
48 |
|
|
|
49 |
|
|
NextStep::NextStep(const std::string &name) |
50 |
|
|
: Entity(name) |
51 |
|
|
|
52 |
|
|
, |
53 |
|
|
footPrintList(), |
54 |
|
|
period(PERIOD_DEFAULT), |
55 |
|
|
timeLastIntroduction(0) |
56 |
|
|
|
57 |
|
|
, |
58 |
|
|
mode(MODE_3D), |
59 |
|
|
state(STATE_STOPED) |
60 |
|
|
|
61 |
|
|
, |
62 |
|
|
zeroStepPosition(ZERO_STEP_POSITION_DEFAULT) |
63 |
|
|
|
64 |
|
|
, |
65 |
|
|
rfMref0(), |
66 |
|
|
lfMref0(), |
67 |
|
|
twoHandObserver(name) |
68 |
|
|
|
69 |
|
|
, |
70 |
|
|
verbose(0x0) |
71 |
|
|
|
72 |
|
|
, |
73 |
|
|
referencePositionLeftSIN( |
74 |
|
|
NULL, "NextStep(" + name + ")::input(vector)::posrefleft"), |
75 |
|
|
referencePositionRightSIN( |
76 |
|
|
NULL, "NextStep(" + name + ")::input(vector)::posrefright") |
77 |
|
|
|
78 |
|
|
, |
79 |
|
|
contactFootSIN(NULL, "NextStep(" + name + ")::input(uint)::contactfoot"), |
80 |
|
|
triggerSOUT("NextStep(" + name + ")::input(dummy)::trigger") { |
81 |
|
|
sotDEBUGIN(5); |
82 |
|
|
|
83 |
|
|
triggerSOUT.setFunction(boost::bind(&NextStep::triggerCall, this, _1, _2)); |
84 |
|
|
|
85 |
|
|
signalRegistration(referencePositionLeftSIN << referencePositionRightSIN |
86 |
|
|
<< contactFootSIN << triggerSOUT); |
87 |
|
|
signalRegistration(twoHandObserver.getSignals()); |
88 |
|
|
|
89 |
|
|
referencePositionLeftSIN.plug(&twoHandObserver.referencePositionLeftSOUT); |
90 |
|
|
referencePositionRightSIN.plug(&twoHandObserver.referencePositionRightSOUT); |
91 |
|
|
|
92 |
|
|
sotDEBUGOUT(5); |
93 |
|
|
} |
94 |
|
|
|
95 |
|
|
NextStep::~NextStep(void) { |
96 |
|
|
sotDEBUGIN(5); |
97 |
|
|
|
98 |
|
|
sotDEBUGOUT(5); |
99 |
|
|
return; |
100 |
|
|
} |
101 |
|
|
|
102 |
|
|
/* --- FUNCTIONS ------------------------------------------------------- */ |
103 |
|
|
/* --- FUNCTIONS ------------------------------------------------------- */ |
104 |
|
|
/* --- FUNCTIONS ------------------------------------------------------- */ |
105 |
|
|
|
106 |
|
|
void NextStep::nextStep(const int &timeCurr) { |
107 |
|
|
sotDEBUGIN(15); |
108 |
|
|
|
109 |
|
|
const unsigned &sfoot = contactFootSIN(timeCurr); |
110 |
|
|
const MatrixHomogeneous &wMlf = |
111 |
|
|
twoHandObserver.leftFootPositionSIN.access(timeCurr); |
112 |
|
|
const MatrixHomogeneous &wMrf = |
113 |
|
|
twoHandObserver.rightFootPositionSIN.access(timeCurr); |
114 |
|
|
|
115 |
|
|
// actual and reference position of reference frame in fly foot, |
116 |
|
|
// position of fly foot in support foot. |
117 |
|
|
|
118 |
|
|
MatrixHomogeneous ffMref, ffMref0; |
119 |
|
|
MatrixHomogeneous sfMff; |
120 |
|
|
if (sfoot != 1) // --- left foot support --- |
121 |
|
|
{ |
122 |
|
|
ffMref = referencePositionRightSIN.access(timeCurr); |
123 |
|
|
ffMref0 = rfMref0; |
124 |
|
|
MatrixHomogeneous sfMw; |
125 |
|
|
sfMw = wMlf.inverse(); |
126 |
|
|
sfMff = sfMw * wMrf; |
127 |
|
|
} else // -- right foot support --- |
128 |
|
|
{ |
129 |
|
|
ffMref = referencePositionLeftSIN.access(timeCurr); |
130 |
|
|
ffMref0 = lfMref0; |
131 |
|
|
MatrixHomogeneous sfMw; |
132 |
|
|
sfMw = wMrf.inverse(); |
133 |
|
|
sfMff = sfMw * wMlf; |
134 |
|
|
} |
135 |
|
|
|
136 |
|
|
// homogeneous transform from ref position of ref frame to |
137 |
|
|
// actual position of ref frame. |
138 |
|
|
|
139 |
|
|
MatrixHomogeneous ref0Mff; |
140 |
|
|
ref0Mff = ffMref0.inverse(); |
141 |
|
|
MatrixHomogeneous ref0Mref; |
142 |
|
|
ref0Mref = ref0Mff * ffMref; |
143 |
|
|
|
144 |
|
|
// extract the translation part and express it in the support |
145 |
|
|
// foot frame. |
146 |
|
|
|
147 |
|
|
MatrixHomogeneous sfMref0; |
148 |
|
|
sfMref0 = sfMff * ffMref0; |
149 |
|
|
Vector t_ref0(3); |
150 |
|
|
t_ref0 = ref0Mref.translation(); |
151 |
|
|
MatrixRotation sfRref0; |
152 |
|
|
sfRref0 = sfMref0.linear(); |
153 |
|
|
Vector t_sf = sfRref0 * t_ref0; |
154 |
|
|
|
155 |
|
|
// add it to the position of the fly foot in support foot to |
156 |
|
|
// get the new position of fly foot in support foot. |
157 |
|
|
|
158 |
|
|
Vector pff_sf(3); |
159 |
|
|
pff_sf = sfMff.translation(); |
160 |
|
|
t_sf += pff_sf; |
161 |
|
|
|
162 |
|
|
// compute the rotation that transforms ref0 into ref, |
163 |
|
|
// express it in the support foot frame. Then get the |
164 |
|
|
// associated yaw (rot around z). |
165 |
|
|
|
166 |
|
|
MatrixRotation ref0Rsf; |
167 |
|
|
ref0Rsf = sfRref0.transpose(); |
168 |
|
|
MatrixRotation ref0Rref; |
169 |
|
|
ref0Rref = ref0Mref.linear(); |
170 |
|
|
MatrixRotation tmp = ref0Rref * ref0Rsf; |
171 |
|
|
MatrixRotation Rref = sfRref0 * tmp; |
172 |
|
|
VectorRollPitchYaw rpy; |
173 |
|
|
rpy = (Rref.eulerAngles(2, 1, 0)).reverse(); |
174 |
|
|
|
175 |
|
|
// get the yaw of the current orientation of the ff wrt sf. |
176 |
|
|
// Add it to the previously computed rpy. |
177 |
|
|
|
178 |
|
|
MatrixRotation sfRff; |
179 |
|
|
sfRff = sfMff.linear(); |
180 |
|
|
VectorRollPitchYaw rpy_ff; |
181 |
|
|
rpy_ff = (sfRff.eulerAngles(2, 1, 0)).reverse(); |
182 |
|
|
rpy += rpy_ff; |
183 |
|
|
|
184 |
|
|
// Now we can compute and insert the new step (we just need |
185 |
|
|
// to express the coordinates of the vector that joins the |
186 |
|
|
// support foot to the new fly foot in the coordinate frame of the |
187 |
|
|
// new fly foot). |
188 |
|
|
// |
189 |
|
|
// [dX;dY] = A^t [X;Y] |
190 |
|
|
// |
191 |
|
|
// where A is the planar rotation matrix of angle theta, [X;Y] |
192 |
|
|
// is the planar column-vector joining the support foot |
193 |
|
|
// to the new fly foot, |
194 |
|
|
// expressed in the support foot frame, and [dX;dY] is this same planar |
195 |
|
|
// column-vector expressed in the coordinates frame of the new fly foot. |
196 |
|
|
// |
197 |
|
|
// See the technical report of Olivier Stasse for more details, |
198 |
|
|
// on top of page 79. |
199 |
|
|
|
200 |
|
|
double ns_x = 0, ns_y = 0, ns_theta = 0; |
201 |
|
|
if (mode != MODE_1D) { |
202 |
|
|
ns_theta = rpy(2) * 180 / 3.14159265; |
203 |
|
|
if (fabs(ns_theta) < 10) { |
204 |
|
|
ns_theta = 0; |
205 |
|
|
rpy(2) = 0; |
206 |
|
|
} |
207 |
|
|
|
208 |
|
|
double x = t_sf(0); |
209 |
|
|
double y = t_sf(1); |
210 |
|
|
|
211 |
|
|
double ctheta = cos(rpy(2)); |
212 |
|
|
double stheta = sin(rpy(2)); |
213 |
|
|
|
214 |
|
|
ns_x = x * ctheta + y * stheta; |
215 |
|
|
ns_y = -x * stheta + y * ctheta; |
216 |
|
|
|
217 |
|
|
ns_theta = rpy(2) * 180 / 3.14159265; |
218 |
|
|
if (fabs(ns_theta) < 10) { |
219 |
|
|
ns_theta = 0; |
220 |
|
|
} |
221 |
|
|
} else { |
222 |
|
|
ns_x = t_sf(0); |
223 |
|
|
if (sfoot != 1) { |
224 |
|
|
ns_y = -ZERO_STEP_POSITION_DEFAULT; |
225 |
|
|
} else { |
226 |
|
|
ns_y = ZERO_STEP_POSITION_DEFAULT; |
227 |
|
|
} |
228 |
|
|
ns_theta = 0.; |
229 |
|
|
} |
230 |
|
|
|
231 |
|
|
FootPrint newStep; |
232 |
|
|
|
233 |
|
|
if (sfoot != 1) { |
234 |
|
|
newStep.contact = CONTACT_LEFT_FOOT; |
235 |
|
|
} else { |
236 |
|
|
newStep.contact = CONTACT_RIGHT_FOOT; |
237 |
|
|
} |
238 |
|
|
|
239 |
|
|
newStep.x = ns_x; |
240 |
|
|
newStep.y = ns_y; |
241 |
|
|
newStep.theta = ns_theta; |
242 |
|
|
|
243 |
|
|
newStep.introductionTime = timeCurr; |
244 |
|
|
|
245 |
|
|
footPrintList.push_back(newStep); |
246 |
|
|
footPrintList.pop_front(); |
247 |
|
|
|
248 |
|
|
sotDEBUGOUT(15); |
249 |
|
|
} |
250 |
|
|
|
251 |
|
|
void NextStep::thisIsZero() { |
252 |
|
|
sotDEBUGIN(15); |
253 |
|
|
|
254 |
|
|
rfMref0 = referencePositionRightSIN.accessCopy(); |
255 |
|
|
lfMref0 = referencePositionLeftSIN.accessCopy(); |
256 |
|
|
|
257 |
|
|
sotDEBUGOUT(15); |
258 |
|
|
} |
259 |
|
|
|
260 |
|
|
void NextStep::starter(const int &timeCurr) { |
261 |
|
|
sotDEBUGIN(15); |
262 |
|
|
|
263 |
|
|
footPrintList.clear(); |
264 |
|
|
FootPrint initSteps[4]; |
265 |
|
|
|
266 |
|
|
initSteps[0].contact = CONTACT_RIGHT_FOOT; |
267 |
|
|
initSteps[0].x = 0; |
268 |
|
|
initSteps[0].y = -zeroStepPosition / 2; |
269 |
|
|
initSteps[0].theta = 0; |
270 |
|
|
initSteps[0].introductionTime = -1; |
271 |
|
|
footPrintList.push_back(initSteps[0]); |
272 |
|
|
introductionCallBack(-1); |
273 |
|
|
|
274 |
|
|
initSteps[1].contact = CONTACT_LEFT_FOOT; |
275 |
|
|
initSteps[1].x = 0; |
276 |
|
|
initSteps[1].y = +zeroStepPosition; |
277 |
|
|
initSteps[1].theta = 0; |
278 |
|
|
initSteps[1].introductionTime = -1; |
279 |
|
|
footPrintList.push_back(initSteps[1]); |
280 |
|
|
introductionCallBack(-1); |
281 |
|
|
|
282 |
|
|
initSteps[2].contact = CONTACT_RIGHT_FOOT; |
283 |
|
|
initSteps[2].x = 0; |
284 |
|
|
initSteps[2].y = -zeroStepPosition; |
285 |
|
|
initSteps[2].theta = 0; |
286 |
|
|
initSteps[2].introductionTime = -1; |
287 |
|
|
footPrintList.push_back(initSteps[2]); |
288 |
|
|
introductionCallBack(-1); |
289 |
|
|
|
290 |
|
|
initSteps[3].contact = CONTACT_LEFT_FOOT; |
291 |
|
|
initSteps[3].x = 0; |
292 |
|
|
initSteps[3].y = +zeroStepPosition; |
293 |
|
|
initSteps[3].theta = 0; |
294 |
|
|
initSteps[3].introductionTime = -1; |
295 |
|
|
footPrintList.push_back(initSteps[3]); |
296 |
|
|
introductionCallBack(-1); |
297 |
|
|
|
298 |
|
|
timeLastIntroduction = timeCurr - period + 1; |
299 |
|
|
if (verbose) (*verbose) << "NextStep started." << std::endl; |
300 |
|
|
|
301 |
|
|
sotDEBUGOUT(15); |
302 |
|
|
return; |
303 |
|
|
} |
304 |
|
|
|
305 |
|
|
void NextStep::stoper(const int &) { |
306 |
|
|
sotDEBUGIN(15); |
307 |
|
|
|
308 |
|
|
sotDEBUGOUT(15); |
309 |
|
|
return; |
310 |
|
|
} |
311 |
|
|
|
312 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
313 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
314 |
|
|
/* --- SIGNALS ---------------------------------------------------------- */ |
315 |
|
|
|
316 |
|
|
int &NextStep::triggerCall(int &dummy, int timeCurrent) { |
317 |
|
|
sotDEBUGIN(45); |
318 |
|
|
|
319 |
|
|
switch (state) { |
320 |
|
|
case STATE_STOPED: |
321 |
|
|
break; |
322 |
|
|
case STATE_STARTED: { |
323 |
|
|
int nextIntoductionTime = timeLastIntroduction + period; |
324 |
|
|
if (nextIntoductionTime <= timeCurrent) { |
325 |
|
|
nextStep(timeCurrent); |
326 |
|
|
if (NULL != verbose) { |
327 |
|
|
FootPrint &lastStep = footPrintList.back(); |
328 |
|
|
(*verbose) << "<T=" << timeCurrent << "> Introduced a new step: "; |
329 |
|
|
switch (lastStep.contact) { |
330 |
|
|
case CONTACT_LEFT_FOOT: |
331 |
|
|
(*verbose) << "LF "; |
332 |
|
|
break; |
333 |
|
|
case CONTACT_RIGHT_FOOT: |
334 |
|
|
(*verbose) << "RF "; |
335 |
|
|
break; |
336 |
|
|
} |
337 |
|
|
(*verbose) << lastStep.x << "," << lastStep.y << "," << lastStep.theta |
338 |
|
|
<< std::endl; |
339 |
|
|
} |
340 |
|
|
introductionCallBack(timeCurrent); |
341 |
|
|
timeLastIntroduction = timeCurrent; |
342 |
|
|
} |
343 |
|
|
break; |
344 |
|
|
} |
345 |
|
|
case STATE_STARTING: { |
346 |
|
|
starter(timeCurrent); |
347 |
|
|
break; |
348 |
|
|
} |
349 |
|
|
case STATE_STOPING: { |
350 |
|
|
stoper(timeCurrent); |
351 |
|
|
break; |
352 |
|
|
} |
353 |
|
|
}; |
354 |
|
|
|
355 |
|
|
sotDEBUGOUT(45); |
356 |
|
|
|
357 |
|
|
return dummy; |
358 |
|
|
} |
359 |
|
|
|
360 |
|
|
/* --- PARAMS ---------------------------------------------------------- */ |
361 |
|
|
/* --- PARAMS ---------------------------------------------------------- */ |
362 |
|
|
/* --- PARAMS ---------------------------------------------------------- */ |
363 |
|
|
|
364 |
|
|
void NextStep::display(std::ostream &os) const { |
365 |
|
|
os << "NextStep <" << getName() << ">:" << std::endl; |
366 |
|
|
for (std::deque<FootPrint>::const_iterator iter = footPrintList.begin(); |
367 |
|
|
iter != footPrintList.end(); ++iter) { |
368 |
|
|
os << "<time=" << iter->introductionTime << "> "; |
369 |
|
|
switch (iter->contact) { |
370 |
|
|
case CONTACT_LEFT_FOOT: |
371 |
|
|
os << "LF "; |
372 |
|
|
break; |
373 |
|
|
case CONTACT_RIGHT_FOOT: |
374 |
|
|
os << "RF "; |
375 |
|
|
break; |
376 |
|
|
} |
377 |
|
|
os << "(" << iter->x << "," << iter->y << "," << iter->theta << ")" |
378 |
|
|
<< std::endl; |
379 |
|
|
} |
380 |
|
|
} |
381 |
|
|
|
382 |
|
|
void NextStep::commandLine(const std::string &cmdLine, |
383 |
|
|
std::istringstream &cmdArgs, std::ostream &os) { |
384 |
|
|
if (cmdLine == "help") { |
385 |
|
|
os << "NextStep: " << std::endl |
386 |
|
|
<< " - verbose [OFF]" << std::endl |
387 |
|
|
<< " - state [{start|stop}] \t get/set the stepper state. " << std::endl |
388 |
|
|
<< " - yZeroStep [<value>] \t get/set the Y default position." |
389 |
|
|
<< std::endl |
390 |
|
|
<< " - thisIsZero {record|disp}" << std::endl |
391 |
|
|
<< std::endl; |
392 |
|
|
} else if (cmdLine == "state") { |
393 |
|
|
cmdArgs >> std::ws; |
394 |
|
|
if (cmdArgs.good()) { |
395 |
|
|
std::string statearg; |
396 |
|
|
cmdArgs >> statearg; |
397 |
|
|
if (statearg == "start") { |
398 |
|
|
state = STATE_STARTING; |
399 |
|
|
} else if (statearg == "stop") { |
400 |
|
|
state = STATE_STOPING; |
401 |
|
|
} |
402 |
|
|
} else { |
403 |
|
|
os << "state = "; |
404 |
|
|
switch (state) { |
405 |
|
|
case STATE_STARTING: |
406 |
|
|
os << "starting"; |
407 |
|
|
break; |
408 |
|
|
case STATE_STOPING: |
409 |
|
|
os << "stoping"; |
410 |
|
|
break; |
411 |
|
|
case STATE_STARTED: |
412 |
|
|
os << "started"; |
413 |
|
|
break; |
414 |
|
|
case STATE_STOPED: |
415 |
|
|
os << "stoped"; |
416 |
|
|
break; |
417 |
|
|
default: |
418 |
|
|
os << "error"; |
419 |
|
|
break; |
420 |
|
|
} |
421 |
|
|
os << std::endl; |
422 |
|
|
} |
423 |
|
|
} else if (cmdLine == "yZeroStep") { |
424 |
|
|
cmdArgs >> std::ws; |
425 |
|
|
if (cmdArgs.good()) { |
426 |
|
|
cmdArgs >> zeroStepPosition; |
427 |
|
|
} else { |
428 |
|
|
os << "yzero = " << zeroStepPosition; |
429 |
|
|
} |
430 |
|
|
} else if (cmdLine == "thisIsZero") { |
431 |
|
|
std::string arg; |
432 |
|
|
cmdArgs >> arg; |
433 |
|
|
if (arg == "disp_left") { |
434 |
|
|
os << "zero_left = " << lfMref0; |
435 |
|
|
} else if (arg == "disp_right") { |
436 |
|
|
os << "zero_right = " << rfMref0; |
437 |
|
|
} else if (arg == "record") { |
438 |
|
|
thisIsZero(); |
439 |
|
|
} |
440 |
|
|
} else if (cmdLine == "verbose") { |
441 |
|
|
cmdArgs >> std::ws; |
442 |
|
|
std::string offarg; |
443 |
|
|
if ((cmdArgs.good()) && (cmdArgs >> offarg, offarg == "OFF")) { |
444 |
|
|
verbose = NULL; |
445 |
|
|
} else { |
446 |
|
|
verbose = &os; |
447 |
|
|
} |
448 |
|
|
} else if (cmdLine == "mode1d") { |
449 |
|
|
mode = MODE_1D; |
450 |
|
|
} else if (cmdLine == "mode3d") { |
451 |
|
|
mode = MODE_3D; |
452 |
|
|
} else { |
453 |
|
|
} |
454 |
|
|
} |
455 |
|
|
|
456 |
|
|
/* --- TWO HAND -------------------------------------------------------- */ |
457 |
|
|
/* --- TWO HAND -------------------------------------------------------- */ |
458 |
|
|
/* --- TWO HAND -------------------------------------------------------- */ |
459 |
|
|
|
460 |
|
|
NextStepTwoHandObserver::NextStepTwoHandObserver(const std::string &name) |
461 |
|
|
: referencePositionLeftSIN(NULL, "NextStepTwoHandObserver(" + name + |
462 |
|
|
")::input(vector)::positionLeft"), |
463 |
|
|
referenceVelocityLeftSIN(NULL, "NextStepTwoHandObserver(" + name + |
464 |
|
|
")::input(vector)::velocityLeft"), |
465 |
|
|
referenceAccelerationLeftSIN(NULL, |
466 |
|
|
"NextStepTwoHandObserver(" + name + |
467 |
|
|
")::input(vector)::accelerationLeft"), |
468 |
|
|
leftFootPositionSIN(NULL, "NextStepTwoHandObserver(" + name + |
469 |
|
|
")::input(matrixhomo)::leftfoot") |
470 |
|
|
|
471 |
|
|
, |
472 |
|
|
referencePositionRightSIN(NULL, "NextStepTwoHandObserver(" + name + |
473 |
|
|
")::input(vector)::positionRight"), |
474 |
|
|
referenceVelocityRightSIN(NULL, "NextStepTwoHandObserver(" + name + |
475 |
|
|
")::input(vector)::velocityRight"), |
476 |
|
|
referenceAccelerationRightSIN(NULL, |
477 |
|
|
"NextStepTwoHandObserver(" + name + |
478 |
|
|
")::input(vector)::accelerationRight"), |
479 |
|
|
rightFootPositionSIN(NULL, "NextStepTwoHandObserver(" + name + |
480 |
|
|
")::input(matrixhomo)::rightfoot") |
481 |
|
|
|
482 |
|
|
, |
483 |
|
|
referencePositionLeftSOUT( |
484 |
|
|
boost::bind(&NextStepTwoHandObserver::computeReferencePositionLeft, |
485 |
|
|
this, _1, _2), |
486 |
|
|
leftFootPositionSIN << referencePositionLeftSIN |
487 |
|
|
<< referencePositionRightSIN, |
488 |
|
|
"NextStepTwoHandObserver(" + name + |
489 |
|
|
")::output(vector)::position2handLeft"), |
490 |
|
|
referencePositionRightSOUT( |
491 |
|
|
boost::bind(&NextStepTwoHandObserver::computeReferencePositionRight, |
492 |
|
|
this, _1, _2), |
493 |
|
|
rightFootPositionSIN << referencePositionLeftSIN |
494 |
|
|
<< referencePositionRightSIN, |
495 |
|
|
"NextStepTwoHandObserver(" + name + |
496 |
|
|
")::output(vector)::position2handRight") |
497 |
|
|
|
498 |
|
|
, |
499 |
|
|
referenceVelocitySOUT( |
500 |
|
|
SOT_MEMBER_SIGNAL_2(NextStepTwoHandObserver::computeReferenceVelocity, |
501 |
|
|
referenceVelocityLeftSIN, Vector, |
502 |
|
|
referenceVelocityRightSIN, Vector), |
503 |
|
|
"NextStepTwoHandObserver(" + name + |
504 |
|
|
")::output(vector)::velocity2Hand"), |
505 |
|
|
referenceAccelerationSOUT( |
506 |
|
|
SOT_MEMBER_SIGNAL_2( |
507 |
|
|
NextStepTwoHandObserver::computeReferenceAcceleration, |
508 |
|
|
referenceAccelerationLeftSIN, Vector, |
509 |
|
|
referenceAccelerationRightSIN, Vector), |
510 |
|
|
"NextStepTwoHandObserver(" + name + |
511 |
|
|
")::output(vector)::acceleration2Hand") { |
512 |
|
|
sotDEBUGINOUT(25); |
513 |
|
|
} |
514 |
|
|
|
515 |
|
|
SignalArray<int> NextStepTwoHandObserver::getSignals(void) { |
516 |
|
|
return (referencePositionLeftSIN |
517 |
|
|
<< referenceVelocityLeftSIN << referenceAccelerationLeftSIN |
518 |
|
|
<< leftFootPositionSIN << referencePositionRightSIN |
519 |
|
|
<< referenceVelocityRightSIN << referenceAccelerationRightSIN |
520 |
|
|
<< rightFootPositionSIN << referencePositionLeftSOUT |
521 |
|
|
<< referencePositionRightSOUT << referenceVelocitySOUT |
522 |
|
|
<< referenceAccelerationSOUT); |
523 |
|
|
} |
524 |
|
|
NextStepTwoHandObserver::operator SignalArray<int>() { |
525 |
|
|
return (referencePositionLeftSIN |
526 |
|
|
<< referenceVelocityLeftSIN << referenceAccelerationLeftSIN |
527 |
|
|
<< leftFootPositionSIN << referencePositionRightSIN |
528 |
|
|
<< referenceVelocityRightSIN << referenceAccelerationRightSIN |
529 |
|
|
<< rightFootPositionSIN << referencePositionLeftSOUT |
530 |
|
|
<< referencePositionRightSOUT << referenceVelocitySOUT |
531 |
|
|
<< referenceAccelerationSOUT); |
532 |
|
|
} |
533 |
|
|
|
534 |
|
|
MatrixHomogeneous &NextStepTwoHandObserver::computeRefPos( |
535 |
|
|
MatrixHomogeneous &res, int timeCurr, const MatrixHomogeneous &wMsf) { |
536 |
|
|
sotDEBUGIN(15); |
537 |
|
|
|
538 |
|
|
#define RIGHT_HAND_REFERENCE 1 |
539 |
|
|
#if RIGHT_HAND_REFERENCE |
540 |
|
|
|
541 |
|
|
const MatrixHomogeneous &wMrh = referencePositionRightSIN(timeCurr); |
542 |
|
|
MatrixHomogeneous sfMw; |
543 |
|
|
sfMw = wMsf.inverse(); |
544 |
|
|
res = sfMw * wMrh; |
545 |
|
|
|
546 |
|
|
#else |
547 |
|
|
|
548 |
|
|
const MatrixHomogeneous &wMlh = referencePositionLeftSIN(timeCurr); |
549 |
|
|
const MatrixHomogeneous &wMrh = referencePositionRightSIN(timeCurr); |
550 |
|
|
|
551 |
|
|
MatrixHomogeneous sfMw; |
552 |
|
|
sfMw = wMsf.inverse(); |
553 |
|
|
MatrixHomogeneous sfMlh; |
554 |
|
|
sfMlh = sfMw * wMlh; |
555 |
|
|
MatrixHomogeneous sfMrh; |
556 |
|
|
sfMrh = sfMw * wMrh; |
557 |
|
|
|
558 |
|
|
MatrixRotation R; |
559 |
|
|
VectorRollPitchYaw rpy; |
560 |
|
|
|
561 |
|
|
Vector prh(3); |
562 |
|
|
prh = sfMrh.translation(); |
563 |
|
|
R = sfMrh.linear(); |
564 |
|
|
VectorRollPitchYaw rpy_rh; |
565 |
|
|
rpy_rh = (R.eulerAngles(2, 1, 0)).reverse(); |
566 |
|
|
|
567 |
|
|
Vector plh(3); |
568 |
|
|
plh = sfMlh.translation(); |
569 |
|
|
R = sfMlh.linear(); |
570 |
|
|
VectorRollPitchYaw rpy_lh; |
571 |
|
|
rpy_lh = (R.eulerAngles(2, 1, 0)).reverse(); |
572 |
|
|
|
573 |
|
|
Vector p = 0.5 * (plh + prh); |
574 |
|
|
rpy = 0.5 * (rpy_rh + rpy_lh); |
575 |
|
|
|
576 |
|
|
R = (Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * |
577 |
|
|
Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * |
578 |
|
|
Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX())) |
579 |
|
|
.toRotationMatrix(); |
580 |
|
|
res.linear() = R; |
581 |
|
|
res.translation() = p; |
582 |
|
|
|
583 |
|
|
#endif |
584 |
|
|
|
585 |
|
|
sotDEBUGOUT(15); |
586 |
|
|
return res; |
587 |
|
|
} |
588 |
|
|
|
589 |
|
|
MatrixHomogeneous &NextStepTwoHandObserver::computeReferencePositionLeft( |
590 |
|
|
MatrixHomogeneous &res, int timeCurr) { |
591 |
|
|
sotDEBUGIN(15); |
592 |
|
|
|
593 |
|
|
const MatrixHomogeneous &wMsf = leftFootPositionSIN(timeCurr); |
594 |
|
|
|
595 |
|
|
sotDEBUGOUT(15); |
596 |
|
|
return computeRefPos(res, timeCurr, wMsf); |
597 |
|
|
} |
598 |
|
|
|
599 |
|
|
MatrixHomogeneous &NextStepTwoHandObserver::computeReferencePositionRight( |
600 |
|
|
MatrixHomogeneous &res, int timeCurr) { |
601 |
|
|
sotDEBUGIN(15); |
602 |
|
|
|
603 |
|
|
const MatrixHomogeneous &wMsf = rightFootPositionSIN(timeCurr); |
604 |
|
|
|
605 |
|
|
sotDEBUGOUT(15); |
606 |
|
|
return computeRefPos(res, timeCurr, wMsf); |
607 |
|
|
} |
608 |
|
|
|
609 |
|
|
Vector &NextStepTwoHandObserver::computeReferenceVelocity(const Vector &, |
610 |
|
|
const Vector &, |
611 |
|
|
Vector &res) { |
612 |
|
|
sotDEBUGIN(15); |
613 |
|
|
|
614 |
|
|
/* TODO */ |
615 |
|
|
|
616 |
|
|
sotDEBUGOUT(15); |
617 |
|
|
return res; |
618 |
|
|
} |
619 |
|
|
|
620 |
|
|
Vector &NextStepTwoHandObserver::computeReferenceAcceleration(const Vector &, |
621 |
|
|
const Vector &, |
622 |
|
|
Vector &res) { |
623 |
|
|
sotDEBUGIN(15); |
624 |
|
|
|
625 |
|
|
/* TODO */ |
626 |
|
|
|
627 |
|
|
sotDEBUGOUT(15); |
628 |
|
|
return res; |
629 |
|
|
} |
630 |
|
|
|
631 |
|
|
} // namespace sot |
632 |
|
|
} // namespace dynamicgraph |