diff --git a/VirtualRobot/IK/HierarchicalIKSolver.cpp b/VirtualRobot/IK/HierarchicalIKSolver.cpp
index 3ad9276efbf3bceab9f4de4e8ea30df1d7c1084c..1ebf80cdaa30ab309796a39c9daa71ad9ba2a231 100644
--- a/VirtualRobot/IK/HierarchicalIKSolver.cpp
+++ b/VirtualRobot/IK/HierarchicalIKSolver.cpp
@@ -55,7 +55,7 @@ bool HierarchicalIKSolver::computeSteps(float stepSize, float minChange, int max
}
bool HierarchicalIKSolver::checkTolerances() {
- for(int i = 0; i < jacobies.size(); i++) {
+ for(size_t i = 0; i < jacobies.size(); i++) {
if (!jacobies[i]->checkTolerances())
return false;
}
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index 5a794e9fdcd91279ba541a8785b47417c5e6950b..c7534d13bca1816fa9e0daf2cc10107bd155b4fd 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -210,9 +210,7 @@ namespace VirtualRobot
void RobotNode::updateTransformationMatrices(const Eigen::Matrix4f& parentPose)
{
- this->globalPose = parentPose * getLocalTransformation();
-
- //globalPosePostJoint = this->globalPose*getPostJointTransformation();
+ this->globalPose = parentPose * getLocalTransformation(); // localTransformation
}
@@ -707,12 +705,16 @@ namespace VirtualRobot
{
std::vector<RobotNodePtr> result;
- if (!rns)
+ std::vector<RobotNodePtr> rn;
+ if (rns)
{
- return result;
+ rn = rns->getAllRobotNodes();
+ }
+ else
+ {
+ RobotPtr r = this->getRobot();
+ rn = r->getRobotNodes();
}
-
- std::vector<RobotNodePtr> rn = rns->getAllRobotNodes();
for (unsigned int i = 0; i < rn.size(); i++)
{
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 6e111b7c9a5d7491e8ec40a98d3f9fa6b27e8ba6..444b92f114a527d8567486c1239c36da841ce633 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -58,6 +58,7 @@ namespace VirtualRobot
{
public:
friend class Robot;
+ friend class LocalRobot;
friend class RobotIO;
friend class RobotNodeSet;
friend class RobotConfig;
@@ -126,7 +127,7 @@ namespace VirtualRobot
/*!
The preJoint/preVisualization transformation. This transformation is applied before the joint and the visualization.
*/
- virtual Eigen::Matrix4f getLocalTransformation()
+ virtual const Eigen::Matrix4f& getLocalTransformation()
{
return localTransformation;
}
@@ -203,8 +204,9 @@ namespace VirtualRobot
/*!
Find all robot nodes whose movements affect this RobotNode
+ \param rns If set, the search is limited to the rns.
*/
- virtual std::vector<RobotNodePtr> getAllParents(RobotNodeSetPtr rns);
+ virtual std::vector<RobotNodePtr> getAllParents(RobotNodeSetPtr rns = RobotNodeSetPtr());
/*!
Clone this RobotNode.
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index ae36fdcba433baf52582590e462bd1b357256de7..381d6bc2dc0d7474387c280ca5f19f75a238639a 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -325,6 +325,16 @@ namespace VirtualRobot
}
}
+
+ /**
+ * Can be called internally, when lock is already set.
+ */
+ void LocalRobot::applyJointValuesNoLock()
+ {
+ rootNode->updatePose(globalPose);
+ }
+
+
std::vector<EndEffectorPtr> Robot::getEndEffectors()
{
std::vector<EndEffectorPtr> res;
@@ -418,6 +428,7 @@ namespace VirtualRobot
void Robot::applyJointValuesNoLock()
{
this->getRootNode()->updatePose(this->getGlobalPose());
+ //rootNode->updatePose(globalPose);
}
/**
@@ -453,24 +464,30 @@ namespace VirtualRobot
void Robot::setUpdateVisualization(bool enable)
{
- updateVisualization = enable;
+ SceneObject::setUpdateVisualization(enable);
std::vector<RobotNodePtr> robotNodes = this->getRobotNodes();
std::vector<RobotNodePtr> ::const_iterator iterator = robotNodes.begin();
- // std::map< std::string, RobotNodePtr>::const_iterator iterator = robotNodeMap.begin();
- // while(robotNodeMap.end() != iterator)
while (robotNodes.end() != iterator)
{
- //iterator->second->setUpdateVisualization(enable);
(*iterator)->setUpdateVisualization(enable);
++iterator;
}
}
- bool Robot::getUpdateVisualizationStatus()
+ void Robot::setUpdateCollisionModel(bool enable)
{
- return updateVisualization;
+ SceneObject::setUpdateCollisionModel(enable);
+
+ std::vector<RobotNodePtr> robotNodes = this->getRobotNodes();
+ std::vector<RobotNodePtr> ::const_iterator iterator = robotNodes.begin();
+
+ while (robotNodes.end() != iterator)
+ {
+ (*iterator)->setUpdateCollisionModel(enable);
+ ++iterator;
+ }
}
RobotNodeSetPtr LocalRobot::getRobotNodeSet(const std::string& nodeSetName)
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index b9245fbf3443cef5134a5400a332f552f90e70a5..3f7f9aeec97c6bb2ce2a7dbb45cf524549554af0 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -132,7 +132,7 @@ namespace VirtualRobot
Enables/Disables the visualization updates of collision model and visualization model.
*/
void setUpdateVisualization(bool enable);
- bool getUpdateVisualizationStatus();
+ void setUpdateCollisionModel(bool enable);
boost::shared_ptr<Robot> shared_from_this()
{
@@ -388,7 +388,7 @@ namespace VirtualRobot
void createVisualizationFromCollisionModels();
//! It is assumed that the mutex is already set
- void applyJointValuesNoLock();
+ virtual void applyJointValuesNoLock();
std::string filename; // RobotIO stores the filename here
@@ -469,6 +469,7 @@ namespace VirtualRobot
protected:
//Eigen::Matrix4f globalPose; //!< The pose of this robot in the world
RobotNodePtr rootNode;
+ virtual void applyJointValuesNoLock();
std::map< std::string, RobotNodePtr > robotNodeMap;
std::map< std::string, RobotNodeSetPtr > robotNodeSetMap;
diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp
index 5db89a02d17f3c2e41903e6aa412094e09fd3534..e2dae6533bdc3d89909742df50b87b9759a25ea2 100644
--- a/VirtualRobot/RobotFactory.cpp
+++ b/VirtualRobot/RobotFactory.cpp
@@ -2,6 +2,7 @@
#include "RobotFactory.h"
#include "Robot.h"
#include "Nodes/RobotNode.h"
+#include "RobotNodeSet.h"
#include "Nodes/RobotNodeRevolute.h"
#include "Nodes/RobotNodePrismatic.h"
#include "Nodes/RobotNodeFixed.h"
@@ -509,4 +510,155 @@ namespace VirtualRobot
return newRobot;
}
+
+ RobotPtr RobotFactory::clone(RobotPtr robot, const std::string& name, CollisionCheckerPtr collisionChecker, float scaling)
+ {
+ THROW_VR_EXCEPTION_IF(!robot, "NULL data");
+ if (!collisionChecker)
+ collisionChecker = robot->getCollisionChecker();
+ VirtualRobot::RobotPtr result = robot->extractSubPart(robot->getRootNode(), robot->getType(), name, true, true, collisionChecker, scaling);
+ result->setGlobalPose(robot->getGlobalPose());
+ return result;
+ }
+
+ void RobotFactory::getChildNodes(RobotNodePtr nodeA, RobotNodePtr nodeExclude, std::vector<RobotNodePtr> &appendNodes)
+ {
+ THROW_VR_EXCEPTION_IF(!nodeA, "NULL data");
+ std::vector < SceneObjectPtr > children = nodeA->getChildren();
+ std::vector<RobotNodePtr> childNodes;
+ for (size_t i = 0; i < children.size(); i++)
+ {
+ SceneObjectPtr c = children[i];
+ RobotNodePtr cRN = boost::dynamic_pointer_cast<RobotNode>(c);
+ if (cRN && cRN != nodeExclude)
+ {
+ appendNodes.push_back(cRN);
+ getChildNodes(cRN, nodeExclude, appendNodes);
+ }
+ }
+ }
+
+ RobotNodePtr RobotFactory::accumulateTransformations(RobotPtr robot, RobotNodePtr nodeA, RobotNodePtr nodeB)
+ {
+ THROW_VR_EXCEPTION_IF(!robot, "NULL data");
+ THROW_VR_EXCEPTION_IF(!nodeA, "NULL data");
+ auto rnf = RobotNodeFixedFactory::createInstance(NULL);
+ SceneObject::Physics p;
+ VisualizationNodePtr v;
+ CollisionModelPtr c;
+ if (nodeA == nodeB)
+ {
+ RobotNodePtr newRN = rnf->createRobotNode(robot, nodeA->getName() + nodeB->getName(), v, c, 0, 0, 0, Eigen::Matrix4f::Identity(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), p);
+ return newRN;
+ }
+
+ std::vector<RobotNodePtr> childNodes;
+
+ std::vector < SceneObjectPtr > children = nodeA->getChildren();
+
+ for (size_t i = 0; i < children.size(); i++)
+ {
+ SceneObjectPtr c = children[i];
+ RobotNodePtr cRN = boost::dynamic_pointer_cast<RobotNode>(c);
+ if (cRN)
+ getChildNodes(cRN, nodeB, childNodes);
+ }
+ return createUnitedRobotNode(robot, childNodes, nodeA);
+ }
+
+ RobotNodePtr RobotFactory::createUnitedRobotNode(RobotPtr robot, const std::vector< RobotNodePtr > &nodes, RobotNodePtr parent)
+ {
+ THROW_VR_EXCEPTION_IF(!robot, "NULL data");
+
+ auto rnf = RobotNodeFixedFactory::createInstance(NULL);
+ SceneObject::Physics p;
+ VisualizationNodePtr v;
+ CollisionModelPtr c;
+ if (nodes.size() == 0)
+ {
+ RobotNodePtr newRN = rnf->createRobotNode(robot, "root", v, c, 0, 0, 0, Eigen::Matrix4f::Identity(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), p);
+ if (parent)
+ parent->attachChild(newRN);
+ return newRN;
+ }
+
+ VisualizationFactoryPtr vf = VisualizationFactory::first(NULL);
+ std::vector < VisualizationNodePtr > visus;
+ std::vector < VisualizationNodePtr > colVisus;
+ float kg = 0;
+
+ for (size_t i = 0; i < nodes.size(); i++)
+ {
+ if (nodes[i]->getVisualization())
+ visus.push_back(nodes[i]->getVisualization());
+ if (nodes[i]->getCollisionModel() && nodes[i]->getCollisionModel()->getVisualization())
+ colVisus.push_back(nodes[i]->getCollisionModel()->getVisualization());
+ kg += nodes[i]->getMass();
+ }
+ if (visus.size()>0)
+ v = vf->createUnitedVisualization(visus)->clone();
+ if (colVisus.size() > 0)
+ {
+ VisualizationNodePtr colVisu = vf->createUnitedVisualization(colVisus)->clone();
+ c.reset(new CollisionModel(colVisu, nodes[0]->getName()));
+ }
+ p.massKg = kg;
+
+ RobotNodePtr newRN = rnf->createRobotNode(robot, "root", v, c, 0, 0, 0, Eigen::Matrix4f::Identity(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), p);
+ if (parent)
+ parent->attachChild(newRN);
+
+ return newRN;
+ }
+
+
+ RobotPtr RobotFactory::cloneSubSet(RobotPtr robot, RobotNodeSetPtr rns, const std::string& name)
+ {
+ THROW_VR_EXCEPTION_IF(!robot, "NULL data");
+ THROW_VR_EXCEPTION_IF(!rns, "NULL data");
+ THROW_VR_EXCEPTION_IF(rns->getRobot() != robot, "Inconsitent data");
+
+ std::vector< RobotNodePtr > nodes = rns->getAllRobotNodes();
+ THROW_VR_EXCEPTION_IF(nodes.size()==0, "0 data");
+ for (size_t i = 1; i < nodes.size(); i++)
+ {
+ RobotNodePtr a = nodes[i - 1];
+ RobotNodePtr b = nodes[i];
+
+ if (!a->hasChild(b, true))
+ {
+ std::stringstream ss;
+ ss << "Node " << a->getName() << " is not parent of " << b->getName();
+ THROW_VR_EXCEPTION(ss.str());
+ }
+ }
+
+ // first create initial node
+ std::vector< RobotNodePtr > initialNodes = nodes[0]->getAllParents();
+
+ RobotPtr result(new LocalRobot(name, robot->getType()));
+
+ RobotNodePtr rootNode = createUnitedRobotNode(result, initialNodes);
+ result->setRootNode(rootNode);
+ RobotNodePtr currentParent = rootNode;
+ for (size_t i = 0; i < nodes.size(); i++)
+ {
+ RobotNodePtr newNode = nodes[i]->clone(result, false, currentParent);
+ currentParent = newNode;
+
+ RobotNodePtr secondNode;
+ if (i < nodes.size() - 1)
+ {
+ secondNode = nodes[i + 1];
+ }
+ RobotNodePtr newNodeFixed = accumulateTransformations(result, nodes[i], secondNode);
+ if (newNodeFixed)
+ currentParent = newNodeFixed;
+ }
+
+ result->setGlobalPose(robot->getGlobalPose());
+ return result;
+ }
+
+
} // namespace VirtualRobot
diff --git a/VirtualRobot/RobotFactory.h b/VirtualRobot/RobotFactory.h
index 305c9d220273e81d4b0a6a5b61d49d52d310c3d7..67c58346f4407ffafe02d60de4ef4ec827bce25d 100644
--- a/VirtualRobot/RobotFactory.h
+++ b/VirtualRobot/RobotFactory.h
@@ -70,6 +70,31 @@ namespace VirtualRobot
std::vector<robotNodeDef> parentChildMapping;
};
+ /*!
+ Clones the robot.
+ \param robot The robot to clone
+ \param name The new name
+ \param collisionChecker Optional: A collision checker to which the robot should be registered. If not set, the collision checker of the input robot is used.
+ \param scaling Scale the resulting robot.
+ */
+ static RobotPtr clone(RobotPtr robot, const std::string& name, CollisionCheckerPtr collisionChecker = CollisionCheckerPtr(), float scaling = 1.0f);
+
+ /*!
+ Clones the robot, but only leave the defined joints active. ALl other joints are accumulated and set to one model which is fixed (may result in faster updates)
+ \param robot The robot to clone
+ \param rns The robot node set of active joints. The joints must be given as an ordered set, i.e. the first node must be located before the second node in the kinemtic structure of the robot.
+ \param name The new name
+ */
+ static RobotPtr cloneSubSet(RobotPtr robot, RobotNodeSetPtr rns, const std::string& name);
+
+
+
+ static RobotNodePtr createUnitedRobotNode(RobotPtr robot, const std::vector< RobotNodePtr > &nodes, RobotNodePtr parent = RobotNodePtr());
+
+ static RobotNodePtr accumulateTransformations(RobotPtr robot, RobotNodePtr nodeA, RobotNodePtr nodeB);
+
+ static void getChildNodes(RobotNodePtr nodeA, RobotNodePtr nodeExclude, std::vector<RobotNodePtr> &appendNodes);
+
static RobotPtr cloneInversed(RobotPtr robot, const std::string& newRootName);
static RobotPtr cloneChangeStructure(RobotPtr robot, robotStructureDef& newStructure);
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 536812b0f1559a31753ef9cc9b2b0a2fac5a4c7b..b4b5e259e244d75f816f5d03a3d50d780ab89740 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -23,10 +23,12 @@ namespace VirtualRobot
this->visualizationModel = visualization;
this->collisionModel = collisionModel;
+
this->physics = p;
this->globalPose = Eigen::Matrix4f::Identity();
this->initialized = false;
updateVisualization = true;
+ updateCollisionModel = true;
if (visualization)
{
@@ -118,12 +120,12 @@ namespace VirtualRobot
void SceneObject::updatePose(bool updateChildren)
{
- if (visualizationModel)
+ if (visualizationModel && updateVisualization)
{
visualizationModel->setGlobalPose(globalPose);
}
- if (collisionModel)
+ if (collisionModel && updateCollisionModel)
{
collisionModel->setGlobalPose(globalPose);
}
@@ -479,12 +481,21 @@ namespace VirtualRobot
collisionModel->setUpdateVisualization(enable);
}
}
+ void SceneObject::setUpdateCollisionModel(bool enable)
+ {
+ updateCollisionModel = enable;
+ }
bool SceneObject::getUpdateVisualizationStatus()
{
return updateVisualization;
}
+ bool SceneObject::getUpdateCollisionModelStatus()
+ {
+ return updateCollisionModel;
+ }
+
void SceneObject::setVisualization(VisualizationNodePtr visualization)
{
visualizationModel = visualization;
@@ -774,6 +785,15 @@ namespace VirtualRobot
{
cout << "<not set>" << endl;
}
+ cout << " * Update collision model status: ";
+ if (updateCollisionModel)
+ {
+ cout << "enabled" << endl;
+ }
+ else
+ {
+ cout << "disabled" << endl;
+ }
std::vector<SceneObjectPtr> children = this->getChildren();
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index e1d74f11b6c8cbd3ee971c50d838a3c8ffea2eb5..8058fccdffe970a6ef7d54e39f04018f0657f34c 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -264,8 +264,10 @@ namespace VirtualRobot
/*!
Enables/Disables the visualization updates of collision model and visualization model.
*/
- void setUpdateVisualization(bool enable);
+ virtual void setUpdateVisualization(bool enable);
bool getUpdateVisualizationStatus();
+ virtual void setUpdateCollisionModel(bool enable);
+ bool getUpdateCollisionModelStatus();
/*!
Setup the visualization of this object.
@@ -511,6 +513,7 @@ namespace VirtualRobot
VisualizationNodePtr visualizationModel; //< This is the main visualization
bool updateVisualization;
+ bool updateCollisionModel;
virtual bool initializePhysics();
Physics physics;