1 /*
2 * Copyright (c) 2011-2021, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include "Controller.hpp"
34
35 #include "State.hpp"
36 #include "StateMachine.hpp"
37 #include "TerminalCondition.hpp"
38
39 using namespace std;
40
41 using namespace Eigen;
42
43 using namespace dart;
44 using namespace math;
45 using namespace constraint;
46 using namespace dynamics;
47
48 //==============================================================================
Controller(SkeletonPtr _atlasRobot,ConstraintSolver * _collisionSolver)49 Controller::Controller(
50 SkeletonPtr _atlasRobot, ConstraintSolver* _collisionSolver)
51 : mAtlasRobot(_atlasRobot),
52 mConstratinSolver(_collisionSolver),
53 mCurrentStateMachine(nullptr),
54 mPelvisHarnessOn(false),
55 mLeftFootHarnessOn(false),
56 mRightFootHarnessOn(false),
57 mMinPelvisHeight(-0.70),
58 mMaxPelvisHeight(0.30),
59 mWeldJointConstraintPelvis(nullptr),
60 mWeldJointConstraintLeftFoot(nullptr),
61 mWeldJointConstraintRightFoot(nullptr),
62 mVerbosity(false)
63 {
64 mCoronalLeftHip = mAtlasRobot->getDof("l_leg_hpx")->getIndexInSkeleton();
65 mCoronalRightHip = mAtlasRobot->getDof("r_leg_hpx")->getIndexInSkeleton();
66 mSagitalLeftHip = mAtlasRobot->getDof("l_leg_hpy")->getIndexInSkeleton();
67 mSagitalRightHip = mAtlasRobot->getDof("r_leg_hpy")->getIndexInSkeleton();
68
69 _buildStateMachines();
70 _setJointDamping();
71
72 // harnessPelvis();
73 // harnessLeftFoot();
74 // harnessRightFoot();
75
76 mInitialState = mAtlasRobot->getConfiguration(
77 Skeleton::CONFIG_POSITIONS | Skeleton::CONFIG_VELOCITIES);
78 }
79
80 //==============================================================================
~Controller()81 Controller::~Controller()
82 {
83 for (vector<StateMachine*>::iterator it = mStateMachines.begin();
84 it != mStateMachines.end();
85 ++it)
86 {
87 delete *it;
88 }
89 }
90
91 //==============================================================================
update()92 void Controller::update()
93 {
94 if (isAllowingControl())
95 {
96 // Compute control force
97 mCurrentStateMachine->computeControlForce(mAtlasRobot->getTimeStep());
98 }
99 }
100
101 //==============================================================================
getAtlasRobot()102 SkeletonPtr Controller::getAtlasRobot()
103 {
104 return mAtlasRobot;
105 }
106
107 //==============================================================================
getCurrentState()108 StateMachine* Controller::getCurrentState()
109 {
110 return mCurrentStateMachine;
111 }
112
113 //==============================================================================
changeStateMachine(StateMachine * _stateMachine,double _currentTime)114 void Controller::changeStateMachine(
115 StateMachine* _stateMachine, double _currentTime)
116 {
117 assert(
118 _containStateMachine(_stateMachine)
119 && "_stateMachine should be in mStateMachines");
120
121 if (mCurrentStateMachine == _stateMachine)
122 {
123 return;
124 }
125
126 string prevName = mCurrentStateMachine->getName();
127 string nextName = _stateMachine->getName();
128
129 // Finish current state
130 mCurrentStateMachine->end(_currentTime);
131
132 // Transite to _state
133 mCurrentStateMachine = _stateMachine;
134 mCurrentStateMachine->begin(_currentTime);
135
136 if (mVerbosity)
137 {
138 dtmsg << "State machine transition: from [" << prevName << "] to ["
139 << nextName << "]." << endl;
140 }
141 }
142
143 //==============================================================================
changeStateMachine(const string & _name,double _currentTime)144 void Controller::changeStateMachine(const string& _name, double _currentTime)
145 {
146 // _state should be in mStates
147 StateMachine* stateMachine = _findStateMachine(_name);
148
149 assert(stateMachine != nullptr && "Invaild state machine.");
150
151 changeStateMachine(stateMachine, _currentTime);
152 }
153
154 //==============================================================================
changeStateMachine(std::size_t _idx,double _currentTime)155 void Controller::changeStateMachine(std::size_t _idx, double _currentTime)
156 {
157 assert(_idx <= mStateMachines.size() && "Invalid index of StateMachine.");
158
159 changeStateMachine(mStateMachines[_idx], _currentTime);
160 }
161
162 //==============================================================================
isAllowingControl() const163 bool Controller::isAllowingControl() const
164 {
165 auto pelvis = mAtlasRobot->getBodyNode("pelvis");
166 const Eigen::Isometry3d tf = pelvis->getTransform();
167 const Eigen::Vector3d pos = tf.translation();
168 const auto y = pos[1];
169
170 if (y < mMinPelvisHeight || mMaxPelvisHeight < y)
171 return false;
172 else
173 return true;
174 }
175
176 //==============================================================================
keyboard(unsigned char _key,int,int,double _currentTime)177 void Controller::keyboard(
178 unsigned char _key, int /*_x*/, int /*_y*/, double _currentTime)
179 {
180 switch (_key)
181 {
182 case 'h': // Harness pelvis toggle
183 if (mPelvisHarnessOn)
184 unharnessPelvis();
185 else
186 harnessPelvis();
187 break;
188 case 'j': // Harness left foot toggle
189 if (mLeftFootHarnessOn)
190 unharnessLeftFoot();
191 else
192 harnessLeftFoot();
193 break;
194 case 'k': // Harness right foot toggle
195 if (mRightFootHarnessOn)
196 unharnessRightFoot();
197 else
198 harnessRightFoot();
199 break;
200 case 'r': // Reset robot
201 resetRobot();
202 break;
203 case 'n': // Transite to the next state manually
204 mCurrentStateMachine->transiteToNextState(_currentTime);
205 break;
206 case '1': // Standing controller
207 changeStateMachine("standing", _currentTime);
208 break;
209 case '2': // Walking in place controller
210 changeStateMachine("walking in place", _currentTime);
211 break;
212 case '3':
213 changeStateMachine("walking", _currentTime);
214 break;
215 case '4':
216 changeStateMachine("running", _currentTime);
217 break;
218
219 default:
220 break;
221 }
222 }
223
224 //==============================================================================
printDebugInfo() const225 void Controller::printDebugInfo() const
226 {
227 std::cout << "[ATLAS Robot]" << std::endl
228 << " NUM NODES : " << mAtlasRobot->getNumBodyNodes() << std::endl
229 << " NUM DOF : " << mAtlasRobot->getNumDofs() << std::endl
230 << " NUM JOINTS: " << mAtlasRobot->getNumBodyNodes() << std::endl;
231
232 for (std::size_t i = 0; i < mAtlasRobot->getNumBodyNodes(); ++i)
233 {
234 Joint* joint = mAtlasRobot->getJoint(i);
235 BodyNode* body = mAtlasRobot->getBodyNode(i);
236 BodyNode* parentBody = mAtlasRobot->getBodyNode(i)->getParentBodyNode();
237
238 std::cout << " Joint [" << i << "]: " << joint->getName() << " ("
239 << joint->getNumDofs() << ")" << std::endl;
240 if (parentBody != nullptr)
241 {
242 std::cout << " Parent body: " << parentBody->getName() << std::endl;
243 }
244
245 std::cout << " Child body : " << body->getName() << std::endl;
246 }
247 }
248
249 //==============================================================================
harnessPelvis()250 void Controller::harnessPelvis()
251 {
252 if (mPelvisHarnessOn)
253 return;
254
255 BodyNode* bd = mAtlasRobot->getBodyNode("pelvis");
256 mWeldJointConstraintPelvis = std::make_shared<WeldJointConstraint>(bd);
257 mConstratinSolver->addConstraint(mWeldJointConstraintPelvis);
258 mPelvisHarnessOn = true;
259
260 if (mVerbosity)
261 dtmsg << "Pelvis is harnessed." << std::endl;
262 }
263
264 //==============================================================================
unharnessPelvis()265 void Controller::unharnessPelvis()
266 {
267 if (!mPelvisHarnessOn)
268 return;
269
270 mConstratinSolver->removeConstraint(mWeldJointConstraintPelvis);
271 mPelvisHarnessOn = false;
272
273 if (mVerbosity)
274 dtmsg << "Pelvis is unharnessed." << std::endl;
275 }
276
277 //==============================================================================
harnessLeftFoot()278 void Controller::harnessLeftFoot()
279 {
280 if (mLeftFootHarnessOn)
281 return;
282
283 BodyNode* bd = mAtlasRobot->getBodyNode("l_foot");
284 mWeldJointConstraintLeftFoot = std::make_shared<WeldJointConstraint>(bd);
285 mLeftFootHarnessOn = true;
286
287 if (mVerbosity)
288 dtmsg << "Left foot is harnessed." << std::endl;
289 }
290
291 //==============================================================================
unharnessLeftFoot()292 void Controller::unharnessLeftFoot()
293 {
294 if (!mLeftFootHarnessOn)
295 return;
296
297 mConstratinSolver->removeConstraint(mWeldJointConstraintLeftFoot);
298 mLeftFootHarnessOn = false;
299
300 if (mVerbosity)
301 dtmsg << "Left foot is unharnessed." << std::endl;
302 }
303
304 //==============================================================================
harnessRightFoot()305 void Controller::harnessRightFoot()
306 {
307 if (mRightFootHarnessOn)
308 return;
309
310 BodyNode* bd = mAtlasRobot->getBodyNode("r_foot");
311 mWeldJointConstraintRightFoot = std::make_shared<WeldJointConstraint>(bd);
312 mRightFootHarnessOn = true;
313
314 if (mVerbosity)
315 dtmsg << "Right foot is harnessed." << std::endl;
316 }
317
318 //==============================================================================
unharnessRightFoot()319 void Controller::unharnessRightFoot()
320 {
321 if (!mRightFootHarnessOn)
322 return;
323
324 mConstratinSolver->removeConstraint(mWeldJointConstraintRightFoot);
325 mRightFootHarnessOn = false;
326
327 if (mVerbosity)
328 dtmsg << "Right foot is unharnessed." << std::endl;
329 }
330
331 //==============================================================================
resetRobot()332 void Controller::resetRobot()
333 {
334 mAtlasRobot->setConfiguration(mInitialState);
335
336 if (mVerbosity)
337 dtmsg << "Robot is reset." << std::endl;
338 }
339
340 //==============================================================================
setVerbosity(bool verbosity)341 void Controller::setVerbosity(bool verbosity)
342 {
343 mVerbosity = verbosity;
344 }
345
346 //==============================================================================
_buildStateMachines()347 void Controller::_buildStateMachines()
348 {
349 // Standing controller
350 mStateMachines.push_back(_createStandingStateMachine());
351
352 // Walking in place controller
353 mStateMachines.push_back(_createWalkingInPlaceStateMachine());
354
355 // Walking controller
356 mStateMachines.push_back(_createWalkingStateMachine());
357
358 // Walking controller
359 mStateMachines.push_back(_createRunningStateMachine());
360
361 // Set initial (default) controller
362 mCurrentStateMachine = mStateMachines[1]; // Standing controller
363
364 // Begin the default controller
365 mCurrentStateMachine->begin(0.0);
366 }
367
368 //==============================================================================
_createStandingStateMachine()369 StateMachine* Controller::_createStandingStateMachine()
370 {
371 using namespace dart::math::suffixes;
372
373 StateMachine* standing = new StateMachine("standing");
374
375 State* standingState0 = new State(mAtlasRobot, "0");
376
377 TerminalCondition* tcStanding0 = new TimerCondition(standingState0, 0.3);
378
379 standingState0->setTerminalCondition(tcStanding0);
380
381 standingState0->setNextState(standingState0);
382
383 standingState0->setDesiredJointPosition(
384 "back_bky", 15.00_deg); // angle b/w pelvis and torso
385 standingState0->setDesiredJointPosition("l_leg_hpy", -10.00_deg);
386 standingState0->setDesiredJointPosition("r_leg_hpy", -10.00_deg);
387 standingState0->setDesiredJointPosition("l_leg_kny", 30.00_deg); // left knee
388 standingState0->setDesiredJointPosition("r_leg_kny", 30.00_deg); // right knee
389 standingState0->setDesiredJointPosition(
390 "l_leg_aky", -16.80_deg); // left ankle
391 standingState0->setDesiredJointPosition(
392 "r_leg_aky", -16.80_deg); // right ankle
393
394 standingState0->setDesiredJointPosition(
395 "l_arm_shx", -90.0_deg); // right ankle
396 standingState0->setDesiredJointPosition(
397 "r_arm_shx", +90.0_deg); // right ankle
398
399 standing->addState(standingState0);
400
401 standing->setInitialState(standingState0);
402
403 return standing;
404 }
405
406 //==============================================================================
_createWalkingInPlaceStateMachine()407 StateMachine* Controller::_createWalkingInPlaceStateMachine()
408 {
409 using namespace dart::math::suffixes;
410
411 const double cd = 0.5;
412 const double cv = 0.2;
413
414 const double pelvis = -4.75_deg; // angle b/w pelvis and torso
415
416 const double swh02 = 0.50; // swing hip
417 const double swk02 = -1.10; // swing knee
418 const double swa02 = 0.60; // swing angle
419 const double stk02 = -0.05; // stance knee
420 const double sta02 = 0.00; // stance ankle
421
422 const double swh13 = -0.10; // swing hip
423 const double swk13 = -0.05; // swing knee
424 const double swa13 = 0.15; // swing angle
425 const double stk13 = -0.10; // stance knee
426 const double sta13 = 0.00; // stance ankle
427
428 StateMachine* sm = new StateMachine("walking in place");
429
430 State* state0 = new State(mAtlasRobot, "0");
431 State* state1 = new State(mAtlasRobot, "1");
432 State* state2 = new State(mAtlasRobot, "2");
433 State* state3 = new State(mAtlasRobot, "3");
434
435 TerminalCondition* cond0 = new TimerCondition(state0, 0.3);
436 TerminalCondition* cond1 = new BodyContactCondition(state1, _getRightFoot());
437 TerminalCondition* cond2 = new TimerCondition(state2, 0.3);
438 TerminalCondition* cond3 = new BodyContactCondition(state3, _getLeftFoot());
439
440 state0->setTerminalCondition(cond0);
441 state1->setTerminalCondition(cond1);
442 state2->setTerminalCondition(cond2);
443 state3->setTerminalCondition(cond3);
444
445 state0->setNextState(state1);
446 state1->setNextState(state2);
447 state2->setNextState(state3);
448 state3->setNextState(state0);
449
450 // Set stance foot
451 state0->setStanceFootToLeftFoot();
452 state1->setStanceFootToLeftFoot();
453 state2->setStanceFootToRightFoot();
454 state3->setStanceFootToRightFoot();
455
456 // Set global desired pelvis angle
457 state0->setDesiredPelvisGlobalAngleOnSagital(0.0_deg);
458 state1->setDesiredPelvisGlobalAngleOnSagital(0.0_deg);
459 state2->setDesiredPelvisGlobalAngleOnSagital(0.0_deg);
460 state3->setDesiredPelvisGlobalAngleOnSagital(0.0_deg);
461 state0->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg);
462 state1->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg);
463 state2->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg);
464 state3->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg);
465
466 // Set desired joint position
467 //-- State 0
468 //---- pelvis
469 state0->setDesiredJointPosition(
470 "back_bky", -pelvis); // angle b/w pelvis and torso
471 //---- swing leg
472 state0->setDesiredJointPosition("r_leg_hpy", -swh02); // right hip
473 state0->setDesiredJointPosition("r_leg_kny", -swk02); // right knee
474 state0->setDesiredJointPosition("r_leg_aky", -swa02); // right ankle
475 //---- stance leg
476 state0->setDesiredJointPosition("l_leg_kny", -stk02); // left knee
477 state0->setDesiredJointPosition("l_leg_aky", -sta02); // left ankle
478 //---- arm
479 state0->setDesiredJointPosition("l_arm_shy", -20.00_deg); // left arm
480 state0->setDesiredJointPosition("r_arm_shy", +10.00_deg); // right arm
481 state0->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm
482 state0->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm
483 //---- feedback gain for hip joints
484 state0->setFeedbackCoronalCOMDistance(
485 mCoronalLeftHip, -cd); // coronal left hip
486 state0->setFeedbackCoronalCOMVelocity(
487 mCoronalLeftHip, -cv); // coronal left hip
488 state0->setFeedbackCoronalCOMDistance(
489 mCoronalRightHip, -cd); // coronal right hip
490 state0->setFeedbackCoronalCOMVelocity(
491 mCoronalRightHip, -cv); // coronal right hip
492 state0->setFeedbackSagitalCOMDistance(
493 mSagitalLeftHip, -cd); // sagital left hip
494 state0->setFeedbackSagitalCOMVelocity(
495 mSagitalLeftHip, -cv); // sagital left hip
496 state0->setFeedbackSagitalCOMDistance(
497 mSagitalRightHip, -cd); // sagital right hip
498 state0->setFeedbackSagitalCOMVelocity(
499 mSagitalRightHip, -cv); // sagital right hip
500
501 //-- State 1
502 //---- pelvis
503 state1->setDesiredJointPosition(
504 "back_bky", -pelvis); // angle b/w pelvis and torso
505 //---- swing leg
506 state1->setDesiredJointPosition("l_leg_hpy", -swh13); // left hip
507 state1->setDesiredJointPosition("l_leg_kny", -swk13); // left knee
508 state1->setDesiredJointPosition("l_leg_aky", -swa13); // left ankle
509 //---- stance leg
510 state1->setDesiredJointPosition("r_leg_kny", -stk13); // right knee
511 state1->setDesiredJointPosition("r_leg_aky", -sta13); // right ankle
512 //---- arm
513 state1->setDesiredJointPosition("l_arm_shy", +10.00_deg); // left arm
514 state1->setDesiredJointPosition("r_arm_shy", -20.00_deg); // right arm
515 state1->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm
516 state1->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm
517 //---- feedback gain for hip joints
518 state1->setFeedbackCoronalCOMDistance(
519 mCoronalLeftHip, -cd); // coronal left hip
520 state1->setFeedbackCoronalCOMVelocity(
521 mCoronalLeftHip, -cv); // coronal left hip
522 state1->setFeedbackCoronalCOMDistance(
523 mCoronalRightHip, -cd); // coronal right hip
524 state1->setFeedbackCoronalCOMVelocity(
525 mCoronalRightHip, -cv); // coronal right hip
526 state1->setFeedbackSagitalCOMDistance(
527 mSagitalLeftHip, -cd); // sagital left hip
528 state1->setFeedbackSagitalCOMVelocity(
529 mSagitalLeftHip, -cv); // sagital left hip
530 state1->setFeedbackSagitalCOMDistance(
531 mSagitalRightHip, -cd); // sagital right hip
532 state1->setFeedbackSagitalCOMVelocity(
533 mSagitalRightHip, -cv); // sagital right hip
534
535 //-- State 2
536 //---- pelvis
537 state2->setDesiredJointPosition(
538 "back_bky", -pelvis); // angle b/w pelvis and torso
539 //---- swing leg
540 state2->setDesiredJointPosition("l_leg_hpy", -swh02); // left hip
541 state2->setDesiredJointPosition("l_leg_kny", -swk02); // left knee
542 state2->setDesiredJointPosition("l_leg_aky", -swa02); // left ankle
543 //---- stance leg
544 state2->setDesiredJointPosition("r_leg_kny", -stk02); // right knee
545 state2->setDesiredJointPosition("r_leg_aky", -sta02); // right ankle
546 //---- arm
547 state2->setDesiredJointPosition("l_arm_shy", +10.00_deg); // left arm
548 state2->setDesiredJointPosition("r_arm_shy", -20.00_deg); // right arm
549 state2->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm
550 state2->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm
551 //---- feedback gain for hip joints
552 state2->setFeedbackCoronalCOMDistance(
553 mCoronalLeftHip, -cd); // coronal left hip
554 state2->setFeedbackCoronalCOMVelocity(
555 mCoronalLeftHip, -cv); // coronal left hip
556 state2->setFeedbackCoronalCOMDistance(
557 mCoronalRightHip, -cd); // coronal right hip
558 state2->setFeedbackCoronalCOMVelocity(
559 mCoronalRightHip, -cv); // coronal right hip
560 state2->setFeedbackSagitalCOMDistance(
561 mSagitalLeftHip, -cd); // sagital left hip
562 state2->setFeedbackSagitalCOMVelocity(
563 mSagitalLeftHip, -cv); // sagital left hip
564 state2->setFeedbackSagitalCOMDistance(
565 mSagitalRightHip, -cd); // sagital right hip
566 state2->setFeedbackSagitalCOMVelocity(
567 mSagitalRightHip, -cv); // sagital right hip
568
569 //-- State 3
570 //---- pelvis
571 state3->setDesiredJointPosition(
572 "back_bky", -pelvis); // angle b/w pelvis and torso
573 //---- swing leg
574 state3->setDesiredJointPosition("r_leg_hpy", -swh13); // right hip
575 state3->setDesiredJointPosition("r_leg_kny", -swk13); // right knee
576 state3->setDesiredJointPosition("r_leg_aky", -swa13); // right ankle
577 //---- stance leg
578 state3->setDesiredJointPosition("l_leg_kny", -stk13); // left knee
579 state3->setDesiredJointPosition("l_leg_aky", -sta13); // left ankle
580 //---- arm
581 state3->setDesiredJointPosition("l_arm_shy", -20.00_deg); // left arm
582 state3->setDesiredJointPosition("r_arm_shy", +10.00_deg); // right arm
583 state3->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm
584 state3->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm
585 //---- feedback gain for hip joints
586 state3->setFeedbackCoronalCOMDistance(
587 mCoronalLeftHip, -cd); // coronal left hip
588 state3->setFeedbackCoronalCOMVelocity(
589 mCoronalLeftHip, -cv); // coronal left hip
590 state3->setFeedbackCoronalCOMDistance(
591 mCoronalRightHip, -cd); // coronal right hip
592 state3->setFeedbackCoronalCOMVelocity(
593 mCoronalRightHip, -cv); // coronal right hip
594 state3->setFeedbackSagitalCOMDistance(
595 mSagitalLeftHip, -cd); // sagital left hip
596 state3->setFeedbackSagitalCOMVelocity(
597 mSagitalLeftHip, -cv); // sagital left hip
598 state3->setFeedbackSagitalCOMDistance(
599 mSagitalRightHip, -cd); // sagital right hip
600 state3->setFeedbackSagitalCOMVelocity(
601 mSagitalRightHip, -cv); // sagital right hip
602
603 sm->addState(state0);
604 sm->addState(state1);
605 sm->addState(state2);
606 sm->addState(state3);
607
608 sm->setInitialState(state1);
609
610 return sm;
611 }
612
613 //==============================================================================
_createWalkingStateMachine()614 StateMachine* Controller::_createWalkingStateMachine()
615 {
616 using namespace dart::math::suffixes;
617
618 const double cd = 0.5;
619 const double cv = 0.2;
620
621 const double pelvis = -10.0_deg; // angle b/w pelvis and torso
622
623 const double swh02 = 0.50; // swing hip
624 const double swk02 = -1.10; // swing knee
625 const double swa02 = 0.60; // swing angle
626 const double stk02 = -0.05; // stance knee
627 const double sta02 = 0.00; // stance ankle
628
629 const double swh13 = -0.10; // swing hip
630 const double swk13 = -0.05; // swing knee
631 const double swa13 = 0.15; // swing angle
632 const double stk13 = -0.10; // stance knee
633 const double sta13 = 0.00; // stance ankle
634
635 StateMachine* sm = new StateMachine("walking");
636
637 State* state0 = new State(mAtlasRobot, "0");
638 State* state1 = new State(mAtlasRobot, "1");
639 State* state2 = new State(mAtlasRobot, "2");
640 State* state3 = new State(mAtlasRobot, "3");
641
642 TerminalCondition* cond0 = new TimerCondition(state0, 0.3);
643 TerminalCondition* cond1 = new BodyContactCondition(state1, _getRightFoot());
644 TerminalCondition* cond2 = new TimerCondition(state2, 0.3);
645 TerminalCondition* cond3 = new BodyContactCondition(state3, _getLeftFoot());
646
647 state0->setTerminalCondition(cond0);
648 state1->setTerminalCondition(cond1);
649 state2->setTerminalCondition(cond2);
650 state3->setTerminalCondition(cond3);
651
652 state0->setNextState(state1);
653 state1->setNextState(state2);
654 state2->setNextState(state3);
655 state3->setNextState(state0);
656
657 // Set stance foot
658 state0->setStanceFootToLeftFoot();
659 state1->setStanceFootToLeftFoot();
660 state2->setStanceFootToRightFoot();
661 state3->setStanceFootToRightFoot();
662
663 // Set global desired pelvis angle
664 state0->setDesiredPelvisGlobalAngleOnSagital(0.0_deg);
665 state1->setDesiredPelvisGlobalAngleOnSagital(0.0_deg);
666 state2->setDesiredPelvisGlobalAngleOnSagital(0.0_deg);
667 state3->setDesiredPelvisGlobalAngleOnSagital(0.0_deg);
668 state0->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg);
669 state1->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg);
670 state2->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg);
671 state3->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg);
672
673 // Set desired joint position
674 //-- State 0
675 //---- pelvis
676 state0->setDesiredJointPosition(
677 "back_bky", -pelvis); // angle b/w pelvis and torso
678 //---- swing leg
679 state0->setDesiredJointPosition("r_leg_hpy", -swh02); // right hip
680 state0->setDesiredJointPosition("r_leg_kny", -swk02); // right knee
681 state0->setDesiredJointPosition("r_leg_aky", -swa02); // right ankle
682 //---- stance leg
683 state0->setDesiredJointPosition("l_leg_kny", -stk02); // left knee
684 state0->setDesiredJointPosition("l_leg_aky", -sta02); // left ankle
685 //---- arm
686 state0->setDesiredJointPosition("l_arm_shy", -20.00_deg); // left arm
687 state0->setDesiredJointPosition("r_arm_shy", +10.00_deg); // right arm
688 state0->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm
689 state0->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm
690 //---- feedback gain for hip joints
691 state0->setFeedbackCoronalCOMDistance(
692 mCoronalLeftHip, -cd); // coronal left hip
693 state0->setFeedbackCoronalCOMVelocity(
694 mCoronalLeftHip, -cv); // coronal left hip
695 state0->setFeedbackCoronalCOMDistance(
696 mCoronalRightHip, -cd); // coronal right hip
697 state0->setFeedbackCoronalCOMVelocity(
698 mCoronalRightHip, -cv); // coronal right hip
699 state0->setFeedbackSagitalCOMDistance(
700 mSagitalLeftHip, -cd); // sagital left hip
701 state0->setFeedbackSagitalCOMVelocity(
702 mSagitalLeftHip, -cv); // sagital left hip
703 state0->setFeedbackSagitalCOMDistance(
704 mSagitalRightHip, -cd); // sagital right hip
705 state0->setFeedbackSagitalCOMVelocity(
706 mSagitalRightHip, -cv); // sagital right hip
707
708 //-- State 1
709 //---- pelvis
710 state1->setDesiredJointPosition(
711 "back_bky", -pelvis); // angle b/w pelvis and torso
712 //---- swing leg
713 state1->setDesiredJointPosition("l_leg_hpy", -swh13); // left hip
714 state1->setDesiredJointPosition("l_leg_kny", -swk13); // left knee
715 state1->setDesiredJointPosition("l_leg_aky", -swa13); // left ankle
716 //---- stance leg
717 state1->setDesiredJointPosition("r_leg_kny", -stk13); // right knee
718 state1->setDesiredJointPosition("r_leg_aky", -sta13); // right ankle
719 //---- arm
720 state1->setDesiredJointPosition("l_arm_shy", +10.00_deg); // left arm
721 state1->setDesiredJointPosition("r_arm_shy", -20.00_deg); // right arm
722 state1->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm
723 state1->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm
724 //---- feedback gain for hip joints
725 state1->setFeedbackCoronalCOMDistance(
726 mCoronalLeftHip, -cd); // coronal left hip
727 state1->setFeedbackCoronalCOMVelocity(
728 mCoronalLeftHip, -cv); // coronal left hip
729 state1->setFeedbackCoronalCOMDistance(
730 mCoronalRightHip, -cd); // coronal right hip
731 state1->setFeedbackCoronalCOMVelocity(
732 mCoronalRightHip, -cv); // coronal right hip
733 state1->setFeedbackSagitalCOMDistance(
734 mSagitalLeftHip, -cd); // sagital left hip
735 state1->setFeedbackSagitalCOMVelocity(
736 mSagitalLeftHip, -cv); // sagital left hip
737 state1->setFeedbackSagitalCOMDistance(
738 mSagitalRightHip, -cd); // sagital right hip
739 state1->setFeedbackSagitalCOMVelocity(
740 mSagitalRightHip, -cv); // sagital right hip
741
742 //-- State 2
743 //---- pelvis
744 state2->setDesiredJointPosition(
745 "back_bky", -pelvis); // angle b/w pelvis and torso
746 //---- swing leg
747 state2->setDesiredJointPosition("l_leg_hpy", -swh02); // left hip
748 state2->setDesiredJointPosition("l_leg_kny", -swk02); // left knee
749 state2->setDesiredJointPosition("l_leg_aky", -swa02); // left ankle
750 //---- stance leg
751 state2->setDesiredJointPosition("r_leg_kny", -stk02); // right knee
752 state2->setDesiredJointPosition("r_leg_aky", -sta02); // right ankle
753 //---- arm
754 state2->setDesiredJointPosition("l_arm_shy", +10.00_deg); // left arm
755 state2->setDesiredJointPosition("r_arm_shy", -20.00_deg); // right arm
756 state2->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm
757 state2->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm
758 //---- feedback gain for hip joints
759 state2->setFeedbackCoronalCOMDistance(
760 mCoronalLeftHip, -cd); // coronal left hip
761 state2->setFeedbackCoronalCOMVelocity(
762 mCoronalLeftHip, -cv); // coronal left hip
763 state2->setFeedbackCoronalCOMDistance(
764 mCoronalRightHip, -cd); // coronal right hip
765 state2->setFeedbackCoronalCOMVelocity(
766 mCoronalRightHip, -cv); // coronal right hip
767 state2->setFeedbackSagitalCOMDistance(
768 mSagitalLeftHip, -cd); // sagital left hip
769 state2->setFeedbackSagitalCOMVelocity(
770 mSagitalLeftHip, -cv); // sagital left hip
771 state2->setFeedbackSagitalCOMDistance(
772 mSagitalRightHip, -cd); // sagital right hip
773 state2->setFeedbackSagitalCOMVelocity(
774 mSagitalRightHip, -cv); // sagital right hip
775
776 //-- State 3
777 //---- pelvis
778 state3->setDesiredJointPosition(
779 "back_bky", -pelvis); // angle b/w pelvis and torso
780 //---- swing leg
781 state3->setDesiredJointPosition("r_leg_hpy", -swh13); // right hip
782 state3->setDesiredJointPosition("r_leg_kny", -swk13); // right knee
783 state3->setDesiredJointPosition("r_leg_aky", -swa13); // right ankle
784 //---- stance leg
785 state3->setDesiredJointPosition("l_leg_kny", -stk13); // left knee
786 state3->setDesiredJointPosition("l_leg_aky", -sta13); // left ankle
787 //---- arm
788 state3->setDesiredJointPosition("l_arm_shy", -20.00_deg); // left arm
789 state3->setDesiredJointPosition("r_arm_shy", +10.00_deg); // right arm
790 state3->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm
791 state3->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm
792 //---- feedback gain for hip joints
793 state3->setFeedbackCoronalCOMDistance(
794 mCoronalLeftHip, -cd); // coronal left hip
795 state3->setFeedbackCoronalCOMVelocity(
796 mCoronalLeftHip, -cv); // coronal left hip
797 state3->setFeedbackCoronalCOMDistance(
798 mCoronalRightHip, -cd); // coronal right hip
799 state3->setFeedbackCoronalCOMVelocity(
800 mCoronalRightHip, -cv); // coronal right hip
801 state3->setFeedbackSagitalCOMDistance(
802 mSagitalLeftHip, -cd); // sagital left hip
803 state3->setFeedbackSagitalCOMVelocity(
804 mSagitalLeftHip, -cv); // sagital left hip
805 state3->setFeedbackSagitalCOMDistance(
806 mSagitalRightHip, -cd); // sagital right hip
807 state3->setFeedbackSagitalCOMVelocity(
808 mSagitalRightHip, -cv); // sagital right hip
809
810 sm->addState(state0);
811 sm->addState(state1);
812 sm->addState(state2);
813 sm->addState(state3);
814
815 sm->setInitialState(state1);
816
817 return sm;
818 }
819
820 //==============================================================================
_createRunningStateMachine()821 StateMachine* Controller::_createRunningStateMachine()
822 {
823 using namespace dart::math::suffixes;
824
825 const double cd = 0.5;
826 const double cv = 0.2;
827
828 const double pelvis = -10.0_deg; // angle b/w pelvis and torso
829
830 const double swh01 = 0.50; // swing hip
831 const double swk01 = -1.10; // swing knee
832 const double swa01 = 0.60; // swing angle
833 const double stk01 = -0.05; // stance knee
834 const double sta01 = 0.00; // stance ankle
835
836 StateMachine* sm = new StateMachine("running");
837
838 State* state0 = new State(mAtlasRobot, "0");
839 State* state1 = new State(mAtlasRobot, "1");
840
841 TerminalCondition* cond0 = new TimerCondition(state0, 0.15);
842 TerminalCondition* cond1 = new TimerCondition(state1, 0.15);
843
844 state0->setTerminalCondition(cond0);
845 state1->setTerminalCondition(cond1);
846
847 state0->setNextState(state1);
848 state1->setNextState(state0);
849
850 // Set stance foot
851 state0->setStanceFootToLeftFoot();
852 state1->setStanceFootToRightFoot();
853
854 // Set global desired pelvis angle
855 state0->setDesiredPelvisGlobalAngleOnSagital(0.0_deg);
856 state1->setDesiredPelvisGlobalAngleOnSagital(0.0_deg);
857 state0->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg);
858 state1->setDesiredPelvisGlobalAngleOnCoronal(0.0_deg);
859
860 // Set desired joint position
861 //-- State 0
862 //---- pelvis
863 state0->setDesiredJointPosition(
864 "back_bky", -pelvis); // angle b/w pelvis and torso
865 //---- swing leg
866 state0->setDesiredJointPosition("r_leg_hpy", -swh01); // right hip
867 state0->setDesiredJointPosition("r_leg_kny", -swk01); // right knee
868 state0->setDesiredJointPosition("r_leg_aky", -swa01); // right ankle
869 //---- stance leg
870 state0->setDesiredJointPosition("l_leg_kny", -stk01); // left knee
871 state0->setDesiredJointPosition("l_leg_aky", -sta01); // left ankle
872 //---- arm
873 state0->setDesiredJointPosition("l_arm_shy", -45.00_deg); // left arm
874 state0->setDesiredJointPosition("r_arm_shy", +15.00_deg); // right arm
875 state0->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm
876 state0->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm
877 // state0->setDesiredJointPosition(23, DART_RADIAN * +90.00); // left arm
878 // state0->setDesiredJointPosition(24, DART_RADIAN * +90.00); // right arm
879 // state0->setDesiredJointPosition(27, DART_RADIAN * +90.00); // left arm
880 // state0->setDesiredJointPosition(28, DART_RADIAN * -90.00); // right arm
881 //---- feedback gain for hip joints
882 state0->setFeedbackCoronalCOMDistance(
883 mCoronalLeftHip, -cd); // coronal left hip
884 state0->setFeedbackCoronalCOMVelocity(
885 mCoronalLeftHip, -cv); // coronal left hip
886 state0->setFeedbackCoronalCOMDistance(
887 mCoronalRightHip, -cd); // coronal right hip
888 state0->setFeedbackCoronalCOMVelocity(
889 mCoronalRightHip, -cv); // coronal right hip
890 state0->setFeedbackSagitalCOMDistance(
891 mSagitalLeftHip, -cd); // sagital left hip
892 state0->setFeedbackSagitalCOMVelocity(
893 mSagitalLeftHip, -cv); // sagital left hip
894 state0->setFeedbackSagitalCOMDistance(
895 mSagitalRightHip, -cd); // sagital right hip
896 state0->setFeedbackSagitalCOMVelocity(
897 mSagitalRightHip, -cv); // sagital right hip
898
899 //-- State 2
900 //---- pelvis
901 state1->setDesiredJointPosition(
902 "back_bky", -pelvis); // angle b/w pelvis and torso
903 //---- swing leg
904 state1->setDesiredJointPosition("l_leg_hpy", -swh01); // left hip
905 state1->setDesiredJointPosition("l_leg_kny", -swk01); // left knee
906 state1->setDesiredJointPosition("l_leg_aky", -swa01); // left ankle
907 //---- stance leg
908 state1->setDesiredJointPosition("r_leg_kny", -stk01); // right knee
909 state1->setDesiredJointPosition("r_leg_aky", -sta01); // right ankle
910 //---- arm
911 state1->setDesiredJointPosition("l_arm_shy", +15.00_deg); // left arm
912 state1->setDesiredJointPosition("r_arm_shy", -45.00_deg); // right arm
913 state1->setDesiredJointPosition("l_arm_shx", -80.00_deg); // left arm
914 state1->setDesiredJointPosition("r_arm_shx", +80.00_deg); // right arm
915 // state1->setDesiredJointPosition(23, DART_RADIAN * +90.00); // left arm
916 // state1->setDesiredJointPosition(24, DART_RADIAN * +90.00); // right arm
917 // state1->setDesiredJointPosition(27, DART_RADIAN * +90.00); // left arm
918 // state1->setDesiredJointPosition(28, DART_RADIAN * -90.00); // right arm
919 //---- feedback gain for hip joints
920 state1->setFeedbackCoronalCOMDistance(
921 mCoronalLeftHip, -cd); // coronal left hip
922 state1->setFeedbackCoronalCOMVelocity(
923 mCoronalLeftHip, -cv); // coronal left hip
924 state1->setFeedbackCoronalCOMDistance(
925 mCoronalRightHip, -cd); // coronal right hip
926 state1->setFeedbackCoronalCOMVelocity(
927 mCoronalRightHip, -cv); // coronal right hip
928 state1->setFeedbackSagitalCOMDistance(
929 mSagitalLeftHip, -cd); // sagital left hip
930 state1->setFeedbackSagitalCOMVelocity(
931 mSagitalLeftHip, -cv); // sagital left hip
932 state1->setFeedbackSagitalCOMDistance(
933 mSagitalRightHip, -cd); // sagital right hip
934 state1->setFeedbackSagitalCOMVelocity(
935 mSagitalRightHip, -cv); // sagital right hip
936
937 sm->addState(state0);
938 sm->addState(state1);
939
940 sm->setInitialState(state0);
941
942 return sm;
943 }
944
945 //==============================================================================
_setJointDamping()946 void Controller::_setJointDamping()
947 {
948 for (std::size_t i = 1; i < mAtlasRobot->getNumBodyNodes(); ++i)
949 {
950 Joint* joint = mAtlasRobot->getJoint(i);
951 if (joint->getNumDofs() > 0)
952 {
953 for (std::size_t j = 0; j < joint->getNumDofs(); ++j)
954 joint->setDampingCoefficient(j, 80.0);
955 }
956 }
957 }
958
959 //==============================================================================
_getLeftFoot() const960 BodyNode* Controller::_getLeftFoot() const
961 {
962 return mAtlasRobot->getBodyNode("l_foot");
963 }
964
965 //==============================================================================
_getRightFoot() const966 BodyNode* Controller::_getRightFoot() const
967 {
968 return mAtlasRobot->getBodyNode("r_foot");
969 }
970
971 //==============================================================================
_containStateMachine(const StateMachine * _stateMachine) const972 bool Controller::_containStateMachine(const StateMachine* _stateMachine) const
973 {
974 for (vector<StateMachine*>::const_iterator it = mStateMachines.begin();
975 it != mStateMachines.end();
976 ++it)
977 {
978 if (*it == _stateMachine)
979 return true;
980 }
981
982 return false;
983 }
984
985 //==============================================================================
_containStateMachine(const string & _name) const986 bool Controller::_containStateMachine(const string& _name) const
987 {
988 return _containStateMachine(_findStateMachine(_name));
989 }
990
991 //==============================================================================
_findStateMachine(const string & _name) const992 StateMachine* Controller::_findStateMachine(const string& _name) const
993 {
994 StateMachine* stateMachine = nullptr;
995
996 for (vector<StateMachine*>::const_iterator it = mStateMachines.begin();
997 it != mStateMachines.end();
998 ++it)
999 {
1000 if ((*it)->getName() == _name)
1001 {
1002 stateMachine = *it;
1003 break;
1004 }
1005 }
1006
1007 return stateMachine;
1008 }
1009