WorkspaceGrid::WorkspaceGrid( float fMinX, float fMaxX, float fMinY, float fMaxY, float fDiscretizeSize )
{
minX = fMinX;
maxX = fMaxX;
minY = fMinY;
maxY = fMaxY;
discretizeSize = fDiscretizeSize;
data = NULL;
if (fMinX>=fMaxX || fMinY>=fMaxY)
{
THROW_VR_EXCEPTION ("ERROR min >= max");
}
gridExtendX = maxX - minX;
gridExtendY = maxY - minY;
gridSizeX = (int)((maxX - minX) / discretizeSize) + 1;
gridSizeY = (int)((maxY - minY) / discretizeSize) + 1;
if (gridSizeY<=0 || gridSizeX<=0 || gridExtendX<=0 || gridExtendY<=0)
{
THROW_VR_EXCEPTION ("ERROR negative grid size");
}
VR_INFO << ": creating grid with " << gridSizeX << "x" << gridSizeY << " = " << gridSizeX*gridSizeY << " entries" << endl;
data = new int[gridSizeX*gridSizeY];
graspLink = new std::vector<GraspPtr>[gridSizeX*gridSizeY];
memset(data,0,sizeof(int)*gridSizeX*gridSizeY);
}
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot,
const std::string &name,
const std::vector< RobotNodePtr > &robotNodes,
const RobotNodePtr kinematicRoot,
const RobotNodePtr tcp,
bool registerToRobot)
{
VR_ASSERT (robot!=NULL);
if (robotNodes.empty() || !robotNodes[0])
{
VR_WARNING << " No robot nodes in set..." << endl;
} else
{
CollisionCheckerPtr cc = robotNodes[0]->getCollisionChecker();
for (unsigned int i=1;i<robotNodes.size();i++)
{
if (robotNodes[0]->getRobot() != robotNodes[i]->getRobot())
{
THROW_VR_EXCEPTION("Robot nodes belong to different robots");
}
if (cc != robotNodes[i]->getCollisionChecker())
{
THROW_VR_EXCEPTION("Robot nodes belong to different collision checkers");
}
}
}
RobotNodePtr kinematicRootNode = kinematicRoot;
if (!kinematicRootNode)
{
kinematicRootNode = robot->getRootNode();
}
RobotNodePtr tcpNode = tcp;
if (!tcpNode)
{
THROW_VR_EXCEPTION_IF(robotNodes.empty(), "can not determine the tcp node need for creating a RobotNodeSet");
tcpNode = robotNodes[robotNodes.size()-1];
}
RobotNodeSetPtr rns(new RobotNodeSet(name, robot, robotNodes, kinematicRootNode, tcpNode));
if (registerToRobot)
{
THROW_VR_EXCEPTION_IF(robot->hasRobotNodeSet(rns), "RobotNodeSet with name " << rns->getName() << " already present in the robot");
robot->registerRobotNodeSet(rns);
}
return rns;
}
VirtualRobot::ObstaclePtr ObjectIO::createObstacleFromString(const std::string& xmlString, const std::string& basePath /*= ""*/)
{
// copy string content to char array
char* y = new char[xmlString.size() + 1];
strncpy(y, xmlString.c_str(), xmlString.size() + 1);
VirtualRobot::ObstaclePtr obj;
try
{
rapidxml::xml_document<char> doc; // character type defaults to char
doc.parse<0>(y); // 0 means default parse flags
rapidxml::xml_node<char>* objectXMLNode = doc.first_node("Obstacle");
obj = processObstacle(objectXMLNode, basePath);
}
catch (rapidxml::parse_error& e)
{
delete[] y;
THROW_VR_EXCEPTION("Could not parse data in xml definition" << endl
<< "Error message:" << e.what() << endl
<< "Position: " << endl << e.where<char>() << endl);
return ObstaclePtr();
}
catch (VirtualRobot::VirtualRobotException&)
{
// rethrow the current exception
delete[] y;
throw;
}
catch (std::exception& e)
{
delete[] y;
THROW_VR_EXCEPTION("Error while parsing xml definition" << endl
<< "Error code:" << e.what() << endl);
return ObstaclePtr();
}
catch (...)
{
delete[] y;
THROW_VR_EXCEPTION("Error while parsing xml definition" << endl);
return ObstaclePtr();
}
delete[] y;
return obj;
}
GraspPtr ObjectIO::processGrasp(rapidxml::xml_node<char>* graspXMLNode, const std::string& robotType, const std::string& eef, const std::string& objName)
{
THROW_VR_EXCEPTION_IF(!graspXMLNode, "No <Grasp> tag ?!");
// get name
std::string name = processNameAttribute(graspXMLNode, true);
std::string method = processStringAttribute("creation", graspXMLNode, true);
float quality = processFloatAttribute(std::string("quality"), graspXMLNode, true);
std::string preshapeName = processStringAttribute("preshape", graspXMLNode, true);
Eigen::Matrix4f pose;
pose.setIdentity();
std::vector< RobotConfig::Configuration > configDefinitions;
std::string configName;
rapidxml::xml_node<>* node = graspXMLNode->first_node();
while (node)
{
std::string nodeName = getLowerCase(node->name());
if (nodeName == "transform")
{
processTransformNode(graspXMLNode, name, pose);
}
else if (nodeName == "configuration")
{
THROW_VR_EXCEPTION_IF(configDefinitions.size() > 0, "Only one configuration per grasp allowed");
bool c*K = processConfigurationNode(node, configDefinitions, configName);
THROW_VR_EXCEPTION_IF(!c*K, "Invalid configuration defined in grasp tag '" << name << "'." << endl);
}
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Grasp <" << name << ">." << endl);
}
node = node->next_sibling();
}
GraspPtr grasp(new Grasp(name, robotType, eef, pose, method, quality, preshapeName));
if (configDefinitions.size() > 0)
{
// create & register configs
std::map< std::string, float > rc;
for (size_t i = 0; i < configDefinitions.size(); i++)
{
rc[ configDefinitions[i].name ] = configDefinitions[i].value;
}
grasp->setConfiguration(rc);
}
return grasp;
}
SensorPtr RobotNode::getSensor(const std::string & name) const
{
for (size_t i=0;i<sensors.size();i++)
{
if (sensors[i]->getName() == name)
return sensors[i];
}
THROW_VR_EXCEPTION("No sensor with name" << name << " registerd at robot node " << getName());
return SensorPtr();
}
/*!
*/
VoxelTree6D(float minExtend[6], float maxExtend[6], float discretizationTransl, float discretizationRot, bool verbose = false):
verbose(verbose)
{
memcpy (&(this->minExtend[0]),&(minExtend[0]),sizeof(float)*6);
memcpy (&(this->maxExtend[0]),&(maxExtend[0]),sizeof(float)*6);
if (discretizationTransl<=0.0f || discretizationRot<=0.0f)
{
THROW_VR_EXCEPTION ("INVALID parameters");
}
float size[6];
for (int i=0;i<6;i++)
{
size[i] = maxExtend[i] - minExtend[i];
THROW_VR_EXCEPTION_IF(size[i]<=0.0f,"Invalid extend parameters?!");
}
float maxSize = 0;
for (int i=0;i<3;i++)
{
if (size[i] > maxSize)
maxSize = size[i];
}
int steps = (int)(maxSize / discretizationTransl + 0.5f);
float maxSize2 = 0;
for (int i=3;i<6;i++)
{
if (size[i] > maxSize2)
maxSize2 = size[i];
}
int steps2 = (int)(maxSize2 / discretizationRot + 0.5f);
if (steps2 > steps)
steps = steps2;
maxLevels = int(ceil(sqrt(double(steps))));
if (verbose)
{
VR_INFO << "Creating Voxelized tree data structure. " << endl;
VR_INFO << "Extends (min/max/size):" << endl;
std::streamsize pr = std::cout.precision(2);
for (int i=0;i<6;i++)
{
cout << std::fixed << minExtend[i] << "," << maxExtend[i] << " -> " << size[i] << endl;
}
std::cout << std::resetiosflags(std::ios::fixed);
std::cout.precision(pr);
VR_INFO << "discretizationTransl:" << discretizationTransl << ". Max translation levels:" << steps << endl;
VR_INFO << "discretizationRot:" << discretizationRot << ". Max rotation levels:" << steps2 << endl;
VR_INFO << "--> Max Levels:" << maxLevels << endl;
}
THROW_VR_EXCEPTION_IF (steps<=0,"Invalid parameters...");
root = new VoxelTree6DElement<T>(minExtend,size,0,maxLevels);
}
bool SceneIO::processSceneObjectSet(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene )
{
THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneObjectSet");
// get name
std::string sosName = processNameAttribute(sceneXMLNode,true);
if (sosName.empty())
{
THROW_VR_EXCEPTION("Please specify the name of the scene object set...");
return false;
}
SceneObjectSetPtr sos(new SceneObjectSet(sosName));
rapidxml::xml_node<>* node = sceneXMLNode->first_node();
while (node)
{
std::string nodeName = getLowerCase(node->name());
if (nodeName == "sceneobject")
{
std::string oname = processNameAttribute(node);
THROW_VR_EXCEPTION_IF(!(scene->hasObstacle(oname) || scene->hasManipulationObject(oname)), " Object with name '" << oname << "' not known in scene '" << scene->getName() << "'." << endl);
if (scene->hasObstacle(oname))
{
sos->addSceneObject(scene->getObstacle(oname));
} else
sos->addSceneObject(scene->getManipulationObject(oname));
} else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in scene's SceneObjectSet definition <" << sosName << ">." << endl);
}
node = node->next_sibling();
}
scene->registerSceneObjectSet(sos);
return true;
}
GraspSetPtr ObjectIO::processGraspSet(rapidxml::xml_node<char>* graspSetXMLNode, const std::string& objName)
{
THROW_VR_EXCEPTION_IF(!graspSetXMLNode, "No <GraspSet> tag ?!");
// get name
std::string gsName = processNameAttribute(graspSetXMLNode, true);
std::string gsRobotType = processStringAttribute(std::string("robottype"), graspSetXMLNode, true);
std::string gsEEF = processStringAttribute(std::string("endeffector"), graspSetXMLNode, true);
if (gsName.empty() || gsRobotType.empty() || gsEEF.empty())
{
THROW_VR_EXCEPTION("GraspSet tags must have valid attributes 'Name', 'RobotType' and 'EndEffector'");
}
GraspSetPtr result(new GraspSet(gsName, gsRobotType, gsEEF));
rapidxml::xml_node<>* node = graspSetXMLNode->first_node();
while (node)
{
std::string nodeName = getLowerCase(node->name());
if (nodeName == "grasp")
{
GraspPtr grasp = processGrasp(node, gsRobotType, gsEEF, objName);
THROW_VR_EXCEPTION_IF(!grasp, "Invalid 'Grasp' tag in '" << objName << "'." << endl);
result->addGrasp(grasp);
}
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in GraspSet <" << gsName << ">." << endl);
}
node = node->next_sibling();
}
return result;
}
Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf &m) const
{
#ifdef CHECK_PERFORMANCE
clock_t startT = clock();
#endif
MatrixXf pseudo;
switch (inverseMethod)
{
case eTranspose:
{
if (jointWeights.rows() == m.cols())
{
Eigen::MatrixXf W = jointWeights.asDiagonal();
Eigen::MatrixXf W_1 = W.inverse();
pseudo = W_1 * m.transpose() * (m*W_1*m.transpose()).inverse();
}
else
{
pseudo = m.transpose() * (m*m.transpose()).inverse();
}
break;
}
case eSVD:
{
float pinvtoler = 0.00001f;
pseudo = MathTools::getPseudoInverse(m, pinvtoler);
break;
}
case eSVDDamped:
{
float pinvtoler = 0.00001f;
pseudo = MathTools::getPseudoInverseDamped(m,pinvtoler);
break;
}
default:
THROW_VR_EXCEPTION("Inverse Jacobi Method nyi...");
}
#ifdef CHECK_PERFORMANCE
clock_t endT = clock();
float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
//if (diffClock>10.0f)
cout << "Inverse Jacobi time:" << diffClock << endl;
#endif
return pseudo;
}
void RobotNode::setGlobalPose( const Eigen::Matrix4f &pose )
{
THROW_VR_EXCEPTION("Use setJointValues to control the position of a RobotNode");
}
bool RobotNodeSet::removeSceneObject( SceneObjectPtr sceneObject )
{
THROW_VR_EXCEPTION("Not allowed for RobotNodeSets.");
return false;
}
bool RobotNodeSet::addSceneObjects( std::vector<RobotNodePtr> robotNodes )
{
THROW_VR_EXCEPTION("Not allowed for RobotNodeSets.");
return false;
}
bool RobotNodeSet::addSceneObjects( RobotNodeSetPtr robotNodeSet )
{
THROW_VR_EXCEPTION("Not allowed for RobotNodeSets.");
return false;
}
ObstaclePtr ObjectIO::processObstacle(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
{
THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <Obstacle> tag in XML definition");
bool visuProcessed = false;
bool colProcessed = false;
VisualizationNodePtr visualizationNode;
CollisionModelPtr collisionModel;
bool useAsColModel = false;
SceneObject::Physics physics;
bool physicsDefined = false;
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);
ObstaclePtr result = loadObstacle(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(), "Obstacle 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 Obstacle '" << objName << "'." << endl);
visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel);
visuProcessed = true;
if (useAsColModel)
{
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << 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 Obstacle '" << objName << "'." << endl);
collisionModel = processCollisionTag(node, objName, basePath);
colProcessed = true;
}
else if (nodeName == "physics")
{
THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in Obstacle '" << objName << "'." << endl);
processPhysicsTag(node, objName, physics);
physicsDefined = true;
}
else if (nodeName == "globalpose")
{
processTransformNode(node, objName, globalPose);
}
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Obstacle <" << objName << ">." << endl);
}
node = node->next_sibling();
}
// build object
ObstaclePtr object(new Obstacle(objName, visualizationNode, collisionModel, physics));
object->setGlobalPose(globalPose);
return object;
}
bool SceneIO::processSceneRobot(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath )
{
THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneRobot");
// get name
std::string robotName = processNameAttribute(sceneXMLNode,true);
if (robotName.empty())
{
THROW_VR_EXCEPTION("Please specify the name of the robot...");
return false;
}
std::string initStr("initconfig");
std::string initConfigName = processStringAttribute(initStr,sceneXMLNode,true);
std::vector< RobotConfigPtr > configs;
std::vector< std::vector< RobotConfig::Configuration > > configDefinitions;
std::vector< std::string > configNames;
Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
std::string fileName;
rapidxml::xml_node<>* node = sceneXMLNode->first_node();
std::vector< rapidxml::xml_node<>* > rnsNodes;
while (node)
{
std::string nodeName = getLowerCase(node->name());
if (nodeName == "file")
{
THROW_VR_EXCEPTION_IF(!fileName.empty(), "Multiple files defined in scene's robot tag '" << robotName << "'." << endl);
fileName = processFileNode(node,basePath);
} else if (nodeName == "configuration")
{
bool c*K = processConfigurationNodeList(node, configDefinitions, configNames);
THROW_VR_EXCEPTION_IF(!c*K, "Invalid configuration defined in scene's robot tag '" << robotName << "'." << endl);
} else if (nodeName == "globalpose")
{
processTransformNode(node, robotName, globalPose);
} else if (nodeName == "robotnodeset")
{
rnsNodes.push_back(node);
} else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in scene's Robot definition <" << robotName << ">." << endl);
}
node = node->next_sibling();
}
// create & register robot
THROW_VR_EXCEPTION_IF(fileName.empty(), "Missing file definition in scene's robot tag '" << robotName << "'." << endl);
RobotPtr robot = RobotIO::loadRobot(fileName);
THROW_VR_EXCEPTION_IF(!robot, "Invalid robot file in scene's robot tag '" << robotName << "'." << endl);
robot->setGlobalPose(globalPose);
scene->registerRobot(robot);
// create & register node sets
int rnsNr = 0;
for (size_t i=0;i<rnsNodes.size();i++)
{
// registers rns to robot
RobotNodeSetPtr r = processRobotNodeSet(rnsNodes[i], robot, robot->getRootNode()->getName(), rnsNr);
THROW_VR_EXCEPTION_IF(!r, "Invalid RobotNodeSet definition " << endl);
}
// create & register configs
THROW_VR_EXCEPTION_IF(configDefinitions.size()!=configNames.size(), "Invalid RobotConfig definitions " << endl);
for (size_t i=0;i<configDefinitions.size();i++)
{
RobotConfigPtr rc(new RobotConfig(robot,configNames[i],configDefinitions[i]));
scene->registerRobotConfig(robot,rc);
}
// process init config
if (!initConfigName.empty())
{
THROW_VR_EXCEPTION_IF(!scene->hasRobotConfig(robot,initConfigName), "Scene's robot tag '" << robotName << "' does not have the initConfig '" << initConfigName << "'." << endl);
robot->setJointValues(scene->getRobotConfig(robot,initConfigName));
}
return true;
}
ScenePtr SceneIO::processScene(rapidxml::xml_node<char>* sceneXMLNode, const std::string& basePath)
{
THROW_VR_EXCEPTION_IF(!sceneXMLNode, "No <Scene> tag in XML definition");
// process Attributes
ScenePtr scene;
scene = processSceneAttributes(sceneXMLNode);
// process xml nodes
std::vector<rapidxml::xml_node<char>* > sceneSetNodes;
std::vector<rapidxml::xml_node<char>* > trajectoryNodes;
rapidxml::xml_node<> *XMLNode = sceneXMLNode->first_node(NULL,0,false);
while (XMLNode)
{
std::string nodeName_ = XMLNode->name();
std::string nodeName = getLowerCase(XMLNode->name());
if (nodeName == "robot")
{
bool r = processSceneRobot(XMLNode, scene, basePath);
if (!r)
{
std::string failedNodeName = processNameAttribute(XMLNode);
THROW_VR_EXCEPTION("Failed to create robot " << failedNodeName << " in scene " << scene->getName() << endl);
}
} else if (nodeName=="obstacle")
{
bool r = processSceneObstacle(XMLNode, scene, basePath);
if (!r)
{
std::string failedNodeName = processNameAttribute(XMLNode);
THROW_VR_EXCEPTION("Failed to create obstacle " << failedNodeName << " in scene " << scene->getName() << endl);
}
} else if (nodeName=="manipulationobject")
{
bool r = processSceneManipulationObject(XMLNode, scene, basePath);
if (!r)
{
std::string failedNodeName = processNameAttribute(XMLNode);
THROW_VR_EXCEPTION("Failed to create ManipulationObject " << failedNodeName << " in scene " << scene->getName() << endl);
}
} else if (nodeName=="trajectory")
{
trajectoryNodes.push_back(XMLNode);
} else if (nodeName=="sceneobjectset")
{
sceneSetNodes.push_back(XMLNode);
} else
{
THROW_VR_EXCEPTION("XML node of type <" << nodeName_ << "> is not supported. Ignoring contents..." << endl);
}
XMLNode = XMLNode->next_sibling(NULL,0,false);
}
// process all sceneSetNodes
for (size_t i=0;i<sceneSetNodes.size();i++)
{
bool r = processSceneObjectSet(sceneSetNodes[i], scene);
if (!r)
{
std::string failedNodeName = processNameAttribute(XMLNode);
THROW_VR_EXCEPTION("Failed to create SceneObjectSet " << failedNodeName << " in scene " << scene->getName() << endl);
}
}
// process all trajectories
for (size_t i=0;i<trajectoryNodes.size();i++)
{
bool r = processSceneTrajectory(trajectoryNodes[i], scene);
if (!r)
{
std::string failedNodeName = processNameAttribute(XMLNode);
THROW_VR_EXCEPTION("Failed to create trajectory " << failedNodeName << " in scene " << scene->getName() << endl);
}
}
return scene;
}
