| 266 | } |
| 267 | |
| 268 | void initialize() |
| 269 | { |
| 270 | mRestConfig = mAtlas->getPositions(); |
| 271 | |
| 272 | mLegs.reserve(12); |
| 273 | for (std::size_t i = 0; i < mAtlas->getNumDofs(); ++i) { |
| 274 | if (mAtlas->getDof(i)->getName().substr(1, 5) == "_leg_") |
| 275 | mLegs.push_back(mAtlas->getDof(i)->getIndexInSkeleton()); |
| 276 | } |
| 277 | // We should also adjust the pelvis when detangling the legs |
| 278 | mLegs.push_back(mAtlas->getDof("rootJoint_rot_x")->getIndexInSkeleton()); |
| 279 | mLegs.push_back(mAtlas->getDof("rootJoint_rot_y")->getIndexInSkeleton()); |
| 280 | mLegs.push_back(mAtlas->getDof("rootJoint_pos_z")->getIndexInSkeleton()); |
| 281 | |
| 282 | mAtlas->eachEndEffector([&](EndEffector* ee) { |
| 283 | if (const InverseKinematicsPtr ik = ee->getIK()) { |
| 284 | mDefaultBounds.push_back(ik->getErrorMethod().getBounds()); |
| 285 | mDefaultTargetTf.push_back(ik->getTarget()->getRelativeTransform()); |
| 286 | mConstraintActive.push_back(false); |
| 287 | mEndEffectorIndex.push_back(ee->getIndexInSkeleton()); |
| 288 | } |
| 289 | }); |
| 290 | |
| 291 | mPosture = std::dynamic_pointer_cast<RelaxedPosture>( |
| 292 | mAtlas->getIK(true)->getObjective()); |
| 293 | |
| 294 | mBalance = std::dynamic_pointer_cast<dart::constraint::BalanceConstraint>( |
| 295 | mAtlas->getIK(true)->getProblem()->getEqConstraint(1)); |
| 296 | |
| 297 | mOptimizationKey = 'r'; |
| 298 | |
| 299 | mMoveComponents.resize(TeleoperationWorld::NUM_MOVE, false); |
| 300 | } |
| 301 | |
| 302 | virtual bool handle( |
| 303 | const ::osgGA::GUIEventAdapter& ea, ::osgGA::GUIActionAdapter&) override |
nothing calls this directly
no test coverage detected