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