//==============================================================================
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);
}
}
}
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);
}