WorkspaceGrid::WorkspaceGrid( float fMinX, float fMaxX, float fMinY, float fMaxY, float fDiscretizeSize ) { minX = fMinX; maxX = fMaxX; minY = fMinY; maxY = fMaxY; discretizeSize = fDiscretizeSize; data = NULL; if (fMinX>=fMaxX || fMinY>=fMaxY) { THROW_VR_EXCEPTION ("ERROR min >= max"); } gridExtendX = maxX - minX; gridExtendY = maxY - minY; gridSizeX = (int)((maxX - minX) / discretizeSize) + 1; gridSizeY = (int)((maxY - minY) / discretizeSize) + 1; if (gridSizeY<=0 || gridSizeX<=0 || gridExtendX<=0 || gridExtendY<=0) { THROW_VR_EXCEPTION ("ERROR negative grid size"); } VR_INFO << ": creating grid with " << gridSizeX << "x" << gridSizeY << " = " << gridSizeX*gridSizeY << " entries" << endl; data = new int[gridSizeX*gridSizeY]; graspLink = new std::vector<GraspPtr>[gridSizeX*gridSizeY]; memset(data,0,sizeof(int)*gridSizeX*gridSizeY); }
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::ObstaclePtr ObjectIO::createObstacleFromString(const std::string& xmlString, const std::string& basePath /*= ""*/) { // copy string content to char array char* y = new char[xmlString.size() + 1]; strncpy(y, xmlString.c_str(), xmlString.size() + 1); VirtualRobot::ObstaclePtr obj; try { rapidxml::xml_document<char> doc; // character type defaults to char doc.parse<0>(y); // 0 means default parse flags rapidxml::xml_node<char>* objectXMLNode = doc.first_node("Obstacle"); obj = processObstacle(objectXMLNode, basePath); } catch (rapidxml::parse_error& e) { delete[] y; THROW_VR_EXCEPTION("Could not parse data in xml definition" << endl << "Error message:" << e.what() << endl << "Position: " << endl << e.where<char>() << endl); return ObstaclePtr(); } catch (VirtualRobot::VirtualRobotException&) { // rethrow the current exception delete[] y; throw; } catch (std::exception& e) { delete[] y; THROW_VR_EXCEPTION("Error while parsing xml definition" << endl << "Error code:" << e.what() << endl); return ObstaclePtr(); } catch (...) { delete[] y; THROW_VR_EXCEPTION("Error while parsing xml definition" << endl); return ObstaclePtr(); } delete[] y; return obj; }
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; }
SensorPtr RobotNode::getSensor(const std::string & name) const { for (size_t i=0;i<sensors.size();i++) { if (sensors[i]->getName() == name) return sensors[i]; } THROW_VR_EXCEPTION("No sensor with name" << name << " registerd at robot node " << getName()); return SensorPtr(); }
/*! */ 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); }
bool SceneIO::processSceneObjectSet(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene ) { THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneObjectSet"); // get name std::string sosName = processNameAttribute(sceneXMLNode,true); if (sosName.empty()) { THROW_VR_EXCEPTION("Please specify the name of the scene object set..."); return false; } SceneObjectSetPtr sos(new SceneObjectSet(sosName)); rapidxml::xml_node<>* node = sceneXMLNode->first_node(); while (node) { std::string nodeName = getLowerCase(node->name()); if (nodeName == "sceneobject") { std::string oname = processNameAttribute(node); THROW_VR_EXCEPTION_IF(!(scene->hasObstacle(oname) || scene->hasManipulationObject(oname)), " Object with name '" << oname << "' not known in scene '" << scene->getName() << "'." << endl); if (scene->hasObstacle(oname)) { sos->addSceneObject(scene->getObstacle(oname)); } else sos->addSceneObject(scene->getManipulationObject(oname)); } else { THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in scene's SceneObjectSet definition <" << sosName << ">." << endl); } node = node->next_sibling(); } scene->registerSceneObjectSet(sos); return true; }
GraspSetPtr ObjectIO::processGraspSet(rapidxml::xml_node<char>* graspSetXMLNode, const std::string& objName) { THROW_VR_EXCEPTION_IF(!graspSetXMLNode, "No <GraspSet> tag ?!"); // get name std::string gsName = processNameAttribute(graspSetXMLNode, true); std::string gsRobotType = processStringAttribute(std::string("robottype"), graspSetXMLNode, true); std::string gsEEF = processStringAttribute(std::string("endeffector"), graspSetXMLNode, true); if (gsName.empty() || gsRobotType.empty() || gsEEF.empty()) { THROW_VR_EXCEPTION("GraspSet tags must have valid attributes 'Name', 'RobotType' and 'EndEffector'"); } GraspSetPtr result(new GraspSet(gsName, gsRobotType, gsEEF)); rapidxml::xml_node<>* node = graspSetXMLNode->first_node(); while (node) { std::string nodeName = getLowerCase(node->name()); if (nodeName == "grasp") { GraspPtr grasp = processGrasp(node, gsRobotType, gsEEF, objName); THROW_VR_EXCEPTION_IF(!grasp, "Invalid 'Grasp' tag in '" << objName << "'." << endl); result->addGrasp(grasp); } else { THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in GraspSet <" << gsName << ">." << endl); } node = node->next_sibling(); } return result; }
Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf &m) const { #ifdef CHECK_PERFORMANCE clock_t startT = clock(); #endif MatrixXf pseudo; switch (inverseMethod) { case eTranspose: { if (jointWeights.rows() == m.cols()) { Eigen::MatrixXf W = jointWeights.asDiagonal(); Eigen::MatrixXf W_1 = W.inverse(); pseudo = W_1 * m.transpose() * (m*W_1*m.transpose()).inverse(); } else { pseudo = m.transpose() * (m*m.transpose()).inverse(); } break; } case eSVD: { float pinvtoler = 0.00001f; pseudo = MathTools::getPseudoInverse(m, pinvtoler); break; } case eSVDDamped: { float pinvtoler = 0.00001f; pseudo = MathTools::getPseudoInverseDamped(m,pinvtoler); break; } default: THROW_VR_EXCEPTION("Inverse Jacobi Method nyi..."); } #ifdef CHECK_PERFORMANCE clock_t endT = clock(); float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f); //if (diffClock>10.0f) cout << "Inverse Jacobi time:" << diffClock << endl; #endif return pseudo; }
void RobotNode::setGlobalPose( const Eigen::Matrix4f &pose ) { THROW_VR_EXCEPTION("Use setJointValues to control the position of a RobotNode"); }
bool RobotNodeSet::removeSceneObject( SceneObjectPtr sceneObject ) { THROW_VR_EXCEPTION("Not allowed for RobotNodeSets."); return false; }
bool RobotNodeSet::addSceneObjects( std::vector<RobotNodePtr> robotNodes ) { THROW_VR_EXCEPTION("Not allowed for RobotNodeSets."); return false; }
bool RobotNodeSet::addSceneObjects( RobotNodeSetPtr robotNodeSet ) { THROW_VR_EXCEPTION("Not allowed for RobotNodeSets."); return false; }
ObstaclePtr ObjectIO::processObstacle(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath) { THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <Obstacle> tag in XML definition"); bool visuProcessed = false; bool colProcessed = false; VisualizationNodePtr visualizationNode; CollisionModelPtr collisionModel; bool useAsColModel = false; SceneObject::Physics physics; bool physicsDefined = false; Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity(); // get name std::string objName = processNameAttribute(objectXMLNode); // first check if there is an xml file to load rapidxml::xml_node<>* xmlFileNode = objectXMLNode->first_node("file", 0, false); if (xmlFileNode) { std::string xmlFile = processFileNode(xmlFileNode, basePath); ObstaclePtr result = loadObstacle(xmlFile); if (!result) { return result; } if (!objName.empty()) { result->setName(objName); } // update global pose rapidxml::xml_node<>* poseNode = objectXMLNode->first_node("globalpose", 0, false); if (poseNode) { processTransformNode(poseNode, objName, globalPose); result->setGlobalPose(globalPose); } return result; } THROW_VR_EXCEPTION_IF(objName.empty(), "Obstacle definition expects attribute 'name'"); rapidxml::xml_node<>* node = objectXMLNode->first_node(); while (node) { std::string nodeName = getLowerCase(node->name()); if (nodeName == "visualization") { THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in Obstacle '" << objName << "'." << endl); visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel); visuProcessed = true; if (useAsColModel) { THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl); std::string colModelName = objName; colModelName += "_VISU_ColModel"; // todo: ID? collisionModel.reset(new CollisionModel(visualizationNode, colModelName, CollisionCheckerPtr())); colProcessed = true; } } else if (nodeName == "collisionmodel") { THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl); collisionModel = processCollisionTag(node, objName, basePath); colProcessed = true; } else if (nodeName == "physics") { THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in Obstacle '" << objName << "'." << endl); processPhysicsTag(node, objName, physics); physicsDefined = true; } else if (nodeName == "globalpose") { processTransformNode(node, objName, globalPose); } else { THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Obstacle <" << objName << ">." << endl); } node = node->next_sibling(); } // build object ObstaclePtr object(new Obstacle(objName, visualizationNode, collisionModel, physics)); object->setGlobalPose(globalPose); return object; }
bool SceneIO::processSceneRobot(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath ) { THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneRobot"); // get name std::string robotName = processNameAttribute(sceneXMLNode,true); if (robotName.empty()) { THROW_VR_EXCEPTION("Please specify the name of the robot..."); return false; } std::string initStr("initconfig"); std::string initConfigName = processStringAttribute(initStr,sceneXMLNode,true); std::vector< RobotConfigPtr > configs; std::vector< std::vector< RobotConfig::Configuration > > configDefinitions; std::vector< std::string > configNames; Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity(); std::string fileName; rapidxml::xml_node<>* node = sceneXMLNode->first_node(); std::vector< rapidxml::xml_node<>* > rnsNodes; while (node) { std::string nodeName = getLowerCase(node->name()); if (nodeName == "file") { THROW_VR_EXCEPTION_IF(!fileName.empty(), "Multiple files defined in scene's robot tag '" << robotName << "'." << endl); fileName = processFileNode(node,basePath); } else if (nodeName == "configuration") { bool c*K = processConfigurationNodeList(node, configDefinitions, configNames); THROW_VR_EXCEPTION_IF(!c*K, "Invalid configuration defined in scene's robot tag '" << robotName << "'." << endl); } else if (nodeName == "globalpose") { processTransformNode(node, robotName, globalPose); } else if (nodeName == "robotnodeset") { rnsNodes.push_back(node); } else { THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in scene's Robot definition <" << robotName << ">." << endl); } node = node->next_sibling(); } // create & register robot THROW_VR_EXCEPTION_IF(fileName.empty(), "Missing file definition in scene's robot tag '" << robotName << "'." << endl); RobotPtr robot = RobotIO::loadRobot(fileName); THROW_VR_EXCEPTION_IF(!robot, "Invalid robot file in scene's robot tag '" << robotName << "'." << endl); robot->setGlobalPose(globalPose); scene->registerRobot(robot); // create & register node sets int rnsNr = 0; for (size_t i=0;i<rnsNodes.size();i++) { // registers rns to robot RobotNodeSetPtr r = processRobotNodeSet(rnsNodes[i], robot, robot->getRootNode()->getName(), rnsNr); THROW_VR_EXCEPTION_IF(!r, "Invalid RobotNodeSet definition " << endl); } // create & register configs THROW_VR_EXCEPTION_IF(configDefinitions.size()!=configNames.size(), "Invalid RobotConfig definitions " << endl); for (size_t i=0;i<configDefinitions.size();i++) { RobotConfigPtr rc(new RobotConfig(robot,configNames[i],configDefinitions[i])); scene->registerRobotConfig(robot,rc); } // process init config if (!initConfigName.empty()) { THROW_VR_EXCEPTION_IF(!scene->hasRobotConfig(robot,initConfigName), "Scene's robot tag '" << robotName << "' does not have the initConfig '" << initConfigName << "'." << endl); robot->setJointValues(scene->getRobotConfig(robot,initConfigName)); } return true; }
ScenePtr SceneIO::processScene(rapidxml::xml_node<char>* sceneXMLNode, const std::string& basePath) { THROW_VR_EXCEPTION_IF(!sceneXMLNode, "No <Scene> tag in XML definition"); // process Attributes ScenePtr scene; scene = processSceneAttributes(sceneXMLNode); // process xml nodes std::vector<rapidxml::xml_node<char>* > sceneSetNodes; std::vector<rapidxml::xml_node<char>* > trajectoryNodes; rapidxml::xml_node<> *XMLNode = sceneXMLNode->first_node(NULL,0,false); while (XMLNode) { std::string nodeName_ = XMLNode->name(); std::string nodeName = getLowerCase(XMLNode->name()); if (nodeName == "robot") { bool r = processSceneRobot(XMLNode, scene, basePath); if (!r) { std::string failedNodeName = processNameAttribute(XMLNode); THROW_VR_EXCEPTION("Failed to create robot " << failedNodeName << " in scene " << scene->getName() << endl); } } else if (nodeName=="obstacle") { bool r = processSceneObstacle(XMLNode, scene, basePath); if (!r) { std::string failedNodeName = processNameAttribute(XMLNode); THROW_VR_EXCEPTION("Failed to create obstacle " << failedNodeName << " in scene " << scene->getName() << endl); } } else if (nodeName=="manipulationobject") { bool r = processSceneManipulationObject(XMLNode, scene, basePath); if (!r) { std::string failedNodeName = processNameAttribute(XMLNode); THROW_VR_EXCEPTION("Failed to create ManipulationObject " << failedNodeName << " in scene " << scene->getName() << endl); } } else if (nodeName=="trajectory") { trajectoryNodes.push_back(XMLNode); } else if (nodeName=="sceneobjectset") { sceneSetNodes.push_back(XMLNode); } else { THROW_VR_EXCEPTION("XML node of type <" << nodeName_ << "> is not supported. Ignoring contents..." << endl); } XMLNode = XMLNode->next_sibling(NULL,0,false); } // process all sceneSetNodes for (size_t i=0;i<sceneSetNodes.size();i++) { bool r = processSceneObjectSet(sceneSetNodes[i], scene); if (!r) { std::string failedNodeName = processNameAttribute(XMLNode); THROW_VR_EXCEPTION("Failed to create SceneObjectSet " << failedNodeName << " in scene " << scene->getName() << endl); } } // process all trajectories for (size_t i=0;i<trajectoryNodes.size();i++) { bool r = processSceneTrajectory(trajectoryNodes[i], scene); if (!r) { std::string failedNodeName = processNameAttribute(XMLNode); THROW_VR_EXCEPTION("Failed to create trajectory " << failedNodeName << " in scene " << scene->getName() << endl); } } return scene; }