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