ApproachMovementGenerator::ApproachMovementGenerator(VirtualRobot::SceneObjectPtr object, VirtualRobot::EndEffectorPtr eef, const std::string& graspPreshape) : object(object), eef(eef), graspPreshape(graspPreshape) { name = "ApproachMovementGenerator"; THROW_VR_EXCEPTION_IF(!object, "NULL object?!"); THROW_VR_EXCEPTION_IF(!object->getCollisionModel(), "No collision model for object " << object->getName()); THROW_VR_EXCEPTION_IF(!eef, "NULL eef?!"); THROW_VR_EXCEPTION_IF(!eef->getGCP(), "Need a GraspCenterPoint Node defined in EEF " << eef->getName()); objectModel = object->getCollisionModel()->getTriMeshModel(); THROW_VR_EXCEPTION_IF(!objectModel, "NULL trimeshmodel of object " << object->getName()); THROW_VR_EXCEPTION_IF(objectModel->faces.size() == 0, "no faces in trimeshmodel of object " << object->getName()); eefRobot = eef->createEefRobot(eef->getName(), eef->getName()); THROW_VR_EXCEPTION_IF(!eefRobot, "Failed cloning EEF " << eef->getName()); eef_cloned = eefRobot->getEndEffector(eef->getName()); THROW_VR_EXCEPTION_IF(!eef_cloned, "No EEF with name " << eef->getName() << " in cloned robot?!"); THROW_VR_EXCEPTION_IF(!eef_cloned->getGCP(), "No GCP in EEF with name " << eef->getName()); if (!graspPreshape.empty()) { THROW_VR_EXCEPTION_IF(!eef_cloned->hasPreshape(graspPreshape), "Preshape with name " << graspPreshape << " not present in EEF"); eef_cloned->setPreshape(graspPreshape); } }
void RobotNode::checkValidRobotNodeType() { switch (nodeType) { case Generic: return; break; case Joint: THROW_VR_EXCEPTION_IF(visualizationModel, "No visualization models allowed in JointNodes"); THROW_VR_EXCEPTION_IF(collisionModel, "No collision models allowed in JointNodes"); //THROW_VR_EXCEPTION_IF(postJointTransformation != Eigen::Matrix4f::Identity() , "No postJoint transformations allowed in JointNodes"); break; case Body: //THROW_VR_EXCEPTION_IF(postJointTransformation != Eigen::Matrix4f::Identity() , "No transformations allowed in BodyNodes"); THROW_VR_EXCEPTION_IF(localTransformation != Eigen::Matrix4f::Identity() , "No transformations allowed in BodyNodes"); break; case Transform: THROW_VR_EXCEPTION_IF(visualizationModel, "No visualization models allowed in TransformationNodes"); THROW_VR_EXCEPTION_IF(collisionModel, "No collision models allowed in TransformationNodes"); break; default: VR_ERROR << "RobotNodeType nyi..." << endl; } }
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..."); }
CSpacePathPtr ShortcutProcessor::shortenSolutionRandom(int shortenLoops /*=300*/, int maxSolutionPathDist) { stopOptimization = false; THROW_VR_EXCEPTION_IF((!cspace || !path), "NULL data"); THROW_VR_EXCEPTION_IF(!initSolution(), "Could not init..."); int counter = 0; if (optimizedPath->getNrOfPoints() <= 2) { return optimizedPath; } int result = 0; int beforeCount = (int)optimizedPath->getNrOfPoints(); float beforeLength = optimizedPath->getLength(); if (verbose) { SABA_INFO << ": solution size before shortenSolutionRandom:" << beforeCount << std::endl; SABA_INFO << ": solution length before shortenSolutionRandom:" << beforeLength << std::endl; } clock_t startT = clock(); int red; int loopsOverall = 0; while (counter < shortenLoops && !stopOptimization) { loopsOverall++; red = tryRandomShortcut(maxSolutionPathDist); counter++; result += red; } if (stopOptimization) { SABA_INFO << "optimization was stopped" << std::endl; } int afterCount = (int)optimizedPath->getNrOfPoints(); float afterLength = optimizedPath->getLength(); clock_t endT = clock(); float timems = (float)(endT - startT) / (float)CLOCKS_PER_SEC * 1000.0f; if (verbose) { SABA_INFO << ": shorten loops: " << loopsOverall << std::endl; SABA_INFO << ": shorten time: " << timems << " ms " << std::endl; SABA_INFO << ": solution size after ShortenSolutionRandom (nr of positions) : " << afterCount << std::endl; SABA_INFO << ": solution length after ShortenSolutionRandom : " << afterLength << std::endl; } return optimizedPath; }
GraspPtr ObjectIO::processGrasp(rapidxml::xml_node<char>* graspXMLNode, const std::string& robotType, const std::string& eef, const std::string& objName) { THROW_VR_EXCEPTION_IF(!graspXMLNode, "No <Grasp> tag ?!"); // get name std::string name = processNameAttribute(graspXMLNode, true); std::string method = processStringAttribute("creation", graspXMLNode, true); float quality = processFloatAttribute(std::string("quality"), graspXMLNode, true); std::string preshapeName = processStringAttribute("preshape", graspXMLNode, true); Eigen::Matrix4f pose; pose.setIdentity(); std::vector< RobotConfig::Configuration > configDefinitions; std::string configName; rapidxml::xml_node<>* node = graspXMLNode->first_node(); while (node) { std::string nodeName = getLowerCase(node->name()); if (nodeName == "transform") { processTransformNode(graspXMLNode, name, pose); } else if (nodeName == "configuration") { THROW_VR_EXCEPTION_IF(configDefinitions.size() > 0, "Only one configuration per grasp allowed"); bool c*K = processConfigurationNode(node, configDefinitions, configName); THROW_VR_EXCEPTION_IF(!c*K, "Invalid configuration defined in grasp tag '" << name << "'." << endl); } else { THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Grasp <" << name << ">." << endl); } node = node->next_sibling(); } GraspPtr grasp(new Grasp(name, robotType, eef, pose, method, quality, preshapeName)); if (configDefinitions.size() > 0) { // create & register configs std::map< std::string, float > rc; for (size_t i = 0; i < configDefinitions.size(); i++) { rc[ configDefinitions[i].name ] = configDefinitions[i].value; } grasp->setConfiguration(rc); } return grasp; }
RobotConfig::RobotConfig(RobotWeakPtr robot, const std::string &name, const std::vector< RobotNodePtr > &robotNodes, const std::vector< float > &values) : name(name), robot(robot) { THROW_VR_EXCEPTION_IF(!robot.lock(),"NULL robot in RobotConfig"); THROW_VR_EXCEPTION_IF(robotNodes.size() != values.size(),"Vector sizes have to be equal in RobotConfig"); for (size_t i=0;i<robotNodes.size();i++) { setConfig(robotNodes[i],values[i]); } }
/*! */ VoxelTree6D(float minExtend[6], float maxExtend[6], float discretizationTransl, float discretizationRot, bool verbose = false): verbose(verbose) { memcpy (&(this->minExtend[0]),&(minExtend[0]),sizeof(float)*6); memcpy (&(this->maxExtend[0]),&(maxExtend[0]),sizeof(float)*6); if (discretizationTransl<=0.0f || discretizationRot<=0.0f) { THROW_VR_EXCEPTION ("INVALID parameters"); } float size[6]; for (int i=0;i<6;i++) { size[i] = maxExtend[i] - minExtend[i]; THROW_VR_EXCEPTION_IF(size[i]<=0.0f,"Invalid extend parameters?!"); } float maxSize = 0; for (int i=0;i<3;i++) { if (size[i] > maxSize) maxSize = size[i]; } int steps = (int)(maxSize / discretizationTransl + 0.5f); float maxSize2 = 0; for (int i=3;i<6;i++) { if (size[i] > maxSize2) maxSize2 = size[i]; } int steps2 = (int)(maxSize2 / discretizationRot + 0.5f); if (steps2 > steps) steps = steps2; maxLevels = int(ceil(sqrt(double(steps)))); if (verbose) { VR_INFO << "Creating Voxelized tree data structure. " << endl; VR_INFO << "Extends (min/max/size):" << endl; std::streamsize pr = std::cout.precision(2); for (int i=0;i<6;i++) { cout << std::fixed << minExtend[i] << "," << maxExtend[i] << " -> " << size[i] << endl; } std::cout << std::resetiosflags(std::ios::fixed); std::cout.precision(pr); VR_INFO << "discretizationTransl:" << discretizationTransl << ". Max translation levels:" << steps << endl; VR_INFO << "discretizationRot:" << discretizationRot << ". Max rotation levels:" << steps2 << endl; VR_INFO << "--> Max Levels:" << maxLevels << endl; } THROW_VR_EXCEPTION_IF (steps<=0,"Invalid parameters..."); root = new VoxelTree6DElement<T>(minExtend,size,0,maxLevels); }
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot, const std::string &name, const std::vector< std::string > &robotNodeNames, const std::string &kinematicRootName, const std::string &tcpName, bool registerToRobot) { VR_ASSERT (robot!=NULL); std::vector< RobotNodePtr > robotNodes; if (robotNodeNames.empty()) { VR_WARNING << " No robot nodes in set..." << endl; } else { for (unsigned int i=0;i<robotNodeNames.size();i++) { RobotNodePtr node = robot->getRobotNode(robotNodeNames[i]); THROW_VR_EXCEPTION_IF(!node, "No robot node with name " << robotNodeNames[i] << " found..."); robotNodes.push_back(node); } } RobotNodePtr kinematicRoot; if (!kinematicRootName.empty()) { RobotNodePtr node = robot->getRobotNode(kinematicRootName); THROW_VR_EXCEPTION_IF(!node, "No root node with name " << kinematicRootName << " found..."); kinematicRoot = node; } else { if (!robotNodes.empty()) kinematicRoot = robotNodes[0]; } RobotNodePtr tcp; if (!tcpName.empty()) { RobotNodePtr node = robot->getRobotNode(tcpName); THROW_VR_EXCEPTION_IF(!node, "No tcp node with name " << tcpName << " found..."); tcp = node; } else { if (!robotNodes.empty()) tcp = robotNodes[robotNodes.size()-1]; } RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(robot, name, robotNodes, kinematicRoot, tcp, registerToRobot); return rns; }
VirtualRobot::ObstaclePtr ObjectIO::loadObstacle(const std::ifstream& xmlFile, const std::string& basePath /*= ""*/) { // load file THROW_VR_EXCEPTION_IF(!xmlFile.is_open(), "Could not open XML file"); std::stringstream buffer; buffer << xmlFile.rdbuf(); std::string objectXML(buffer.str()); VirtualRobot::ObstaclePtr res = createObstacleFromString(objectXML, basePath); THROW_VR_EXCEPTION_IF(!res, "Error while parsing file."); return res; }
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot, const std::string &name, const std::vector< RobotNodePtr > &robotNodes, const RobotNodePtr kinematicRoot, const RobotNodePtr tcp, bool registerToRobot) { VR_ASSERT (robot!=NULL); if (robotNodes.empty() || !robotNodes[0]) { VR_WARNING << " No robot nodes in set..." << endl; } else { CollisionCheckerPtr cc = robotNodes[0]->getCollisionChecker(); for (unsigned int i=1;i<robotNodes.size();i++) { if (robotNodes[0]->getRobot() != robotNodes[i]->getRobot()) { THROW_VR_EXCEPTION("Robot nodes belong to different robots"); } if (cc != robotNodes[i]->getCollisionChecker()) { THROW_VR_EXCEPTION("Robot nodes belong to different collision checkers"); } } } RobotNodePtr kinematicRootNode = kinematicRoot; if (!kinematicRootNode) { kinematicRootNode = robot->getRootNode(); } RobotNodePtr tcpNode = tcp; if (!tcpNode) { THROW_VR_EXCEPTION_IF(robotNodes.empty(), "can not determine the tcp node need for creating a RobotNodeSet"); tcpNode = robotNodes[robotNodes.size()-1]; } RobotNodeSetPtr rns(new RobotNodeSet(name, robot, robotNodes, kinematicRootNode, tcpNode)); if (registerToRobot) { THROW_VR_EXCEPTION_IF(robot->hasRobotNodeSet(rns), "RobotNodeSet with name " << rns->getName() << " already present in the robot"); robot->registerRobotNodeSet(rns); } return rns; }
VirtualRobot::ManipulationObjectPtr ObjectIO::loadManipulationObject(const std::string& xmlFile) { // load file std::ifstream in(xmlFile.c_str()); THROW_VR_EXCEPTION_IF(!in.is_open(), "Could not open XML file:" << xmlFile); boost::filesystem::path filenameBaseComplete(xmlFile); boost::filesystem::path filenameBasePath = filenameBaseComplete.branch_path(); std::string basePath = filenameBasePath.string(); VirtualRobot::ManipulationObjectPtr res = loadManipulationObject(in, basePath); THROW_VR_EXCEPTION_IF(!res, "Error while parsing file " << xmlFile); res->setFilename(xmlFile); return res; }
bool RobotConfig::setConfig( RobotNodePtr node, float value ) { THROW_VR_EXCEPTION_IF(!node,"Null data"); RobotPtr r = robot.lock(); if (!r) { VR_WARNING << "Robot is already deleted, skipping update..." << endl; return false; } THROW_VR_EXCEPTION_IF(!r->hasRobotNode(node),"RobotNode with name " << r->getName() << " does not belong to robot " << r->getName()); configs[node] = value; return true; }
bool RobotNode::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr> &children) { RobotPtr rob = robot.lock(); THROW_VR_EXCEPTION_IF(!rob, "Could not init RobotNode without robot" ); // robot if (!rob->hasRobotNode(static_pointer_cast<RobotNode>(shared_from_this()))) rob->registerRobotNode(static_pointer_cast<RobotNode>(shared_from_this())); // update visualization of coordinate systems if (visualizationModel && visualizationModel->hasAttachedVisualization("CoordinateSystem")) { VisualizationNodePtr v = visualizationModel->getAttachedVisualization("CoordinateSystem"); // not needed any more! // this is a little hack: The globalPose is used to set the "local" position of the attached Visualization: // Since the attached visualizations are already positioned at the global pose of the visualizationModel, // we just need the local postJointTransform //v->setGlobalPose(postJointTransformation); } checkValidRobotNodeType(); for (size_t i=0;i<sensors.size();i++) { sensors[i]->initialize(shared_from_this()); } return SceneObject::initialize(parent,children); }
void RobotNode::updatePose(bool updateChildren) { THROW_VR_EXCEPTION_IF(!initialized, "Not initialized"); updateTransformationMatrices(); // update collision and visualization model and children SceneObject::updatePose(updateChildren); // apply propagated joint values if (propagatedJointValues.size()>0) { RobotPtr r = robot.lock(); std::map< std::string, float>::iterator it = propagatedJointValues.begin(); while (it!=propagatedJointValues.end()) { RobotNodePtr rn = r->getRobotNode(it->first); if (!rn) { VR_WARNING << "Could not propagate joint value from " << name << " to " << it->first << " because dependent joint does not exist..."; } else { rn->setJointValue(jointValue*it->second); } it++; } } }
//! constructor PathProcessingThread::PathProcessingThread(PathProcessorPtr processor) { pathProcessor = processor; THROW_VR_EXCEPTION_IF(!processor,"NULL data"); threadStarted = false; processingFinished = false; }
void ShortcutProcessor::doPathPruning() { THROW_VR_EXCEPTION_IF(!initSolution(), "Could not init"); unsigned int i = 0; while (i < optimizedPath->getNrOfPoints() - 2) { Eigen::VectorXf startConfig = optimizedPath->getPoint(i); Eigen::VectorXf endConfig = optimizedPath->getPoint(i + 2); if (cspace->isPathValid(startConfig, endConfig)) { optimizedPath->erasePosition(i + 1); if (i > 0) { i--; } } else { i++; } } }
VirtualRobot::VisualizationNodePtr VisualizationNode::CreateUnitedVisualization( const std::vector<VisualizationNodePtr> &visualizations ) { if (visualizations.size()==0) return VisualizationNodePtr(); VisualizationFactoryPtr f; std::vector<VisualizationNodePtr>::const_iterator i = visualizations.begin(); while (!f && i!=visualizations.end()) { if ((*i)->getType() != VisualizationFactory::getName()) { f = VisualizationFactory::fromName((*i)->getType(),NULL); break; } i++; } if (i==visualizations.end()) { VR_ERROR << "Could not find visualization factory. Aborting..." << endl; return VisualizationNodePtr(); } THROW_VR_EXCEPTION_IF(!f,"No VisualizationFactory"); return f->createUnitedVisualization(visualizations); }
std::string Trajectory::toXML(int tabs) const { std::stringstream ss; std::string tab = ""; for (int i=0;i<tabs;i++) tab += "\t"; RobotPtr robot = rns->getRobot(); THROW_VR_EXCEPTION_IF(( !robot || !rns),"Need a valid robot and rns"); ss << tab << "<Trajectory Robot='" << robot->getType() << "' RobotNodeSet='" << rns->getName() << "' name='" << name << "' dim='" << dimension << "'>\n"; for (unsigned int i = 0; i < path.size(); i++) { ss << tab << "\t<Point id='" << i << "'>\n"; Eigen::VectorXf c = path[i]; for (unsigned int k = 0; k < dimension; k++) { ss << tab << "\t\t<c value='" << c[k] << "'/>\n"; } ss << tab << "\t</Point>\n"; } ss << tab << "</Trajectory>\n"; return ss.str(); }
DynamicsRobot::DynamicsRobot(VirtualRobot::RobotPtr rob) { THROW_VR_EXCEPTION_IF(!rob,"NULL object"); robot = rob; sensors = rob->getSensors(); }
PoseQualityMeasurement::PoseQualityMeasurement(VirtualRobot::RobotNodeSetPtr rns) :rns(rns) { THROW_VR_EXCEPTION_IF( (!rns || !rns->getTCP()), "NULL data"); name = "PoseQualityMeasurement"; considerObstacle = false; verbose = false; }
void RobotNodeSet::getJointValues( RobotConfigPtr fillVector ) const { THROW_VR_EXCEPTION_IF(!fillVector,"NULL data"); for (size_t i=0;i<robotNodes.size();i++) { fillVector->setConfig(robotNodes[i]->getName(), robotNodes[i]->getJointValue()); } }
void RobotNode::updatePose(const Eigen::Matrix4f &globalPose, bool updateChildren) { THROW_VR_EXCEPTION_IF(!initialized, "Not initialized"); updateTransformationMatrices(globalPose); // update collision and visualization model and children SceneObject::updatePose(updateChildren); }
void RobotNodeSet::respectJointLimits(std::vector<float> &jointValues) const { THROW_VR_EXCEPTION_IF(jointValues.size() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl); for (unsigned int i=0;i<robotNodes.size();i++) { robotNodes[i]->respectJointLimits(jointValues[i]); } }
RobotConfig::RobotConfig(RobotWeakPtr robot, const std::string &name, const std::vector< Configuration > &configs) : name(name), robot(robot) { THROW_VR_EXCEPTION_IF(!robot.lock(),"NULL robot in RobotConfig"); for (std::vector< Configuration >::const_iterator i=configs.begin(); i!=configs.end(); i++ ) { setConfig((*i)); } }
RobotConfig::RobotConfig(RobotWeakPtr robot, const std::string &name, const std::map< RobotNodePtr, float > &configs) : name(name), robot(robot) { THROW_VR_EXCEPTION_IF(!robot.lock(),"NULL robot in RobotConfig"); for (std::map< RobotNodePtr, float >::const_iterator i=configs.begin(); i!=configs.end(); i++ ) { setConfig(i->first,i->second); } }
bool SceneIO::processSceneObstacle(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath ) { THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneObstacle"); ObstaclePtr o = ObjectIO::processObstacle(sceneXMLNode,basePath); if (!o) return false; scene->registerObstacle(o); return true; }
bool SceneIO::processSceneManipulationObject(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath ) { THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneManipulationObject"); ManipulationObjectPtr o = ObjectIO::processManipulationObject(sceneXMLNode,basePath); if (!o) return false; scene->registerManipulationObject(o); return true; }
bool RobotNodeSet::checkJointLimits( std::vector<float> &jointValues, bool verbose /*= false*/ ) const { THROW_VR_EXCEPTION_IF(jointValues.size() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl); bool res = true; for (unsigned int i=0;i<robotNodes.size();i++) { res = res & robotNodes[i]->checkJointLimits(jointValues[i],verbose); } return res; }
/** * This method constructs an instance of TriMeshModel and stores it in * CoinVisualizationNode::triMeshModel. * If CoinVisualizationMode::visualization is invalid VirtualRobotException * is thrown. * Otherwise CoinVisualizationNode::InventorTriangleCB() is called on the * Inventor graph stored in CoinVisualizationNode::visualization. */ void CoinVisualizationNode::createTriMeshModel() { THROW_VR_EXCEPTION_IF(!visualization, "CoinVisualizationNode::createTriMeshModel(): no Coin model present!"); if (triMeshModel) triMeshModel->clear(); else triMeshModel.reset(new TriMeshModel()); SoCallbackAction ca; ca.addTriangleCallback(SoShape::getClassTypeId(), &CoinVisualizationNode::InventorTriangleCB, triMeshModel.get()); ca.apply(visualization); }
ScenePtr SceneIO::processSceneAttributes(rapidxml::xml_node<char>* sceneXMLNode) { // process attributes of scene // get name std::string sceneName = processNameAttribute(sceneXMLNode); THROW_VR_EXCEPTION_IF (sceneName.empty(),"Scene definition expects attribute 'name'"); // build scene ScenePtr scene(new Scene(sceneName)); return scene; }