Esempio n. 1
0
//==============================================================================
void InputHandler::initialize()
{
  mRestConfig = mWam->getPositions();

  for (std::size_t i=0; i < mWam->getNumEndEffectors(); ++i)
  {
    const InverseKinematicsPtr ik = mWam->getEndEffector(i)->getIK();
    if (ik)
    {
      mDefaultBounds.push_back(ik->getErrorMethod().getBounds());
      mDefaultTargetTf.push_back(ik->getTarget()->getRelativeTransform());
      mConstraintActive.push_back(false);
      mEndEffectorIndex.push_back(i);
    }
  }
}
Esempio n. 2
0
  void initialize()
  {
    mRestConfig = mAtlas->getPositions();

    mLegs.reserve(12);
    for(size_t i=0; i<mAtlas->getNumDofs(); ++i)
    {
      if(mAtlas->getDof(i)->getName().substr(1, 5) == "_leg_")
        mLegs.push_back(mAtlas->getDof(i)->getIndexInSkeleton());
    }
    // We should also adjust the pelvis when detangling the legs
    mLegs.push_back(mAtlas->getDof("rootJoint_rot_x")->getIndexInSkeleton());
    mLegs.push_back(mAtlas->getDof("rootJoint_rot_y")->getIndexInSkeleton());
    mLegs.push_back(mAtlas->getDof("rootJoint_pos_z")->getIndexInSkeleton());

    for(size_t i=0; i < mAtlas->getNumEndEffectors(); ++i)
    {
      const InverseKinematicsPtr ik = mAtlas->getEndEffector(i)->getIK();
      if(ik)
      {
        mDefaultBounds.push_back(ik->getErrorMethod().getBounds());
        mDefaultTargetTf.push_back(ik->getTarget()->getRelativeTransform());
        mConstraintActive.push_back(false);
        mEndEffectorIndex.push_back(i);
      }
    }

    mPosture = std::dynamic_pointer_cast<RelaxedPosture>(
          mAtlas->getIK(true)->getObjective());

    mBalance = std::dynamic_pointer_cast<dart::constraint::BalanceConstraint>(
          mAtlas->getIK(true)->getProblem()->getEqConstraint(1));

    mOptimizationKey = 'r';

    mMoveComponents.resize(TeleoperationWorld::NUM_MOVE, false);
  }