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