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 <dart/dart.hpp>
34 #include <dart/gui/osg/osg.hpp>
35 #include <dart/utils/urdf/urdf.hpp>
36 #include <dart/utils/utils.hpp>
37
38 using namespace dart::common;
39 using namespace dart::dynamics;
40 using namespace dart::simulation;
41 using namespace dart::utils;
42 using namespace dart::math;
43
44 const double display_elevation = 0.05;
45
46 class RelaxedPosture : public dart::optimizer::Function
47 {
48 public:
RelaxedPosture(const Eigen::VectorXd & idealPosture,const Eigen::VectorXd & lower,const Eigen::VectorXd & upper,const Eigen::VectorXd & weights,bool enforceIdeal=false)49 RelaxedPosture(
50 const Eigen::VectorXd& idealPosture,
51 const Eigen::VectorXd& lower,
52 const Eigen::VectorXd& upper,
53 const Eigen::VectorXd& weights,
54 bool enforceIdeal = false)
55 : enforceIdealPosture(enforceIdeal),
56 mIdeal(idealPosture),
57 mLower(lower),
58 mUpper(upper),
59 mWeights(weights)
60 {
61 int dofs = mIdeal.size();
62 if (mLower.size() != dofs || mWeights.size() != dofs
63 || mUpper.size() != dofs)
64 {
65 dterr << "[RelaxedPose::RelaxedPose] Dimension mismatch:\n"
66 << " ideal: " << mIdeal.size() << "\n"
67 << " lower: " << mLower.size() << "\n"
68 << " upper: " << mUpper.size() << "\n"
69 << " weights: " << mWeights.size() << "\n";
70 }
71 mResultVector.setZero(dofs);
72 }
73
eval(const Eigen::VectorXd & _x)74 double eval(const Eigen::VectorXd& _x) override
75 {
76 computeResultVector(_x);
77 return 0.5 * mResultVector.dot(mResultVector);
78 }
79
evalGradient(const Eigen::VectorXd & _x,Eigen::Map<Eigen::VectorXd> _grad)80 void evalGradient(
81 const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override
82 {
83 computeResultVector(_x);
84
85 _grad.setZero();
86 int smaller = std::min(mResultVector.size(), _grad.size());
87 for (int i = 0; i < smaller; ++i)
88 _grad[i] = mResultVector[i];
89 }
90
computeResultVector(const Eigen::VectorXd & _x)91 void computeResultVector(const Eigen::VectorXd& _x)
92 {
93 mResultVector.setZero();
94
95 if (enforceIdealPosture)
96 {
97 // Try to get the robot into the best possible posture
98 for (int i = 0; i < _x.size(); ++i)
99 {
100 if (mIdeal.size() <= i)
101 break;
102
103 mResultVector[i] = mWeights[i] * (_x[i] - mIdeal[i]);
104 }
105 }
106 else
107 {
108 // Only adjust the posture if it is really bad
109 for (int i = 0; i < _x.size(); ++i)
110 {
111 if (mIdeal.size() <= i)
112 break;
113
114 if (_x[i] < mLower[i])
115 mResultVector[i] = mWeights[i] * (_x[i] - mLower[i]);
116 else if (mUpper[i] < _x[i])
117 mResultVector[i] = mWeights[i] * (_x[i] - mUpper[i]);
118 }
119 }
120 }
121
122 bool enforceIdealPosture;
123
124 protected:
125 Eigen::VectorXd mResultVector;
126
127 Eigen::VectorXd mIdeal;
128
129 Eigen::VectorXd mLower;
130
131 Eigen::VectorXd mUpper;
132
133 Eigen::VectorXd mWeights;
134 };
135
136 class TeleoperationWorld : public dart::gui::osg::WorldNode
137 {
138 public:
139 enum MoveEnum_t
140 {
141 MOVE_Q = 0,
142 MOVE_W,
143 MOVE_E,
144 MOVE_A,
145 MOVE_S,
146 MOVE_D,
147 MOVE_F,
148 MOVE_Z,
149
150 NUM_MOVE
151 };
152
TeleoperationWorld(WorldPtr _world,SkeletonPtr _robot)153 TeleoperationWorld(WorldPtr _world, SkeletonPtr _robot)
154 : dart::gui::osg::WorldNode(_world),
155 mAtlas(_robot),
156 iter(0),
157 l_foot(_robot->getEndEffector("l_foot")),
158 r_foot(_robot->getEndEffector("r_foot"))
159 {
160 mMoveComponents.resize(NUM_MOVE, false);
161 mAnyMovement = false;
162 }
163
setMovement(const std::vector<bool> & moveComponents)164 void setMovement(const std::vector<bool>& moveComponents)
165 {
166 mMoveComponents = moveComponents;
167
168 mAnyMovement = false;
169
170 for (bool move : mMoveComponents)
171 {
172 if (move)
173 {
174 mAnyMovement = true;
175 break;
176 }
177 }
178 }
179
customPreRefresh()180 void customPreRefresh() override
181 {
182 if (mAnyMovement)
183 {
184 Eigen::Isometry3d old_tf = mAtlas->getBodyNode(0)->getWorldTransform();
185 Eigen::Isometry3d new_tf = Eigen::Isometry3d::Identity();
186 Eigen::Vector3d forward = old_tf.linear().col(0);
187 forward[2] = 0.0;
188 if (forward.norm() > 1e-10)
189 forward.normalize();
190 else
191 forward.setZero();
192
193 Eigen::Vector3d left = old_tf.linear().col(1);
194 left[2] = 0.0;
195 if (left.norm() > 1e-10)
196 left.normalize();
197 else
198 left.setZero();
199
200 const Eigen::Vector3d& up = Eigen::Vector3d::UnitZ();
201
202 const double linearStep = 0.01;
203 const double elevationStep = 0.2 * linearStep;
204 const double rotationalStep = 2.0 * constantsd::pi() / 180.0;
205
206 if (mMoveComponents[MOVE_W])
207 new_tf.translate(linearStep * forward);
208
209 if (mMoveComponents[MOVE_S])
210 new_tf.translate(-linearStep * forward);
211
212 if (mMoveComponents[MOVE_A])
213 new_tf.translate(linearStep * left);
214
215 if (mMoveComponents[MOVE_D])
216 new_tf.translate(-linearStep * left);
217
218 if (mMoveComponents[MOVE_F])
219 new_tf.translate(elevationStep * up);
220
221 if (mMoveComponents[MOVE_Z])
222 new_tf.translate(-elevationStep * up);
223
224 if (mMoveComponents[MOVE_Q])
225 new_tf.rotate(Eigen::AngleAxisd(rotationalStep, up));
226
227 if (mMoveComponents[MOVE_E])
228 new_tf.rotate(Eigen::AngleAxisd(-rotationalStep, up));
229
230 new_tf.pretranslate(old_tf.translation());
231 new_tf.rotate(old_tf.rotation());
232
233 mAtlas->getJoint(0)->setPositions(FreeJoint::convertToPositions(new_tf));
234 }
235
236 bool solved = mAtlas->getIK(true)->solveAndApply(true);
237
238 if (!solved)
239 ++iter;
240 else
241 iter = 0;
242
243 if (iter == 1000)
244 {
245 std::cout << "Failing!" << std::endl;
246 }
247 }
248
249 protected:
250 SkeletonPtr mAtlas;
251 std::size_t iter;
252
253 EndEffectorPtr l_foot;
254 EndEffectorPtr r_foot;
255
256 Eigen::VectorXd grad;
257
258 std::vector<bool> mMoveComponents;
259
260 bool mAnyMovement;
261 };
262
263 class InputHandler : public ::osgGA::GUIEventHandler
264 {
265 public:
InputHandler(dart::gui::osg::Viewer * viewer,TeleoperationWorld * teleop,const SkeletonPtr & atlas,const WorldPtr & world)266 InputHandler(
267 dart::gui::osg::Viewer* viewer,
268 TeleoperationWorld* teleop,
269 const SkeletonPtr& atlas,
270 const WorldPtr& world)
271 : mViewer(viewer), mTeleop(teleop), mAtlas(atlas), mWorld(world)
272 {
273 initialize();
274 }
275
initialize()276 void initialize()
277 {
278 mRestConfig = mAtlas->getPositions();
279
280 mLegs.reserve(12);
281 for (std::size_t i = 0; i < mAtlas->getNumDofs(); ++i)
282 {
283 if (mAtlas->getDof(i)->getName().substr(1, 5) == "_leg_")
284 mLegs.push_back(mAtlas->getDof(i)->getIndexInSkeleton());
285 }
286 // We should also adjust the pelvis when detangling the legs
287 mLegs.push_back(mAtlas->getDof("rootJoint_rot_x")->getIndexInSkeleton());
288 mLegs.push_back(mAtlas->getDof("rootJoint_rot_y")->getIndexInSkeleton());
289 mLegs.push_back(mAtlas->getDof("rootJoint_pos_z")->getIndexInSkeleton());
290
291 for (std::size_t i = 0; i < mAtlas->getNumEndEffectors(); ++i)
292 {
293 const InverseKinematicsPtr ik = mAtlas->getEndEffector(i)->getIK();
294 if (ik)
295 {
296 mDefaultBounds.push_back(ik->getErrorMethod().getBounds());
297 mDefaultTargetTf.push_back(ik->getTarget()->getRelativeTransform());
298 mConstraintActive.push_back(false);
299 mEndEffectorIndex.push_back(i);
300 }
301 }
302
303 mPosture = std::dynamic_pointer_cast<RelaxedPosture>(
304 mAtlas->getIK(true)->getObjective());
305
306 mBalance = std::dynamic_pointer_cast<dart::constraint::BalanceConstraint>(
307 mAtlas->getIK(true)->getProblem()->getEqConstraint(1));
308
309 mOptimizationKey = 'r';
310
311 mMoveComponents.resize(TeleoperationWorld::NUM_MOVE, false);
312 }
313
handle(const::osgGA::GUIEventAdapter & ea,::osgGA::GUIActionAdapter &)314 virtual bool handle(
315 const ::osgGA::GUIEventAdapter& ea, ::osgGA::GUIActionAdapter&) override
316 {
317 if (nullptr == mAtlas)
318 {
319 return false;
320 }
321
322 if (::osgGA::GUIEventAdapter::KEYDOWN == ea.getEventType())
323 {
324 if (ea.getKey() == 'p')
325 {
326 for (std::size_t i = 0; i < mAtlas->getNumDofs(); ++i)
327 std::cout << mAtlas->getDof(i)->getName() << ": "
328 << mAtlas->getDof(i)->getPosition() << std::endl;
329 std::cout << " -- -- -- -- -- " << std::endl;
330 return true;
331 }
332
333 if (ea.getKey() == 't')
334 {
335 // Reset all the positions except for x, y, and yaw
336 for (std::size_t i = 0; i < mAtlas->getNumDofs(); ++i)
337 {
338 if (i < 2 || 4 < i)
339 mAtlas->getDof(i)->setPosition(mRestConfig[i]);
340 }
341 return true;
342 }
343
344 if ('1' <= ea.getKey() && ea.getKey() <= '9')
345 {
346 std::size_t index = ea.getKey() - '1';
347 if (index < mConstraintActive.size())
348 {
349 EndEffector* ee = mAtlas->getEndEffector(mEndEffectorIndex[index]);
350 const InverseKinematicsPtr& ik = ee->getIK();
351 if (ik && mConstraintActive[index])
352 {
353 mConstraintActive[index] = false;
354
355 ik->getErrorMethod().setBounds(mDefaultBounds[index]);
356 ik->getTarget()->setRelativeTransform(mDefaultTargetTf[index]);
357 mWorld->removeSimpleFrame(ik->getTarget());
358 }
359 else if (ik)
360 {
361 mConstraintActive[index] = true;
362
363 // Use the standard default bounds instead of our custom default
364 // bounds
365 ik->getErrorMethod().setBounds();
366 ik->getTarget()->setTransform(ee->getTransform());
367 mWorld->addSimpleFrame(ik->getTarget());
368 }
369 }
370 return true;
371 }
372
373 if ('x' == ea.getKey())
374 {
375 EndEffector* ee = mAtlas->getEndEffector("l_foot");
376 ee->getSupport()->setActive(!ee->getSupport()->isActive());
377 return true;
378 }
379
380 if ('c' == ea.getKey())
381 {
382 EndEffector* ee = mAtlas->getEndEffector("r_foot");
383 ee->getSupport()->setActive(!ee->getSupport()->isActive());
384 return true;
385 }
386
387 switch (ea.getKey())
388 {
389 case 'w':
390 mMoveComponents[TeleoperationWorld::MOVE_W] = true;
391 break;
392 case 'a':
393 mMoveComponents[TeleoperationWorld::MOVE_A] = true;
394 break;
395 case 's':
396 mMoveComponents[TeleoperationWorld::MOVE_S] = true;
397 break;
398 case 'd':
399 mMoveComponents[TeleoperationWorld::MOVE_D] = true;
400 break;
401 case 'q':
402 mMoveComponents[TeleoperationWorld::MOVE_Q] = true;
403 break;
404 case 'e':
405 mMoveComponents[TeleoperationWorld::MOVE_E] = true;
406 break;
407 case 'f':
408 mMoveComponents[TeleoperationWorld::MOVE_F] = true;
409 break;
410 case 'z':
411 mMoveComponents[TeleoperationWorld::MOVE_Z] = true;
412 break;
413 }
414
415 switch (ea.getKey())
416 {
417 case 'w':
418 case 'a':
419 case 's':
420 case 'd':
421 case 'q':
422 case 'e':
423 case 'f':
424 case 'z':
425 {
426 mTeleop->setMovement(mMoveComponents);
427 return true;
428 }
429 }
430
431 if (mOptimizationKey == ea.getKey())
432 {
433 if (mPosture)
434 mPosture->enforceIdealPosture = true;
435
436 if (mBalance)
437 mBalance->setErrorMethod(
438 dart::constraint::BalanceConstraint::OPTIMIZE_BALANCE);
439
440 return true;
441 }
442 }
443
444 if (::osgGA::GUIEventAdapter::KEYUP == ea.getEventType())
445 {
446 if (ea.getKey() == mOptimizationKey)
447 {
448 if (mPosture)
449 mPosture->enforceIdealPosture = false;
450
451 if (mBalance)
452 mBalance->setErrorMethod(
453 dart::constraint::BalanceConstraint::FROM_CENTROID);
454
455 return true;
456 }
457
458 switch (ea.getKey())
459 {
460 case 'w':
461 mMoveComponents[TeleoperationWorld::MOVE_W] = false;
462 break;
463 case 'a':
464 mMoveComponents[TeleoperationWorld::MOVE_A] = false;
465 break;
466 case 's':
467 mMoveComponents[TeleoperationWorld::MOVE_S] = false;
468 break;
469 case 'd':
470 mMoveComponents[TeleoperationWorld::MOVE_D] = false;
471 break;
472 case 'q':
473 mMoveComponents[TeleoperationWorld::MOVE_Q] = false;
474 break;
475 case 'e':
476 mMoveComponents[TeleoperationWorld::MOVE_E] = false;
477 break;
478 case 'f':
479 mMoveComponents[TeleoperationWorld::MOVE_F] = false;
480 break;
481 case 'z':
482 mMoveComponents[TeleoperationWorld::MOVE_Z] = false;
483 break;
484 }
485
486 switch (ea.getKey())
487 {
488 case 'w':
489 case 'a':
490 case 's':
491 case 'd':
492 case 'q':
493 case 'e':
494 case 'f':
495 case 'z':
496 {
497 mTeleop->setMovement(mMoveComponents);
498 return true;
499 }
500 }
501 }
502
503 return false;
504 }
505
506 protected:
507 dart::gui::osg::Viewer* mViewer;
508
509 TeleoperationWorld* mTeleop;
510
511 SkeletonPtr mAtlas;
512
513 WorldPtr mWorld;
514
515 Eigen::VectorXd mRestConfig;
516
517 std::vector<std::size_t> mLegs;
518
519 std::vector<bool> mConstraintActive;
520
521 std::vector<std::size_t> mEndEffectorIndex;
522
523 std::vector<std::pair<Eigen::Vector6d, Eigen::Vector6d> > mDefaultBounds;
524
525 dart::common::aligned_vector<Eigen::Isometry3d> mDefaultTargetTf;
526
527 std::shared_ptr<RelaxedPosture> mPosture;
528
529 std::shared_ptr<dart::constraint::BalanceConstraint> mBalance;
530
531 char mOptimizationKey;
532
533 std::vector<bool> mMoveComponents;
534 };
535
createGround()536 SkeletonPtr createGround()
537 {
538 // Create a Skeleton to represent the ground
539 SkeletonPtr ground = Skeleton::create("ground");
540 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
541 double thickness = 0.01;
542 tf.translation() = Eigen::Vector3d(0, 0, -thickness / 2.0);
543 WeldJoint::Properties joint;
544 joint.mT_ParentBodyToJoint = tf;
545 ground->createJointAndBodyNodePair<WeldJoint>(nullptr, joint);
546 ShapePtr groundShape
547 = std::make_shared<BoxShape>(Eigen::Vector3d(10, 10, thickness));
548
549 auto shapeNode = ground->getBodyNode(0)
550 ->createShapeNodeWith<
551 VisualAspect,
552 CollisionAspect,
553 DynamicsAspect>(groundShape);
554 shapeNode->getVisualAspect()->setColor(dart::Color::Blue(0.2));
555
556 return ground;
557 }
558
createAtlas()559 SkeletonPtr createAtlas()
560 {
561 // Parse in the atlas model
562 DartLoader urdf;
563 SkeletonPtr atlas
564 = urdf.parseSkeleton("dart://sample/sdf/atlas/atlas_v3_no_head.urdf");
565
566 // Add a box to the root node to make it easier to click and drag
567 double scale = 0.25;
568 ShapePtr boxShape
569 = std::make_shared<BoxShape>(scale * Eigen::Vector3d(1.0, 1.0, 0.5));
570
571 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
572 tf.translation() = Eigen::Vector3d(0.1 * Eigen::Vector3d(0.0, 0.0, 1.0));
573
574 auto shapeNode
575 = atlas->getBodyNode(0)->createShapeNodeWith<VisualAspect>(boxShape);
576 shapeNode->getVisualAspect()->setColor(dart::Color::Black());
577 shapeNode->setRelativeTransform(tf);
578
579 return atlas;
580 }
581
setupStartConfiguration(const SkeletonPtr & atlas)582 void setupStartConfiguration(const SkeletonPtr& atlas)
583 {
584 // Squat with the right leg
585 atlas->getDof("r_leg_hpy")->setPosition(-45.0 * constantsd::pi() / 180.0);
586 atlas->getDof("r_leg_kny")->setPosition(90.0 * constantsd::pi() / 180.0);
587 atlas->getDof("r_leg_aky")->setPosition(-45.0 * constantsd::pi() / 180.0);
588
589 // Squat with the left left
590 atlas->getDof("l_leg_hpy")->setPosition(-45.0 * constantsd::pi() / 180.0);
591 atlas->getDof("l_leg_kny")->setPosition(90.0 * constantsd::pi() / 180.0);
592 atlas->getDof("l_leg_aky")->setPosition(-45.0 * constantsd::pi() / 180.0);
593
594 // Get the right arm into a comfortable position
595 atlas->getDof("r_arm_shx")->setPosition(65.0 * constantsd::pi() / 180.0);
596 atlas->getDof("r_arm_ely")->setPosition(90.0 * constantsd::pi() / 180.0);
597 atlas->getDof("r_arm_elx")->setPosition(-90.0 * constantsd::pi() / 180.0);
598 atlas->getDof("r_arm_wry")->setPosition(65.0 * constantsd::pi() / 180.0);
599
600 // Get the left arm into a comfortable position
601 atlas->getDof("l_arm_shx")->setPosition(-65.0 * constantsd::pi() / 180.0);
602 atlas->getDof("l_arm_ely")->setPosition(90.0 * constantsd::pi() / 180.0);
603 atlas->getDof("l_arm_elx")->setPosition(90.0 * constantsd::pi() / 180.0);
604 atlas->getDof("l_arm_wry")->setPosition(65.0 * constantsd::pi() / 180.0);
605
606 // Prevent the knees from bending backwards
607 atlas->getDof("r_leg_kny")
608 ->setPositionLowerLimit(10 * constantsd::pi() / 180.0);
609 atlas->getDof("l_leg_kny")
610 ->setPositionLowerLimit(10 * constantsd::pi() / 180.0);
611 }
612
setupEndEffectors(const SkeletonPtr & atlas)613 void setupEndEffectors(const SkeletonPtr& atlas)
614 {
615 // Apply very small weights to the gradient of the root joint in order to
616 // encourage the arms to use arm joints instead of only moving around the root
617 // joint
618 Eigen::VectorXd rootjoint_weights = Eigen::VectorXd::Ones(6);
619 rootjoint_weights = 0.01 * rootjoint_weights;
620
621 // Setting the bounds to be infinite allows the end effector to be implicitly
622 // unconstrained
623 Eigen::Vector3d linearBounds
624 = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
625
626 Eigen::Vector3d angularBounds
627 = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
628
629 // -- Set up the left hand --
630
631 // Create a relative transform for the EndEffector frame. This is the
632 // transform that the left hand will have relative to the BodyNode that it is
633 // attached to
634 Eigen::Isometry3d tf_hand(Eigen::Isometry3d::Identity());
635 tf_hand.translation() = Eigen::Vector3d(0.0009, 0.1254, 0.012);
636 tf_hand.rotate(Eigen::AngleAxisd(
637 90.0 * constantsd::pi() / 180.0, Eigen::Vector3d::UnitZ()));
638
639 // Create the left hand's end effector and set its relative transform
640 EndEffector* l_hand
641 = atlas->getBodyNode("l_hand")->createEndEffector("l_hand");
642 l_hand->setDefaultRelativeTransform(tf_hand, true);
643
644 // Create an interactive frame to use as the target for the left hand
645 dart::gui::osg::InteractiveFramePtr lh_target(
646 new dart::gui::osg::InteractiveFrame(Frame::World(), "lh_target"));
647
648 // Set the target of the left hand to the interactive frame. We pass true into
649 // the function to tell it that it should create the IK module if it does not
650 // already exist. If we don't do that, then calling getIK() could return a
651 // nullptr if the IK module was never created.
652 l_hand->getIK(true)->setTarget(lh_target);
653
654 // Tell the left hand to use the whole body for its IK
655 l_hand->getIK()->useWholeBody();
656
657 // Set the weights for the gradient
658 l_hand->getIK()->getGradientMethod().setComponentWeights(rootjoint_weights);
659
660 // Set the bounds for the IK to be infinite so that the hands start out
661 // unconstrained
662 l_hand->getIK()->getErrorMethod().setLinearBounds(
663 -linearBounds, linearBounds);
664
665 l_hand->getIK()->getErrorMethod().setAngularBounds(
666 -angularBounds, angularBounds);
667
668 // -- Set up the right hand --
669
670 // The orientation of the right hand frame is different than the left, so we
671 // need to adjust the signs of the relative transform
672 tf_hand.translation()[0] = -tf_hand.translation()[0];
673 tf_hand.translation()[1] = -tf_hand.translation()[1];
674 tf_hand.linear() = tf_hand.linear().inverse().eval();
675
676 // Create the right hand's end effector and set its relative transform
677 EndEffector* r_hand
678 = atlas->getBodyNode("r_hand")->createEndEffector("r_hand");
679 r_hand->setDefaultRelativeTransform(tf_hand, true);
680
681 // Create an interactive frame to use as the target for the right hand
682 dart::gui::osg::InteractiveFramePtr rh_target(
683 new dart::gui::osg::InteractiveFrame(Frame::World(), "rh_target"));
684
685 // Create the right hand's IK and set its target
686 r_hand->getIK(true)->setTarget(rh_target);
687
688 // Tell the right hand to use the whole body for its IK
689 r_hand->getIK()->useWholeBody();
690
691 // Set the weights for the gradient
692 r_hand->getIK()->getGradientMethod().setComponentWeights(rootjoint_weights);
693
694 // Set the bounds for the IK to be infinite so that the hands start out
695 // unconstrained
696 r_hand->getIK()->getErrorMethod().setLinearBounds(
697 -linearBounds, linearBounds);
698
699 r_hand->getIK()->getErrorMethod().setAngularBounds(
700 -angularBounds, angularBounds);
701
702 // Define the support geometry for the feet. These points will be used to
703 // compute the convex hull of the robot's support polygon
704 dart::math::SupportGeometry support;
705 const double sup_pos_x = 0.10 - 0.186;
706 const double sup_neg_x = -0.03 - 0.186;
707 const double sup_pos_y = 0.03;
708 const double sup_neg_y = -0.03;
709 support.push_back(Eigen::Vector3d(sup_neg_x, sup_neg_y, 0.0));
710 support.push_back(Eigen::Vector3d(sup_pos_x, sup_neg_y, 0.0));
711 support.push_back(Eigen::Vector3d(sup_pos_x, sup_pos_y, 0.0));
712 support.push_back(Eigen::Vector3d(sup_neg_x, sup_pos_y, 0.0));
713
714 // Create a relative transform that goes from the center of the feet to the
715 // bottom of the feet
716 Eigen::Isometry3d tf_foot(Eigen::Isometry3d::Identity());
717 tf_foot.translation() = Eigen::Vector3d(0.186, 0.0, -0.08);
718
719 // Constrain the feet to snap to the ground
720 linearBounds[2] = 1e-8;
721
722 // Constrain the feet to lie flat on the ground
723 angularBounds[0] = 1e-8;
724 angularBounds[1] = 1e-8;
725
726 // Create an end effector for the left foot and set its relative transform
727 EndEffector* l_foot
728 = atlas->getBodyNode("l_foot")->createEndEffector("l_foot");
729 l_foot->setRelativeTransform(tf_foot);
730
731 // Create an interactive frame to use as the target for the left foot
732 dart::gui::osg::InteractiveFramePtr lf_target(
733 new dart::gui::osg::InteractiveFrame(Frame::World(), "lf_target"));
734
735 // Create the left foot's IK and set its target
736 l_foot->getIK(true)->setTarget(lf_target);
737
738 // Set the left foot's IK hierarchy level to 1. This will project its IK goals
739 // through the null space of any IK modules that are on level 0. This means
740 // that it will try to accomplish its goals while also accommodating the goals
741 // of other modules.
742 l_foot->getIK()->setHierarchyLevel(1);
743
744 // Use the bounds defined above
745 l_foot->getIK()->getErrorMethod().setLinearBounds(
746 -linearBounds, linearBounds);
747 l_foot->getIK()->getErrorMethod().setAngularBounds(
748 -angularBounds, angularBounds);
749
750 // Create Support for the foot and give it geometry
751 l_foot->getSupport(true)->setGeometry(support);
752
753 // Turn on support mode so that it can be used as a foot
754 l_foot->getSupport()->setActive();
755
756 // Create an end effector for the right foot and set its relative transform
757 EndEffector* r_foot
758 = atlas->getBodyNode("r_foot")->createEndEffector("r_foot");
759 r_foot->setRelativeTransform(tf_foot);
760
761 // Create an interactive frame to use as the target for the right foot
762 dart::gui::osg::InteractiveFramePtr rf_target(
763 new dart::gui::osg::InteractiveFrame(Frame::World(), "rf_target"));
764
765 // Create the right foot's IK module and set its target
766 r_foot->getIK(true)->setTarget(rf_target);
767
768 // Set the right foot's IK hierarchy level to 1
769 r_foot->getIK()->setHierarchyLevel(1);
770
771 // Use the bounds defined above
772 r_foot->getIK()->getErrorMethod().setLinearBounds(
773 -linearBounds, linearBounds);
774 r_foot->getIK()->getErrorMethod().setAngularBounds(
775 -angularBounds, angularBounds);
776
777 // Create Support for the foot and give it geometry
778 r_foot->getSupport(true)->setGeometry(support);
779
780 // Turn on support mode so that it can be used as a foot
781 r_foot->getSupport()->setActive();
782
783 // Move atlas to the ground so that it starts out squatting with its feet on
784 // the ground
785 double heightChange = -r_foot->getWorldTransform().translation()[2];
786 atlas->getDof(5)->setPosition(heightChange);
787
788 // Now that the feet are on the ground, we should set their target transforms
789 l_foot->getIK()->getTarget()->setTransform(l_foot->getTransform());
790 r_foot->getIK()->getTarget()->setTransform(r_foot->getTransform());
791 }
792
setupWholeBodySolver(const SkeletonPtr & atlas)793 void setupWholeBodySolver(const SkeletonPtr& atlas)
794 {
795 // The default
796 std::shared_ptr<dart::optimizer::GradientDescentSolver> solver
797 = std::dynamic_pointer_cast<dart::optimizer::GradientDescentSolver>(
798 atlas->getIK(true)->getSolver());
799 solver->setNumMaxIterations(10);
800
801 std::size_t nDofs = atlas->getNumDofs();
802
803 double default_weight = 0.01;
804 Eigen::VectorXd weights = default_weight * Eigen::VectorXd::Ones(nDofs);
805 weights[2] = 0.0;
806 weights[3] = 0.0;
807 weights[4] = 0.0;
808
809 weights[6] *= 0.2;
810 weights[7] *= 0.2;
811 weights[8] *= 0.2;
812
813 Eigen::VectorXd lower_posture = Eigen::VectorXd::Constant(
814 nDofs, -std::numeric_limits<double>::infinity());
815 lower_posture[0] = -0.35;
816 lower_posture[1] = -0.35;
817 lower_posture[5] = 0.600;
818
819 lower_posture[6] = -0.1;
820 lower_posture[7] = -0.1;
821 lower_posture[8] = -0.1;
822
823 Eigen::VectorXd upper_posture = Eigen::VectorXd::Constant(
824 nDofs, std::numeric_limits<double>::infinity());
825 upper_posture[0] = 0.35;
826 upper_posture[1] = 0.35;
827 upper_posture[5] = 0.885;
828
829 upper_posture[6] = 0.1;
830 upper_posture[7] = 0.1;
831 upper_posture[8] = 0.1;
832
833 std::shared_ptr<RelaxedPosture> objective = std::make_shared<RelaxedPosture>(
834 atlas->getPositions(), lower_posture, upper_posture, weights);
835 atlas->getIK()->setObjective(objective);
836
837 std::shared_ptr<dart::constraint::BalanceConstraint> balance
838 = std::make_shared<dart::constraint::BalanceConstraint>(atlas->getIK());
839 atlas->getIK()->getProblem()->addEqConstraint(balance);
840
841 // // Shift the center of mass towards the support polygon center while
842 // trying
843 // // to keep the support polygon where it is
844 // balance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_CENTROID);
845 // balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_COM);
846
847 // // Keep shifting the center of mass towards the center of the support
848 // // polygon, even if it is already inside. This is useful for trying to
849 // // optimize a stance
850 // balance->setErrorMethod(dart::constraint::BalanceConstraint::OPTIMIZE_BALANCE);
851 // balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_COM);
852
853 // // Try to leave the center of mass where it is while moving the support
854 // // polygon to be under the current center of mass location
855 balance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_CENTROID);
856 balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_SUPPORT);
857
858 // // Try to leave the center of mass where it is while moving the support
859 // // point that is closest to the center of mass
860 // balance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_EDGE);
861 // balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_SUPPORT);
862
863 // Note that using the FROM_EDGE error method is liable to leave the center of
864 // mass visualization red even when the constraint was successfully solved.
865 // This is because the constraint solver has a tiny bit of tolerance that
866 // allows the Problem to be considered solved when the center of mass is
867 // microscopically outside of the support polygon. This is an inherent risk of
868 // using FROM_EDGE instead of FROM_CENTROID.
869
870 // Feel free to experiment with the different balancing methods. You will find
871 // that some work much better for user interaction than others.
872 }
873
enableDragAndDrops(dart::gui::osg::Viewer & viewer,const SkeletonPtr & atlas)874 void enableDragAndDrops(
875 dart::gui::osg::Viewer& viewer, const SkeletonPtr& atlas)
876 {
877 // Turn on drag-and-drop for the whole Skeleton
878 for (std::size_t i = 0; i < atlas->getNumBodyNodes(); ++i)
879 viewer.enableDragAndDrop(atlas->getBodyNode(i), false, false);
880
881 for (std::size_t i = 0; i < atlas->getNumEndEffectors(); ++i)
882 {
883 EndEffector* ee = atlas->getEndEffector(i);
884 if (!ee->getIK())
885 continue;
886
887 // Check whether the target is an interactive frame, and add it if it is
888 if (const auto& frame
889 = std::dynamic_pointer_cast<dart::gui::osg::InteractiveFrame>(
890 ee->getIK()->getTarget()))
891 viewer.enableDragAndDrop(frame.get());
892 }
893 }
894
main()895 int main()
896 {
897 WorldPtr world = World::create();
898
899 SkeletonPtr atlas = createAtlas();
900 world->addSkeleton(atlas);
901
902 SkeletonPtr ground = createGround();
903 world->addSkeleton(ground);
904
905 setupStartConfiguration(atlas);
906
907 setupEndEffectors(atlas);
908
909 setupWholeBodySolver(atlas);
910
911 ::osg::ref_ptr<TeleoperationWorld> node
912 = new TeleoperationWorld(world, atlas);
913
914 dart::gui::osg::Viewer viewer;
915
916 // Prevent this World from simulating
917 viewer.allowSimulation(false);
918 viewer.addWorldNode(node);
919
920 // Add our custom input handler to the Viewer
921 viewer.addEventHandler(new InputHandler(&viewer, node, atlas, world));
922
923 enableDragAndDrops(viewer, atlas);
924
925 // Attach a support polygon visualizer
926 viewer.addAttachment(
927 new dart::gui::osg::SupportPolygonVisual(atlas, display_elevation));
928
929 // Print out instructions for the viewer
930 std::cout << viewer.getInstructions() << std::endl;
931
932 std::cout
933 << "Alt + Click: Try to translate a body without changing its "
934 "orientation\n"
935 << "Ctrl + Click: Try to rotate a body without changing its "
936 "translation\n"
937 << "Shift + Click: Move a body using only its parent joint\n"
938 << "1 -> 4: Toggle the interactive target of an EndEffector\n"
939 << "W A S D: Move the robot around the scene\n"
940 << "Q E: Rotate the robot counter-clockwise and clockwise\n"
941 << "F Z: Shift the robot's elevation up and down\n"
942 << "X C: Toggle support on the left and right foot\n"
943 << "R: Optimize the robot's posture\n"
944 << "T: Reset the robot to its relaxed posture\n\n"
945 << " Because this uses iterative Jacobian methods, the solver can get "
946 "finicky,\n"
947 << " and the robot can get tangled up. Use 'R' and 'T' keys when the "
948 "robot is\n"
949 << " in a messy configuration\n\n"
950 << " The green polygon is the support polygon of the robot, and the "
951 "blue/red ball is\n"
952 << " the robot's center of mass. The green ball is the centroid of the "
953 "polygon.\n\n"
954 << "Note that this is purely kinematic. Physical simulation is not "
955 "allowed in this app.\n"
956 << std::endl;
957
958 // Set up the window
959 viewer.setUpViewInWindow(0, 0, 1280, 960);
960
961 // Set up the default viewing position
962 viewer.getCameraManipulator()->setHomePosition(
963 ::osg::Vec3(5.34, 3.00, 2.41),
964 ::osg::Vec3(0.00, 0.00, 1.00),
965 ::osg::Vec3(-0.20, -0.08, 0.98));
966
967 // Reset the camera manipulator so that it starts in the new viewing position
968 viewer.setCameraManipulator(viewer.getCameraManipulator());
969
970 // Run the Viewer
971 viewer.run();
972 }
973