virtual bool handle(const osgGA::GUIEventAdapter& ea,
osgGA::GUIActionAdapter&) override
{
if(nullptr == mAtlas)
{
return false;
}
if( osgGA::GUIEventAdapter::KEYDOWN == ea.getEventType() )
{
if( ea.getKey() == 'p' )
{
for(size_t i=0; i < mAtlas->getNumDofs(); ++i)
std::cout << mAtlas->getDof(i)->getName() << ": "
<< mAtlas->getDof(i)->getPosition() << std::endl;
std::cout << " -- -- -- -- -- " << std::endl;
return true;
}
if( ea.getKey() == 't' )
{
// Reset all the positions except for x, y, and yaw
for(size_t i=0; i < mAtlas->getNumDofs(); ++i)
{
if( i < 2 || 4 < i )
mAtlas->getDof(i)->setPosition(mRestConfig[i]);
}
return true;
}
if( '1' <= ea.getKey() && ea.getKey() <= '9' )
{
size_t index = ea.getKey() - '1';
if(index < mConstraintActive.size())
{
EndEffector* ee = mAtlas->getEndEffector(mEndEffectorIndex[index]);
const InverseKinematicsPtr& ik = ee->getIK();
if(ik && mConstraintActive[index])
{
mConstraintActive[index] = false;
ik->getErrorMethod().setBounds(mDefaultBounds[index]);
ik->getTarget()->setRelativeTransform(mDefaultTargetTf[index]);
mWorld->removeSimpleFrame(ik->getTarget());
}
else if(ik)
{
mConstraintActive[index] = true;
// Use the standard default bounds instead of our custom default
// bounds
ik->getErrorMethod().setBounds();
ik->getTarget()->setTransform(ee->getTransform());
mWorld->addSimpleFrame(ik->getTarget());
}
}
return true;
}
if( 'x' == ea.getKey() )
{
EndEffector* ee = mAtlas->getEndEffector("l_foot");
ee->getSupport()->setActive(!ee->getSupport()->isActive());
return true;
}
if( 'c' == ea.getKey() )
{
EndEffector* ee = mAtlas->getEndEffector("r_foot");
ee->getSupport()->setActive(!ee->getSupport()->isActive());
return true;
}
switch(ea.getKey())
{
case 'w': mMoveComponents[TeleoperationWorld::MOVE_W] = true; break;
case 'a': mMoveComponents[TeleoperationWorld::MOVE_A] = true; break;
case 's': mMoveComponents[TeleoperationWorld::MOVE_S] = true; break;
case 'd': mMoveComponents[TeleoperationWorld::MOVE_D] = true; break;
case 'q': mMoveComponents[TeleoperationWorld::MOVE_Q] = true; break;
case 'e': mMoveComponents[TeleoperationWorld::MOVE_E] = true; break;
case 'f': mMoveComponents[TeleoperationWorld::MOVE_F] = true; break;
case 'z': mMoveComponents[TeleoperationWorld::MOVE_Z] = true; break;
}
switch(ea.getKey())
{
case 'w': case 'a': case 's': case 'd': case 'q': case'e': case 'f': case 'z':
{
mTeleop->setMovement(mMoveComponents);
return true;
}
}
if(mOptimizationKey == ea.getKey())
{
if(mPosture)
mPosture->enforceIdealPosture = true;
if(mBalance)
mBalance->setErrorMethod(dart::constraint::BalanceConstraint::OPTIMIZE_BALANCE);
return true;
}
}
if( osgGA::GUIEventAdapter::KEYUP == ea.getEventType() )
{
if(ea.getKey() == mOptimizationKey)
{
if(mPosture)
mPosture->enforceIdealPosture = false;
if(mBalance)
mBalance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_CENTROID);
return true;
}
switch(ea.getKey())
{
case 'w': mMoveComponents[TeleoperationWorld::MOVE_W] = false; break;
case 'a': mMoveComponents[TeleoperationWorld::MOVE_A] = false; break;
case 's': mMoveComponents[TeleoperationWorld::MOVE_S] = false; break;
case 'd': mMoveComponents[TeleoperationWorld::MOVE_D] = false; break;
case 'q': mMoveComponents[TeleoperationWorld::MOVE_Q] = false; break;
case 'e': mMoveComponents[TeleoperationWorld::MOVE_E] = false; break;
case 'f': mMoveComponents[TeleoperationWorld::MOVE_F] = false; break;
case 'z': mMoveComponents[TeleoperationWorld::MOVE_Z] = false; break;
}
switch(ea.getKey())
{
case 'w': case 'a': case 's': case 'd': case 'q': case'e': case 'f': case 'z':
{
mTeleop->setMovement(mMoveComponents);
return true;
}
}
}
return false;
}
