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(); } }
bool ObjectIO::saveManipulationObject(ManipulationObjectPtr object, const std::string& xmlFile) { THROW_VR_EXCEPTION_IF(!object, "NULL object"); boost::filesystem::path filenameBaseComplete(xmlFile); boost::filesystem::path filenameBasePath = filenameBaseComplete.branch_path(); std::string basePath = filenameBasePath.string(); std::string xmlString = object->toXML(basePath); // save file return BaseIO::writeXMLFile(xmlFile, xmlString, true); }
bool WorkspaceGrid::fillGridData( WorkspaceRepresentationPtr ws, ManipulationObjectPtr o, GraspPtr g, RobotNodePtr baseRobotNode ) { if (!ws || !o || !g) return false; Eigen::Matrix4f graspGlobal = g->getTcpPoseGlobal(o->getGlobalPose()); WorkspaceRepresentation::WorkspaceCut2DPtr cutXY = ws->createCut(graspGlobal,discretizeSize); std::vector<WorkspaceRepresentation::WorkspaceCut2DTransformationPtr> transformations = ws->createCutTransformations(cutXY,baseRobotNode); setEntries(transformations,graspGlobal,g); return true; }
SimDynamicsWindow::SimDynamicsWindow(std::string& sRobotFilename, Qt::WFlags flags) : QMainWindow(NULL) { VR_INFO << " start " << endl; //this->setCaption(QString("ShowRobot - KIT - Humanoids Group")); //resize(1100, 768); useColModel = false; VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(sRobotFilename); std::string robotFilename = sRobotFilename; sceneSep = new SoSeparator; sceneSep->ref(); comSep = new SoSeparator; sceneSep->addChild(comSep); contactsSep = new SoSeparator; sceneSep->addChild(contactsSep); forceSep = new SoSeparator; sceneSep->addChild(forceSep); // optional visualizations (not considered by dynamics) SoSeparator* cc = CoinVisualizationFactory::CreateCoordSystemVisualization(10.0f); sceneSep->addChild(cc); BulletEngineConfigPtr config(new BulletEngineConfig()); config->bulletSolverIterations = 2000; config->bulletObjectRestitution = btScalar(0.1); config->bulletObjectFriction = btScalar(1.0); config->bulletSolverGlobalContactForceMixing = btScalar(0.00001); config->bulletObjectDampingLinear = btScalar(0.3); config->bulletObjectDampingAngular = btScalar(0.3); dynamicsWorld = SimDynamics::DynamicsWorld::Init(config); SIMDYNAMICS_ASSERT(dynamicsWorld); dynamicsWorld->createFloorPlane(); VirtualRobot::ObstaclePtr o = VirtualRobot::Obstacle::createBox(1000.0f, 1000.0f, 1000.0f, VirtualRobot::VisualizationFactory::Color::Blue()); o->setMass(1.0f); // 1kg dynamicsObject = dynamicsWorld->CreateDynamicsObject(o); dynamicsObject->setPosition(Eigen::Vector3f(1000, 2000, 1000.0f)); dynamicsWorld->addObject(dynamicsObject); addObject(); #if 0 std::string f = "/home/niko/coding/armarx/SimulationX/data/environment/KIT_Robot_Kitchen.xml"; ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f); mo->setSimulationType(VirtualRobot::SceneObject::Physics::eStatic); dynamicsObject2 = dynamicsWorld->CreateDynamicsObject(mo); //dynObj->setPosition(Eigen::Vector3f(3000,3000,1000.0f)); dynamicsWorld->addObject(dynamicsObject2); #endif #if 0 std::string f = "/home/SMBAD/vahrenka/.armarx/mongo/.cache/files/lowersink.xml"; ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f); mo->setSimulationType(SceneObject::Physics::eDynamic); SimDynamics::DynamicsObjectPtr dynObj2 = dynamicsWorld->CreateDynamicsObject(mo); //dynObj2->setSimType(VirtualRobot::SceneObject::Physics::eDynamic); dynObj2->setPosition(Eigen::Vector3f(0, 0, -200.0f)); dynamicsWorld->addObject(dynObj2); dynamicsObjects.push_back(dynObj2); #endif setupUI(); loadRobot(robotFilename); // build visualization buildVisualization(); viewer->viewAll(); // register callback float TIMER_MS = 30.0f; SoSensorManager* sensor_mgr = SoDB::getSensorManager(); timerSensor = new SoTimerSensor(timerCB, this); timerSensor->setInterval(SbTime(TIMER_MS / 1000.0f)); sensor_mgr->insertTimerSensor(timerSensor); }
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; }
void showSceneWindow::loadScene() { sceneVisuSep->removeAllChildren(); currentEEF.reset(); currentGrasp.reset(); currentGraspSet.reset(); currentObject.reset(); currentRobot.reset(); currentTrajectory.reset(); cout << "Loading Scene from " << sceneFile << endl; scene.reset(); try { scene = SceneIO::loadScene(sceneFile); } catch (VirtualRobotException& e) { cout << "Could not find valid scene in file " << sceneFile << endl; } if (!scene) { // try manip object try { ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(sceneFile); //mo = mo->clone(mo->getName()); if (mo) { VR_INFO << "Loaded Manipulation object:" << endl; mo->print(); scene.reset(new Scene(mo->getName())); scene->registerManipulationObject(mo); } } catch (VirtualRobotException& e) { cout << "Could not find valid manipulation object in file " << sceneFile << endl; } } if (!scene) { // try object try { ObstaclePtr mo = ObjectIO::loadObstacle(sceneFile); if (mo) { VR_INFO << "Loaded obstacle:" << endl; mo->print(); scene.reset(new Scene(mo->getName())); scene->registerObstacle(mo); } } catch (VirtualRobotException& e) { cout << "Could not find valid obstacle in file " << sceneFile << endl; } } if (!scene) { cout << " ERROR while creating scene" << endl; return; } /*std::vector<VirtualRobot::ManipulationObjectPtr> mo; mo = scene->getManipulationObjects(); cout << "Printing " << mo.size() << " objects" << endl; for (size_t i=0;i<mo.size();i++) { mo[i]->print(); mo[i]->showCoordinateSystem(true); Eigen::Vector3f c = mo[i]->getCoMGlobal(); cout << "com global: \n" << c << endl; c = mo[i]->getCoMLocal(); cout << "com local: \n" << c << endl; //mo[i]->showBoundingBox(true); }*/ /*std::vector<VirtualRobot::ObstaclePtr> o; o = scene->getObstacles(); cout << "Printing " << o.size() << " obstacles" << endl; for (size_t i=0;i<o.size();i++) { o[i]->print(); o[i]->showCoordinateSystem(true); Eigen::Vector3f c = o[i]->getCoMGlobal(); cout << "com global: \n" << c << endl; c = o[i]->getCoMLocal(); cout << "com local: \n" << c << endl; //mo[i]->showBoundingBox(true); }*/ // get nodes /*m_pRobot->getRobotNodes(allRobotNodes); m_pRobot->getRobotNodeSets(robotNodeSets); m_pRobot->getEndEffectors(eefs); updateEEFBox(); updateRNSBox(); selectRNS(0); if (allRobotNodes.size()==0) selectJoint(-1); else selectJoint(0); displayTriangles(); // build visualization collisionModel(); robotStructure();*/ updateGui(); buildVisu(); viewer->viewAll(); }