void SimDynamicsWindow::addObject() { ManipulationObjectPtr vitalis; std::string vitalisPath = "objects/VitalisWithPrimitives.xml"; if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(vitalisPath)) { vitalis = ObjectIO::loadManipulationObject(vitalisPath); } if (vitalis) { vitalis->setMass(0.5f); float x, y, z; int counter = 0; do { x = float(rand() % 2000 - 1000); y = float(rand() % 2000 - 1000); z = float(rand() % 2000 + 100); Eigen::Matrix4f gp = vitalis->getGlobalPose(); gp.block(0, 3, 3, 1) = Eigen::Vector3f(x, y, z); vitalis->setGlobalPose(gp); counter++; if (counter > 100) { cout << "Error, could not find valid pose" << endl; return; } } while (robot && CollisionChecker::getGlobalCollisionChecker()->checkCollision(robot, vitalis)); SimDynamics::DynamicsObjectPtr dynamicsObjectVitalis = dynamicsWorld->CreateDynamicsObject(vitalis); dynamicsObjectVitalis->setPosition(Eigen::Vector3f(x, y, z)); dynamicsObjects.push_back(dynamicsObjectVitalis); dynamicsWorld->addObject(dynamicsObjectVitalis); buildVisualization(); } }
ManipulationObjectPtr ObjectIO::processManipulationObject(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath) { THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <ManipulationObject> tag in XML definition"); bool visuProcessed = false; bool colProcessed = false; VisualizationNodePtr visualizationNode; CollisionModelPtr collisionModel; bool useAsColModel = false; SceneObject::Physics physics; bool physicsDefined = false; std::vector<GraspSetPtr> graspSets; 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); ManipulationObjectPtr result = loadManipulationObject(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(), "ManipulationObject 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 ManipulationObject '" << objName << "'." << endl); visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel); visuProcessed = true; if (useAsColModel) { THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in ManipulationObject '" << 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 ManipulationObject '" << objName << "'." << endl); collisionModel = processCollisionTag(node, objName, basePath); colProcessed = true; } else if (nodeName == "physics") { THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in ManipulationObject '" << objName << "'." << endl); processPhysicsTag(node, objName, physics); physicsDefined = true; } else if (nodeName == "graspset") { GraspSetPtr gs = processGraspSet(node, objName); THROW_VR_EXCEPTION_IF(!gs, "Invalid grasp set in '" << objName << "'." << endl); graspSets.push_back(gs); } else if (nodeName == "globalpose") { processTransformNode(node, objName, globalPose); } else { THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in ManipulationObject <" << objName << ">." << endl); } node = node->next_sibling(); } // build object ManipulationObjectPtr object(new ManipulationObject(objName, visualizationNode, collisionModel, physics)); for (size_t i = 0; i < graspSets.size(); i++) { object->addGraspSet(graspSets[i]); } object->setGlobalPose(globalPose); return object; }