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