bool SimoxRobotViewer::setJointLimits( const std::string &robotNodeSet, std::vector<float> &min, std::vector<float> &max )
{
if (!robot)
{
VR_ERROR << "No robot..." << endl;
return false;
}
lock();
RobotNodeSetPtr rns = robot->getRobotNodeSet(robotNodeSet);
if (!rns)
{
VR_ERROR << "No robot node set with name " << robotNodeSet << endl;
unlock();
return false;
}
if (rns->getSize()!=min.size() || rns->getSize()!=max.size())
{
VR_ERROR << "Wrong sizes. RNS (" << robotNodeSet << ") : " << rns->getSize() <<", minSize:" << min.size() << ", maxSize:" << max.size() << endl;
unlock();
return false;
}
for (size_t i=0;i<min.size();i++)
{
RobotNodePtr rn = rns->getNode(i);
if (!rn)
return false;
rn->setJointLimits(min[i],max[i]);
}
unlock();
return true;
}
Trajectory::Trajectory(RobotNodeSetPtr rns, const std::string &name)
:rns(rns),name(name)
{
THROW_VR_EXCEPTION_IF(!rns,"Need a rns defined...");
dimension = rns->getSize();
THROW_VR_EXCEPTION_IF(dimension==0,"No joints defined in RNS...");
}
CoMIK::CoMIK(RobotNodeSetPtr rnsJoints, RobotNodeSetPtr rnsBodies, RobotNodePtr coordSystem, int dimensions)
: JacobiProvider(rnsJoints), coordSystem(coordSystem)
{
VR_ASSERT(rns);
VR_ASSERT(rnsBodies);
checkImprovement = false;
this->rnsBodies = rnsBodies;
numDimensions = dimensions;
bodyNodes = rnsBodies->getAllRobotNodes();
for (size_t i = 0; i < bodyNodes.size(); i++)
{
// get all joints that influence the body
std::vector<RobotNodePtr> parentsN = bodyNodes[i]->getAllParents(rns);
bodyNodeParents[bodyNodes[i]] = parentsN;
}
if (rnsBodies->getMass()==0)
{
VR_ERROR << "The RNS does not contain any bodies or masses are not specified (mass==0)" << endl;
}
}
std::vector<RobotNodePtr> RobotNode::getAllParents( RobotNodeSetPtr rns )
{
std::vector<RobotNodePtr> result;
if (!rns)
return result;
std::vector<RobotNodePtr> rn = rns->getAllRobotNodes();
for (unsigned int i=0; i<rn.size(); i++)
{
if (rn[i]->hasChild(static_pointer_cast<SceneObject>(shared_from_this()),true))
result.push_back(rn[i]);
}
return result;
}
bool SimoxRobotViewer::setJoints( const std::string &robotNodeSet, std::vector<float> jointValues )
{
if (!robot)
return false;
lock();
if (!robot->hasRobotNodeSet(robotNodeSet))
{
VR_ERROR << "No RNS with name " << robotNodeSet << " found..." << endl;
unlock();
return false;
}
RobotNodeSetPtr rns = robot->getRobotNodeSet(robotNodeSet);
if (rns->getSize() != jointValues.size())
{
VR_ERROR << "RNS with name " << robotNodeSet << " covers " << rns->getSize() << " joints, but the given joint vector is of size " << jointValues.size() << endl;
unlock();
return false;
}
rns->setJointValues(jointValues);
unlock();
redraw();
return true;
}
