ApproachMovementGenerator::ApproachMovementGenerator(VirtualRobot::SceneObjectPtr object, VirtualRobot::EndEffectorPtr eef, const std::string& graspPreshape)
: object(object), eef(eef), graspPreshape(graspPreshape)
{
name = "ApproachMovementGenerator";
THROW_VR_EXCEPTION_IF(!object, "NULL object?!");
THROW_VR_EXCEPTION_IF(!object->getCollisionModel(), "No collision model for object " << object->getName());
THROW_VR_EXCEPTION_IF(!eef, "NULL eef?!");
THROW_VR_EXCEPTION_IF(!eef->getGCP(), "Need a GraspCenterPoint Node defined in EEF " << eef->getName());
objectModel = object->getCollisionModel()->getTriMeshModel();
THROW_VR_EXCEPTION_IF(!objectModel, "NULL trimeshmodel of object " << object->getName());
THROW_VR_EXCEPTION_IF(objectModel->faces.size() == 0, "no faces in trimeshmodel of object " << object->getName());
eefRobot = eef->createEefRobot(eef->getName(), eef->getName());
THROW_VR_EXCEPTION_IF(!eefRobot, "Failed cloning EEF " << eef->getName());
eef_cloned = eefRobot->getEndEffector(eef->getName());
THROW_VR_EXCEPTION_IF(!eef_cloned, "No EEF with name " << eef->getName() << " in cloned robot?!");
THROW_VR_EXCEPTION_IF(!eef_cloned->getGCP(), "No GCP in EEF with name " << eef->getName());
if (!graspPreshape.empty())
{
THROW_VR_EXCEPTION_IF(!eef_cloned->hasPreshape(graspPreshape), "Preshape with name " << graspPreshape << " not present in EEF");
eef_cloned->setPreshape(graspPreshape);
}
}
void RobotNode::checkValidRobotNodeType()
{
switch (nodeType)
{
case Generic:
return;
break;
case Joint:
THROW_VR_EXCEPTION_IF(visualizationModel, "No visualization models allowed in JointNodes");
THROW_VR_EXCEPTION_IF(collisionModel, "No collision models allowed in JointNodes");
//THROW_VR_EXCEPTION_IF(postJointTransformation != Eigen::Matrix4f::Identity() , "No postJoint transformations allowed in JointNodes");
break;
case Body:
//THROW_VR_EXCEPTION_IF(postJointTransformation != Eigen::Matrix4f::Identity() , "No transformations allowed in BodyNodes");
THROW_VR_EXCEPTION_IF(localTransformation != Eigen::Matrix4f::Identity() , "No transformations allowed in BodyNodes");
break;
case Transform:
THROW_VR_EXCEPTION_IF(visualizationModel, "No visualization models allowed in TransformationNodes");
THROW_VR_EXCEPTION_IF(collisionModel, "No collision models allowed in TransformationNodes");
break;
default:
VR_ERROR << "RobotNodeType nyi..." << endl;
}
}
Trajectory::Trajectory(RobotNodeSetPtr rns, const std::string &name)
:rns(rns),name(name)
{
THROW_VR_EXCEPTION_IF(!rns,"Need a rns defined...");
dimension = rns->getSize();
THROW_VR_EXCEPTION_IF(dimension==0,"No joints defined in RNS...");
}
CSpacePathPtr ShortcutProcessor::shortenSolutionRandom(int shortenLoops /*=300*/, int maxSolutionPathDist)
{
stopOptimization = false;
THROW_VR_EXCEPTION_IF((!cspace || !path), "NULL data");
THROW_VR_EXCEPTION_IF(!initSolution(), "Could not init...");
int counter = 0;
if (optimizedPath->getNrOfPoints() <= 2)
{
return optimizedPath;
}
int result = 0;
int beforeCount = (int)optimizedPath->getNrOfPoints();
float beforeLength = optimizedPath->getLength();
if (verbose)
{
SABA_INFO << ": solution size before shortenSolutionRandom:" << beforeCount << std::endl;
SABA_INFO << ": solution length before shortenSolutionRandom:" << beforeLength << std::endl;
}
clock_t startT = clock();
int red;
int loopsOverall = 0;
while (counter < shortenLoops && !stopOptimization)
{
loopsOverall++;
red = tryRandomShortcut(maxSolutionPathDist);
counter++;
result += red;
}
if (stopOptimization)
{
SABA_INFO << "optimization was stopped" << std::endl;
}
int afterCount = (int)optimizedPath->getNrOfPoints();
float afterLength = optimizedPath->getLength();
clock_t endT = clock();
float timems = (float)(endT - startT) / (float)CLOCKS_PER_SEC * 1000.0f;
if (verbose)
{
SABA_INFO << ": shorten loops: " << loopsOverall << std::endl;
SABA_INFO << ": shorten time: " << timems << " ms " << std::endl;
SABA_INFO << ": solution size after ShortenSolutionRandom (nr of positions) : " << afterCount << std::endl;
SABA_INFO << ": solution length after ShortenSolutionRandom : " << afterLength << std::endl;
}
return optimizedPath;
}
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;
}
RobotConfig::RobotConfig(RobotWeakPtr robot, const std::string &name, const std::vector< RobotNodePtr > &robotNodes, const std::vector< float > &values)
: name(name),
robot(robot)
{
THROW_VR_EXCEPTION_IF(!robot.lock(),"NULL robot in RobotConfig");
THROW_VR_EXCEPTION_IF(robotNodes.size() != values.size(),"Vector sizes have to be equal in RobotConfig");
for (size_t i=0;i<robotNodes.size();i++)
{
setConfig(robotNodes[i],values[i]);
}
}
/*!
*/
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);
}
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot,
const std::string &name,
const std::vector< std::string > &robotNodeNames,
const std::string &kinematicRootName,
const std::string &tcpName,
bool registerToRobot)
{
VR_ASSERT (robot!=NULL);
std::vector< RobotNodePtr > robotNodes;
if (robotNodeNames.empty())
{
VR_WARNING << " No robot nodes in set..." << endl;
}
else
{
for (unsigned int i=0;i<robotNodeNames.size();i++)
{
RobotNodePtr node = robot->getRobotNode(robotNodeNames[i]);
THROW_VR_EXCEPTION_IF(!node, "No robot node with name " << robotNodeNames[i] << " found...");
robotNodes.push_back(node);
}
}
RobotNodePtr kinematicRoot;
if (!kinematicRootName.empty())
{
RobotNodePtr node = robot->getRobotNode(kinematicRootName);
THROW_VR_EXCEPTION_IF(!node, "No root node with name " << kinematicRootName << " found...");
kinematicRoot = node;
}
else
{
if (!robotNodes.empty())
kinematicRoot = robotNodes[0];
}
RobotNodePtr tcp;
if (!tcpName.empty())
{
RobotNodePtr node = robot->getRobotNode(tcpName);
THROW_VR_EXCEPTION_IF(!node, "No tcp node with name " << tcpName << " found...");
tcp = node;
}
else
{
if (!robotNodes.empty())
tcp = robotNodes[robotNodes.size()-1];
}
RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(robot, name, robotNodes, kinematicRoot, tcp, registerToRobot);
return rns;
}
VirtualRobot::ObstaclePtr ObjectIO::loadObstacle(const std::ifstream& xmlFile, const std::string& basePath /*= ""*/)
{
// load file
THROW_VR_EXCEPTION_IF(!xmlFile.is_open(), "Could not open XML file");
std::stringstream buffer;
buffer << xmlFile.rdbuf();
std::string objectXML(buffer.str());
VirtualRobot::ObstaclePtr res = createObstacleFromString(objectXML, basePath);
THROW_VR_EXCEPTION_IF(!res, "Error while parsing file.");
return res;
}
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::ManipulationObjectPtr ObjectIO::loadManipulationObject(const std::string& xmlFile)
{
// load file
std::ifstream in(xmlFile.c_str());
THROW_VR_EXCEPTION_IF(!in.is_open(), "Could not open XML file:" << xmlFile);
boost::filesystem::path filenameBaseComplete(xmlFile);
boost::filesystem::path filenameBasePath = filenameBaseComplete.branch_path();
std::string basePath = filenameBasePath.string();
VirtualRobot::ManipulationObjectPtr res = loadManipulationObject(in, basePath);
THROW_VR_EXCEPTION_IF(!res, "Error while parsing file " << xmlFile);
res->setFilename(xmlFile);
return res;
}
bool RobotConfig::setConfig( RobotNodePtr node, float value )
{
THROW_VR_EXCEPTION_IF(!node,"Null data");
RobotPtr r = robot.lock();
if (!r)
{
VR_WARNING << "Robot is already deleted, skipping update..." << endl;
return false;
}
THROW_VR_EXCEPTION_IF(!r->hasRobotNode(node),"RobotNode with name " << r->getName() << " does not belong to robot " << r->getName());
configs[node] = value;
return true;
}
bool RobotNode::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr> &children)
{
RobotPtr rob = robot.lock();
THROW_VR_EXCEPTION_IF(!rob, "Could not init RobotNode without robot" );
// robot
if (!rob->hasRobotNode(static_pointer_cast<RobotNode>(shared_from_this())))
rob->registerRobotNode(static_pointer_cast<RobotNode>(shared_from_this()));
// update visualization of coordinate systems
if (visualizationModel && visualizationModel->hasAttachedVisualization("CoordinateSystem"))
{
VisualizationNodePtr v = visualizationModel->getAttachedVisualization("CoordinateSystem");
// not needed any more!
// this is a little hack: The globalPose is used to set the "local" position of the attached Visualization:
// Since the attached visualizations are already positioned at the global pose of the visualizationModel,
// we just need the local postJointTransform
//v->setGlobalPose(postJointTransformation);
}
checkValidRobotNodeType();
for (size_t i=0;i<sensors.size();i++)
{
sensors[i]->initialize(shared_from_this());
}
return SceneObject::initialize(parent,children);
}
void RobotNode::updatePose(bool updateChildren)
{
THROW_VR_EXCEPTION_IF(!initialized, "Not initialized");
updateTransformationMatrices();
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
// apply propagated joint values
if (propagatedJointValues.size()>0)
{
RobotPtr r = robot.lock();
std::map< std::string, float>::iterator it = propagatedJointValues.begin();
while (it!=propagatedJointValues.end())
{
RobotNodePtr rn = r->getRobotNode(it->first);
if (!rn)
{
VR_WARNING << "Could not propagate joint value from " << name << " to " << it->first << " because dependent joint does not exist...";
} else
{
rn->setJointValue(jointValue*it->second);
}
it++;
}
}
}
//! constructor
PathProcessingThread::PathProcessingThread(PathProcessorPtr processor)
{
pathProcessor = processor;
THROW_VR_EXCEPTION_IF(!processor,"NULL data");
threadStarted = false;
processingFinished = false;
}
void ShortcutProcessor::doPathPruning()
{
THROW_VR_EXCEPTION_IF(!initSolution(), "Could not init");
unsigned int i = 0;
while (i < optimizedPath->getNrOfPoints() - 2)
{
Eigen::VectorXf startConfig = optimizedPath->getPoint(i);
Eigen::VectorXf endConfig = optimizedPath->getPoint(i + 2);
if (cspace->isPathValid(startConfig, endConfig))
{
optimizedPath->erasePosition(i + 1);
if (i > 0)
{
i--;
}
}
else
{
i++;
}
}
}
VirtualRobot::VisualizationNodePtr VisualizationNode::CreateUnitedVisualization( const std::vector<VisualizationNodePtr> &visualizations )
{
if (visualizations.size()==0)
return VisualizationNodePtr();
VisualizationFactoryPtr f;
std::vector<VisualizationNodePtr>::const_iterator i = visualizations.begin();
while (!f && i!=visualizations.end())
{
if ((*i)->getType() != VisualizationFactory::getName())
{
f = VisualizationFactory::fromName((*i)->getType(),NULL);
break;
}
i++;
}
if (i==visualizations.end())
{
VR_ERROR << "Could not find visualization factory. Aborting..." << endl;
return VisualizationNodePtr();
}
THROW_VR_EXCEPTION_IF(!f,"No VisualizationFactory");
return f->createUnitedVisualization(visualizations);
}
std::string Trajectory::toXML(int tabs) const
{
std::stringstream ss;
std::string tab = "";
for (int i=0;i<tabs;i++)
tab += "\t";
RobotPtr robot = rns->getRobot();
THROW_VR_EXCEPTION_IF(( !robot || !rns),"Need a valid robot and rns");
ss << tab << "<Trajectory Robot='" << robot->getType() << "' RobotNodeSet='" << rns->getName() << "' name='" << name << "' dim='" << dimension << "'>\n";
for (unsigned int i = 0; i < path.size(); i++)
{
ss << tab << "\t<Point id='" << i << "'>\n";
Eigen::VectorXf c = path[i];
for (unsigned int k = 0; k < dimension; k++)
{
ss << tab << "\t\t<c value='" << c[k] << "'/>\n";
}
ss << tab << "\t</Point>\n";
}
ss << tab << "</Trajectory>\n";
return ss.str();
}
DynamicsRobot::DynamicsRobot(VirtualRobot::RobotPtr rob)
{
THROW_VR_EXCEPTION_IF(!rob,"NULL object");
robot = rob;
sensors = rob->getSensors();
}
PoseQualityMeasurement::PoseQualityMeasurement(VirtualRobot::RobotNodeSetPtr rns)
:rns(rns)
{
THROW_VR_EXCEPTION_IF( (!rns || !rns->getTCP()), "NULL data");
name = "PoseQualityMeasurement";
considerObstacle = false;
verbose = false;
}
void RobotNodeSet::getJointValues( RobotConfigPtr fillVector ) const
{
THROW_VR_EXCEPTION_IF(!fillVector,"NULL data");
for (size_t i=0;i<robotNodes.size();i++)
{
fillVector->setConfig(robotNodes[i]->getName(), robotNodes[i]->getJointValue());
}
}
void RobotNode::updatePose(const Eigen::Matrix4f &globalPose, bool updateChildren)
{
THROW_VR_EXCEPTION_IF(!initialized, "Not initialized");
updateTransformationMatrices(globalPose);
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
}
void RobotNodeSet::respectJointLimits(std::vector<float> &jointValues) const
{
THROW_VR_EXCEPTION_IF(jointValues.size() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl);
for (unsigned int i=0;i<robotNodes.size();i++)
{
robotNodes[i]->respectJointLimits(jointValues[i]);
}
}
RobotConfig::RobotConfig(RobotWeakPtr robot, const std::string &name, const std::vector< Configuration > &configs)
: name(name),
robot(robot)
{
THROW_VR_EXCEPTION_IF(!robot.lock(),"NULL robot in RobotConfig");
for (std::vector< Configuration >::const_iterator i=configs.begin(); i!=configs.end(); i++ )
{
setConfig((*i));
}
}
RobotConfig::RobotConfig(RobotWeakPtr robot, const std::string &name, const std::map< RobotNodePtr, float > &configs)
: name(name),
robot(robot)
{
THROW_VR_EXCEPTION_IF(!robot.lock(),"NULL robot in RobotConfig");
for (std::map< RobotNodePtr, float >::const_iterator i=configs.begin(); i!=configs.end(); i++ )
{
setConfig(i->first,i->second);
}
}
bool SceneIO::processSceneObstacle(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath )
{
THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneObstacle");
ObstaclePtr o = ObjectIO::processObstacle(sceneXMLNode,basePath);
if (!o)
return false;
scene->registerObstacle(o);
return true;
}
bool SceneIO::processSceneManipulationObject(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath )
{
THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneManipulationObject");
ManipulationObjectPtr o = ObjectIO::processManipulationObject(sceneXMLNode,basePath);
if (!o)
return false;
scene->registerManipulationObject(o);
return true;
}
bool RobotNodeSet::checkJointLimits( std::vector<float> &jointValues, bool verbose /*= false*/ ) const
{
THROW_VR_EXCEPTION_IF(jointValues.size() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl);
bool res = true;
for (unsigned int i=0;i<robotNodes.size();i++)
{
res = res & robotNodes[i]->checkJointLimits(jointValues[i],verbose);
}
return res;
}
/**
* This method constructs an instance of TriMeshModel and stores it in
* CoinVisualizationNode::triMeshModel.
* If CoinVisualizationMode::visualization is invalid VirtualRobotException
* is thrown.
* Otherwise CoinVisualizationNode::InventorTriangleCB() is called on the
* Inventor graph stored in CoinVisualizationNode::visualization.
*/
void CoinVisualizationNode::createTriMeshModel()
{
THROW_VR_EXCEPTION_IF(!visualization, "CoinVisualizationNode::createTriMeshModel(): no Coin model present!");
if (triMeshModel)
triMeshModel->clear();
else
triMeshModel.reset(new TriMeshModel());
SoCallbackAction ca;
ca.addTriangleCallback(SoShape::getClassTypeId(), &CoinVisualizationNode::InventorTriangleCB, triMeshModel.get());
ca.apply(visualization);
}
ScenePtr SceneIO::processSceneAttributes(rapidxml::xml_node<char>* sceneXMLNode)
{
// process attributes of scene
// get name
std::string sceneName = processNameAttribute(sceneXMLNode);
THROW_VR_EXCEPTION_IF (sceneName.empty(),"Scene definition expects attribute 'name'");
// build scene
ScenePtr scene(new Scene(sceneName));
return scene;
}
