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...");
}
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;
}
