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; }