//==============================================================================
void EndEffector::copy(const EndEffector& _otherEndEffector)
{
if(this == &_otherEndEffector)
return;
setState(_otherEndEffector.getEndEffectorState());
setProperties(_otherEndEffector.getEndEffectorProperties());
}
//==============================================================================
void EndEffector::copy(const EndEffector& otherEndEffector)
{
if(this == &otherEndEffector)
return;
setCompositeState(otherEndEffector.getCompositeState());
setCompositeProperties(otherEndEffector.getCompositeProperties());
}
//==============================================================================
bool InputHandler::handle(
const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter&)
{
if (!mWam)
return false;
if (::osgGA::GUIEventAdapter::KEYDOWN == ea.getEventType())
{
if (ea.getKey() == 'p' || ea.getKey() == 'P')
{
for (std::size_t i=0; i < mWam->getNumDofs(); ++i)
std::cout << mWam->getDof(i)->getName() << ": "
<< mWam->getDof(i)->getPosition() << std::endl;
return true;
}
if (ea.getKey() == 't' || ea.getKey() == 'T')
{
mWam->setPositions(mRestConfig);
return true;
}
if ('1' <= ea.getKey() && ea.getKey() <= '9')
{
std::size_t index = ea.getKey() - '1';
if (index < mConstraintActive.size())
{
EndEffector* ee = mWam->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;
}
}
return false;
}
//==============================================================================
Node* EndEffector::cloneNode(BodyNode* _parent) const
{
EndEffector* ee = new EndEffector(_parent, PropertiesData());
ee->duplicateAddons(this);
ee->copy(this);
if(mIK)
ee->mIK = mIK->clone(ee);
return ee;
}
void enableDragAndDrops(osgDart::Viewer& viewer, const SkeletonPtr& atlas)
{
// Turn on drag-and-drop for the whole Skeleton
for(size_t i=0; i < atlas->getNumBodyNodes(); ++i)
viewer.enableDragAndDrop(atlas->getBodyNode(i), false, false);
for(size_t i=0; i < atlas->getNumEndEffectors(); ++i)
{
EndEffector* ee = atlas->getEndEffector(i);
if(!ee->getIK())
continue;
// Check whether the target is an interactive frame, and add it if it is
if(const auto& frame = std::dynamic_pointer_cast<osgDart::InteractiveFrame>(
ee->getIK()->getTarget()))
viewer.enableDragAndDrop(frame.get());
}
}
TEST(Skeleton, NodePersistence)
{
SkeletonPtr skel = Skeleton::create();
skel->createJointAndBodyNodePair<FreeJoint>(nullptr);
{
EndEffector* manip =
skel->getBodyNode(0)->createEndEffector(Entity::Properties("manip"));
EXPECT_TRUE(skel->getEndEffector("manip") == manip);
EXPECT_TRUE(skel->getEndEffector(0) == manip);
EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip);
WeakEndEffectorPtr weakManip = manip;
EXPECT_FALSE(weakManip.lock() == nullptr);
manip->remove();
// The Node has been removed, and no strong reference to it exists, so it
// should be gone from the Skeleton
EXPECT_TRUE(skel->getEndEffector("manip") == nullptr);
EXPECT_TRUE(skel->getNumEndEffectors() == 0);
EXPECT_TRUE(skel->getBodyNode(0)->getNumEndEffectors() == 0);
EXPECT_TRUE(weakManip.lock() == nullptr);
}
{
EndEffector* manip =
skel->getBodyNode(0)->createEndEffector(Entity::Properties("manip"));
EXPECT_TRUE(skel->getEndEffector("manip") == manip);
EXPECT_TRUE(skel->getEndEffector(0) == manip);
EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip);
EndEffectorPtr strongManip = manip;
WeakEndEffectorPtr weakManip = strongManip;
EXPECT_FALSE(weakManip.lock() == nullptr);
manip->remove();
// The Node has been removed, but a strong reference to it still exists, so
// it will remain in the Skeleton for now
EXPECT_TRUE(skel->getEndEffector("manip") == manip);
EXPECT_TRUE(skel->getEndEffector(0) == manip);
EXPECT_TRUE(skel->getBodyNode(0)->getEndEffector(0) == manip);
EXPECT_FALSE(weakManip.lock() == nullptr);
strongManip = nullptr;
// The Node has been removed, and no strong reference to it exists any
// longer, so it should be gone from the Skeleton
EXPECT_TRUE(skel->getEndEffector("manip") == nullptr);
EXPECT_TRUE(skel->getNumEndEffectors() == 0);
EXPECT_TRUE(skel->getBodyNode(0)->getNumEndEffectors() == 0);
EXPECT_TRUE(weakManip.lock() == nullptr);
}
}
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;
}
