void GraspSet::addGrasp(GraspPtr grasp)
{
VR_ASSERT_MESSAGE(grasp, "NULL grasp");
VR_ASSERT_MESSAGE(!hasGrasp(grasp), "Grasp already added!");
VR_ASSERT_MESSAGE(isCompatibleGrasp(grasp), "Grasp is not compatible with this grasp set");
grasps.push_back(grasp);
}
void RobotNode::setJointValueNoUpdate(float q)
{
VR_ASSERT_MESSAGE( initialized, "Not initialized");
VR_ASSERT_MESSAGE( (!boost::math::isnan(q) && !boost::math::isinf(q)) ,"Not a valid number...");
//std::cout << "######## Setting Joint to: " << q << " degrees" << std::endl;
if (q < jointLimitLo)
{
q = jointLimitLo;
}
if (q > jointLimitHi)
{
q = jointLimitHi;
}
jointValue = q;
}
bool GraspSet::hasGrasp(GraspPtr grasp)
{
VR_ASSERT_MESSAGE(grasp, "NULL grasp");
for (size_t i = 0; i < grasps.size(); i++)
if (grasps[i] == grasp)
{
return true;
}
return false;
}
