diff --git a/VirtualRobot/ManipulationObject.cpp b/VirtualRobot/ManipulationObject.cpp
index a64158b79367e2d7685b9bff2eef3ea6c03d9e5b..c2e23672256499eda6016ced98eb2ebab46c4be1 100644
--- a/VirtualRobot/ManipulationObject.cpp
+++ b/VirtualRobot/ManipulationObject.cpp
@@ -109,6 +109,7 @@ namespace VirtualRobot
ManipulationObjectPtr result(new ManipulationObject(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
result->setGlobalPose(getGlobalPose());
+ result->primitiveApproximation = primitiveApproximation;
appendSensorsTo(result);
appendGraspSetsTo(result);
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index 4236c59c1a3720be13acd8ffe3a286b4663a7943..2107a7b3d1de25b922ca77ce3296f05e1ef3b467 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -129,6 +129,25 @@ namespace VirtualRobot
enforceJointLimits = value;
}
+ void RobotNode::removeAllSensors(bool recursive)
+ {
+ for (auto it = children.begin(); it != children.end();)
+ {
+ if (auto node = std::dynamic_pointer_cast<RobotNode>(*it))
+ {
+ if (recursive) node->removeAllSensors(recursive);
+ }
+ else if (auto node = std::dynamic_pointer_cast<Sensor>(*it))
+ {
+ //(*it)->detachedFromParent();
+ it = children.erase(it);
+ continue;
+ }
+ it++;
+ }
+ sensors.clear();
+ }
+
RobotPtr RobotNode::getRobot() const
{
RobotPtr result(robot);
@@ -604,6 +623,7 @@ namespace VirtualRobot
newRobot->registerRobotNode(result);
+ result->primitiveApproximation = primitiveApproximation.clone();
if (initializeWithParent)
{
@@ -611,6 +631,7 @@ namespace VirtualRobot
}
result->basePath = basePath;
result->setScaling(actualScaling);
+
return result;
}
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index f0188efac5f6026be1b95aa4bf4a2217554837f8..9bd9cd44eecee77fa3e81356ddea146242578b15 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -401,6 +401,9 @@ namespace VirtualRobot
bool getEnforceJointLimits() const;
void setEnforceJointLimits(bool value);
+ /*! Removes all sensors (for faster forward kinematics) */
+ void removeAllSensors(bool recursive = true);
+
protected:
/*!
Queries parent for global pose and updates visualization accordingly
diff --git a/VirtualRobot/Obstacle.cpp b/VirtualRobot/Obstacle.cpp
index 3881df846ded2e30bb40be1cf1958af5f443d8ef..2f1121b9a902b2d65b300f6dfabc366c8c6538d7 100644
--- a/VirtualRobot/Obstacle.cpp
+++ b/VirtualRobot/Obstacle.cpp
@@ -301,6 +301,7 @@ namespace VirtualRobot
ObstaclePtr result(new Obstacle(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
result->setGlobalPose(getGlobalPose());
+ result->primitiveApproximation = primitiveApproximation;
appendSensorsTo(result);
appendGraspSetsTo(result);
diff --git a/VirtualRobot/Primitive.cpp b/VirtualRobot/Primitive.cpp
index 8a79daaa497aac4883e8b884a5855e31e5dbc5e1..5abec0115f45e9d536c3b3b5e81e65275d12f6b9 100644
--- a/VirtualRobot/Primitive.cpp
+++ b/VirtualRobot/Primitive.cpp
@@ -63,5 +63,13 @@ namespace VirtualRobot
return getXMLString("Cylinder", format.str(), tabs);
}
+ std::string Capsule::toXMLString(int tabs)
+ {
+ std::stringstream format;
+ format << "radius=\"" << radius
+ << "\" height=\"" << height << "\"";
+ return getXMLString("Capsule", format.str(), tabs);
+ }
+
} //namespace Primitive
} //namespace VirtualRobot
diff --git a/VirtualRobot/Primitive.h b/VirtualRobot/Primitive.h
index 5e27bf5fb68ffcf2ece4a3962f33c7c757027fb3..9d03fef485396abd4c647cdfce38adde4fa8a806 100644
--- a/VirtualRobot/Primitive.h
+++ b/VirtualRobot/Primitive.h
@@ -20,6 +20,8 @@ namespace VirtualRobot
virtual std::string toXMLString(int tabs) = 0;
+ virtual std::unique_ptr<Primitive> clone() const = 0;
+
protected:
Primitive(int type) : type(type), transform(Eigen::Matrix4f::Identity()) {}
std::string getTransformString(int tabs = 0);
@@ -38,6 +40,13 @@ namespace VirtualRobot
float height;
float depth;
std::string toXMLString(int tabs = 0) override;
+
+ std::unique_ptr<Primitive> clone() const final
+ {
+ auto clone = std::make_unique<Box>(width, height, depth);
+ clone->transform = transform;
+ return clone;
+ }
};
class VIRTUAL_ROBOT_IMPORT_EXPORT Sphere : public Primitive
@@ -48,6 +57,13 @@ namespace VirtualRobot
Sphere(float radius) : Primitive(TYPE), radius(radius) {}
float radius;
std::string toXMLString(int tabs = 0) override;
+
+ std::unique_ptr<Primitive> clone() const final
+ {
+ auto clone = std::make_unique<Sphere>(radius);
+ clone->transform = transform;
+ return clone;
+ }
};
class VIRTUAL_ROBOT_IMPORT_EXPORT Cylinder : public Primitive
@@ -59,6 +75,34 @@ namespace VirtualRobot
float radius;
float height;
std::string toXMLString(int tabs = 0) override;
+
+ std::unique_ptr<Primitive> clone() const final
+ {
+ auto clone = std::make_unique<Cylinder>(radius, height);
+ clone->transform = transform;
+ return clone;
+ }
+ };
+
+ /**
+ * @brief The Capsule class. The capsule is extended along the y-axis.
+ */
+ class VIRTUAL_ROBOT_IMPORT_EXPORT Capsule : public Primitive
+ {
+ public:
+ static const int TYPE = 4;
+ Capsule() : Primitive(TYPE) {}
+ Capsule(float radius, float height) : Primitive(TYPE), radius(radius), height(height) {}
+ float radius;
+ float height;
+ std::string toXMLString(int tabs = 0) override;
+
+ std::unique_ptr<Primitive> clone() const final
+ {
+ auto clone = std::make_unique<Capsule>(radius, height);
+ clone->transform = transform;
+ return clone;
+ }
};
typedef std::shared_ptr<Primitive> PrimitivePtr;
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index a5b3652b05ce4f6852e82a75db699af4d762b6a0..2945832a5f56d06054004234315f14eb776e6804 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -923,6 +923,7 @@ namespace VirtualRobot
{
result->registerHumanMapping(getHumanMapping().value());
}
+ result->primitiveApproximation = primitiveApproximation.clone();
return result;
}
@@ -1285,6 +1286,13 @@ namespace VirtualRobot
this->humanMapping = humanMapping;
}
+ void Robot::removeAllSensors()
+ {
+ if (auto rootNode = getRootNode())
+ {
+ rootNode->removeAllSensors();
+ }
+ }
const NodeMapping& Robot::getNodeMapping() const
{
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 2849f4848b64eaddcee99351cd1f8a5daeccafef..1ab3255d1128438ff71ff27369eefe55df24a82d 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -502,6 +502,8 @@ namespace VirtualRobot
void registerHumanMapping(const HumanMapping& humanMapping);
+ /*! Removes all sensors (for faster forward kinematics) */
+ void removeAllSensors();
protected:
Robot();
diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp
index d8a2af0d9737d21bd042f69137fa5a3f6cf5a688..e49afc9cb4e22e1fccceaf54eb3987e565d74434 100644
--- a/VirtualRobot/RobotFactory.cpp
+++ b/VirtualRobot/RobotFactory.cpp
@@ -13,6 +13,8 @@
#include "Visualization//VisualizationFactory.h"
#include "VirtualRobotException.h"
+#include <SimoxUtility/math/pose/invert.h>
+
#include <algorithm>
#include <cassert>
#include <deque>
@@ -321,6 +323,7 @@ namespace VirtualRobot
RobotNodePtr newRN = rnf->createRobotNode(robot, name, v, c, 0, 0, 0, transformation, Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), p);
rn->attachChild(newRN);
newRN->initialize(rn);
+ newRN->primitiveApproximation = o->getPrimitiveApproximation().clone();
return true;
}
@@ -656,15 +659,9 @@ namespace VirtualRobot
{
THROW_VR_EXCEPTION_IF(!robot, "NULL data");
THROW_VR_EXCEPTION_IF(!nodeA, "NULL data");
- auto rnf = RobotNodeFixedFactory::createInstance(nullptr);
- SceneObject::Physics p;
- VisualizationNodePtr v;
- CollisionModelPtr c;
-
if (nodeA == nodeB)
{
- RobotNodePtr newRN = rnf->createRobotNode(robot, nodeA->getName() + "_non_trafo", v, c, 0, 0, 0, Eigen::Matrix4f::Identity(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), p);
- return newRN;
+ return RobotNodePtr();
}
std::vector<RobotNodePtr> childNodes;
@@ -684,7 +681,7 @@ namespace VirtualRobot
{
Eigen::Matrix4f startPose = nodeA->getGlobalPose();
Eigen::Matrix4f goalPose = nodeB->getParent()->getGlobalPose();
- storeTrafo = goalPose * startPose.inverse();
+ storeTrafo = simox::math::inverted_pose(startPose) * goalPose;
}
RobotNodePtr res = createUnitedRobotNode(robot, childNodes, nodeA, nodeAClone, Eigen::Matrix4f::Identity(), childSensorNodes);
@@ -751,7 +748,7 @@ namespace VirtualRobot
v = vf->createUnitedVisualization(visus)->clone();
if (parent)
{
- Eigen::Matrix4f invTr = parent->getGlobalPose().inverse();
+ Eigen::Matrix4f invTr = simox::math::inverted_pose(parent->getGlobalPose());
vf->applyDisplacement(v, invTr);
}
}
@@ -761,7 +758,7 @@ namespace VirtualRobot
VisualizationNodePtr colVisu = vf->createUnitedVisualization(colVisus)->clone();
if (parent)
{
- Eigen::Matrix4f invTr = parent->getGlobalPose().inverse();
+ Eigen::Matrix4f invTr = simox::math::inverted_pose(parent->getGlobalPose());
vf->applyDisplacement(colVisu, invTr);
}
c.reset(new CollisionModel(colVisu, nodes[0]->getName()));
@@ -773,20 +770,57 @@ namespace VirtualRobot
newRN->initialize(parentClone);
+ const Eigen::Matrix4f trafoToNewRN = parent ? parent->getGlobalPose() * trafo : trafo;
+
+ // add primitive models
+ for (const auto& node : nodes)
+ {
+ const auto& primitiveApproximation = node->getPrimitiveApproximation();
+ const Eigen::Matrix4f localTransformation = simox::math::inverted_pose(trafoToNewRN) * node->getGlobalPose();
+ newRN->getPrimitiveApproximation().join(primitiveApproximation.clone().localTransformation(localTransformation));
+ }
+
// attach sensors
for (const auto& sensor : sensors)
{
SensorPtr s = sensor->clone(newRN);
- Eigen::Matrix4f trafoToNewRN = parent ? parent->getGlobalPose() * trafo : trafo;
- Eigen::Matrix4f t = trafoToNewRN.inverse() * sensor->getGlobalPose();
+ Eigen::Matrix4f t = simox::math::inverted_pose(trafoToNewRN) * sensor->getGlobalPose();
s->setRobotNodeToSensorTransformation(t);
}
return newRN;
}
+ void RobotFactory::cloneRNS(const Robot& from, RobotPtr to)
+ {
+ for (const auto& rns : from.getRobotNodeSets())
+ {
+ bool hasNodes = true;
+ for (const auto& nodeName : rns->getNodeNames())
+ {
+ if (!to->hasRobotNode(nodeName))
+ {
+ hasNodes = false;
+ break;
+ }
+ }
+ const auto &tcp = rns->getTCP();
+ if (hasNodes && (!tcp || to->hasRobotNode(tcp->getName())))
+ {
+ if (const auto& kinRoot = rns->getKinematicRoot())
+ {
+ if (!to->hasRobotNode(kinRoot->getName()))
+ {
+ rns->clone(to, to->getRootNode());
+ continue;
+ }
+ }
+ rns->clone(to);
+ }
+ }
+ }
- RobotPtr RobotFactory::cloneSubSet(RobotPtr robot, RobotNodeSetPtr rns, const std::string& name)
+ RobotPtr RobotFactory::cloneSubSet(RobotPtr robot, RobotNodeSetPtr rns, const std::string& name, bool addTCP)
{
THROW_VR_EXCEPTION_IF(!robot, "NULL data");
THROW_VR_EXCEPTION_IF(!rns, "NULL data");
@@ -796,8 +830,8 @@ namespace VirtualRobot
THROW_VR_EXCEPTION_IF(nodes.size() == 0, "0 data");
// ensure tcp is part of nodes
- //if (rns->getTCP() && !rns->hasRobotNode(rns->getTCP()))
- // nodes.push_back(rns->getTCP());
+ if (addTCP && rns->getTCP() && !rns->hasRobotNode(rns->getTCP()))
+ nodes.push_back(rns->getTCP());
// ensure kinemtic root is part of nodes
if (rns->getKinematicRoot() && !rns->hasRobotNode(rns->getKinematicRoot()))
@@ -904,7 +938,13 @@ namespace VirtualRobot
}
}
+ cloneRNS(*robot.get(), result);
+
result->setGlobalPose(robot->getGlobalPose());
+ if(robot->getHumanMapping().has_value())
+ {
+ result->registerHumanMapping(robot->getHumanMapping().value());
+ }
return result;
}
@@ -931,41 +971,197 @@ namespace VirtualRobot
cloneRecursiveUnite(result, currentNode, currentNodeClone, uniteWithAllChildren);
- // clone RNS
- std::vector<RobotNodeSetPtr> rnsets = robot->getRobotNodeSets();
- for (auto rns : rnsets)
+ cloneRNS(*robot.get(), result);
+
+ result->setGlobalPose(robot->getGlobalPose());
+ if(robot->getHumanMapping().has_value())
+ {
+ result->registerHumanMapping(robot->getHumanMapping().value());
+ }
+ return result;
+ }
+
+ RobotPtr RobotFactory::createReducedModel(Robot &robot, const std::vector<std::string> &actuatedJointNames,
+ const std::vector<std::string> &otherNodeNames)
+ {
+ const Eigen::Matrix4f globalPose = robot.getGlobalPose();
+ robot.setGlobalPose(Eigen::Matrix4f::Identity());
+
+ std::set<std::string> joint_set(actuatedJointNames.begin(), actuatedJointNames.end());
+ std::set<std::string> other_set(otherNodeNames.begin(), otherNodeNames.end());
+
+ // Check joint nodes and set robot joints to default value
+ std::map<std::string, float> joint_values;
+ for (const auto& joint_name : actuatedJointNames)
+ {
+ if (robot.hasRobotNode(joint_name))
+ {
+ auto node = robot.getRobotNode(joint_name);
+ if (!node->isJoint())
+ {
+ VR_WARNING << "Robot node " << joint_name << " is not a joint node";
+ return nullptr;
+ }
+ joint_values[joint_name] = node->getJointValue();
+ // Set joints do default starting value
+ node->setJointValueNoUpdate(0.);
+ }
+ else
+ {
+ VR_WARNING << "Robot does not contain node " << joint_name;
+ return nullptr;
+ }
+ }
+ robot.updatePose();
+
+ RobotPtr reducedModel = std::make_shared<LocalRobot>("Reduced_" + robot.getName(), robot.getType());
+
+ struct Node
+ {
+ RobotNodePtr node;
+ RobotNodePtr node_cloned = nullptr;
+ bool is_actuated_joint = false;
+ RobotNodePtr parentNode_cloned = nullptr;
+ std::vector<RobotNodePtr> childNodes = std::vector<RobotNodePtr>();
+ };
+ std::queue<Node> nodes;
+ nodes.push(Node { .node = robot.getRootNode() });
+
+ std::function<void(RobotNodePtr, Node&)> collect;
+ collect = [joint_set, other_set, &collect, &nodes](RobotNodePtr currentNode, Node& node) -> void
{
- bool ok = true;
- for (const auto& j : uniteWithAllChildren)
+ for (const auto &child : currentNode->getChildren())
{
- RobotNodePtr rn = robot->getRobotNode(j);
- std::vector<RobotNodePtr> allChildren;
- rn->collectAllRobotNodes(allChildren);
- for (auto& k : allChildren)
+ if (const auto& childNode = std::dynamic_pointer_cast<RobotNode>(child))
{
- if (k == rn)
+ const bool is_actuated_joint = joint_set.count(childNode->getName()) > 0;
+ if (!is_actuated_joint && other_set.count(childNode->getName()) == 0)
{
- continue;
+ node.childNodes.push_back(childNode);
+ collect(childNode, node);
+ }
+ else
+ {
+ nodes.push(Node { .node = childNode, .is_actuated_joint=is_actuated_joint, .parentNode_cloned = node.node_cloned });
}
- if (rns->hasRobotNode(k->getName()) || rns->getTCP() == k)
+ }
+ }
+ };
+
+ const auto fixed_node_factory = RobotNodeFixedFactory::createInstance(nullptr);
+ while(!nodes.empty())
+ {
+ auto n = nodes.front();
+ nodes.pop();
+
+ // If node is a joint but is not part of the joints it will be converted to a fixed transformation node
+ if (!n.is_actuated_joint && n.node->isJoint())
+ {
+ n.node_cloned = fixed_node_factory->createRobotNode(reducedModel, n.node->getName(),
+ n.node->getVisualization() ? n.node->getVisualization()->clone() : nullptr,
+ n.node->getCollisionModel() ? n.node->getCollisionModel()->clone() : nullptr,
+ 0.f, 0.f, 0.f,
+ n.node->getLocalTransformation(),
+ Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(),
+ n.node->getPhysics());
+ n.node_cloned->primitiveApproximation = n.node->getPrimitiveApproximation().clone();
+ if (n.parentNode_cloned)
+ {
+ n.node_cloned->initialize(n.parentNode_cloned);
+ }
+ n.node_cloned->basePath = n.node->basePath;
+ }
+ else
+ {
+ n.node_cloned = n.node->clone(reducedModel, false, n.parentNode_cloned);
+ }
+ // Probably will not be valid anymore after reducing robot as models will be missing
+ n.node_cloned->visualizationModelXML = std::string();
+ n.node_cloned->collisionModelXML = std::string();
+
+ collect(n.node, n);
+
+ if (!n.parentNode_cloned)
+ {
+ reducedModel->setRootNode(n.node_cloned);
+ }
+ else
+ {
+ Eigen::Matrix4f localTransformation = simox::math::inverted_pose(n.parentNode_cloned->getGlobalPose()) * n.node->getGlobalPose();
+ n.node_cloned->setLocalTransformation(localTransformation);
+ n.node_cloned->updateTransformationMatrices();
+ }
+
+ const VisualizationFactoryPtr vf = VisualizationFactory::first(NULL);
+ std::vector<VisualizationNodePtr> visus;
+ std::vector<VisualizationNodePtr> colVisus;
+ for (const auto& childNode : n.childNodes)
+ {
+ if (childNode->getVisualization())
+ {
+ visus.push_back(childNode->getVisualization());
+ }
+ if (childNode->getCollisionModel() && childNode->getCollisionModel()->getVisualization())
+ {
+ colVisus.push_back(childNode->getCollisionModel()->getVisualization());
+ }
+ n.node_cloned->physics.massKg += childNode->getMass(); // TODO fix physics somehow
+ }
+ const Eigen::Matrix4f globalPoseInv = simox::math::inverted_pose(n.node_cloned->getGlobalPose());
+ if (!visus.empty())
+ {
+ if (const auto visu = n.node->getVisualization())
+ {
+ visus.insert(visus.begin(), visu->clone());
+ }
+ auto v = vf->createUnitedVisualization(visus);
+ if (n.parentNode_cloned)
+ {
+ vf->applyDisplacement(v, globalPoseInv);
+ }
+ n.node_cloned->setVisualization(v);
+ }
+ if (!colVisus.empty())
+ {
+ if (const auto colModel = n.node->getCollisionModel())
+ {
+ if (const auto vis = colModel->getVisualization())
{
- ok = false;
- break;
+ colVisus.insert(colVisus.begin(), vis->clone());
}
}
+ auto colVisu = vf->createUnitedVisualization(colVisus);
+ if (n.parentNode_cloned)
+ {
+ vf->applyDisplacement(colVisu, globalPoseInv);
+ }
+ n.node_cloned->setCollisionModel(std::make_shared<CollisionModel>(colVisu, n.node->getName()));
}
- if (ok)
+
+ for (const auto& childNode : n.childNodes)
{
- rns->clone(result);
+ const auto& primitiveApproximation = childNode->getPrimitiveApproximation();
+ n.node_cloned->getPrimitiveApproximation().join(primitiveApproximation.clone().localTransformation(globalPoseInv * childNode->getGlobalPose()));
+
+ // attach sensors
+ for (const auto& sensor : childNode->getSensors())
+ {
+ SensorPtr s = sensor->clone(n.node_cloned);
+ s->setRobotNodeToSensorTransformation(globalPoseInv * sensor->getGlobalPose());
+ }
}
}
- result->setGlobalPose(robot->getGlobalPose());
- if(robot->getHumanMapping().has_value())
+ reducedModel->setGlobalPose(globalPose);
+ reducedModel->setJointValues(joint_values);
+
+ cloneRNS(robot, reducedModel);
+
+ if(robot.getHumanMapping().has_value())
{
- result->registerHumanMapping(robot->getHumanMapping().value());
+ reducedModel->registerHumanMapping(robot.getHumanMapping().value());
}
- return result;
+ return reducedModel;
}
void RobotFactory::cloneRecursiveUnite(RobotPtr robot, RobotNodePtr currentNode, RobotNodePtr currentNodeClone, std::vector<std::string> uniteWithAllChildren)
diff --git a/VirtualRobot/RobotFactory.h b/VirtualRobot/RobotFactory.h
index c97ed648f012e92106d31f9af9ea4098e4b30e55..0b5303446453bcd3bf55baee5bffc92b7308374a 100644
--- a/VirtualRobot/RobotFactory.h
+++ b/VirtualRobot/RobotFactory.h
@@ -79,22 +79,45 @@ namespace VirtualRobot
*/
static RobotPtr clone(RobotPtr robot, const std::string& name, CollisionCheckerPtr collisionChecker = CollisionCheckerPtr(), float scaling = 1.0f);
+ /*!
+ * \brief Clones all nodesets from one robot to another that fullfill the condition that all nodes are contained in the other robot
+ * \param from The nodesets copied from
+ * \param to The nodesets copied to
+ */
+ static void cloneRNS(const Robot& from, RobotPtr to);
+
/*!
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)
+ The human mapping is cloned but it is not guranteed that all joints are still contained in the robot.
\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. node i must be located before node i+1 in the kinematic structure of the robot.
\param name The new name
+ \param addTCP Wheter the tcp of the robot node set should also be added
*/
- static RobotPtr cloneSubSet(RobotPtr robot, RobotNodeSetPtr rns, const std::string& name);
+ static RobotPtr cloneSubSet(RobotPtr robot, RobotNodeSetPtr rns, const std::string& name, bool addTCP = false);
/*!
Creates a robot clone with reduced structure.
+ The human mapping is cloned but it is not guranteed that all joints are still contained in the robot.
\param robot The robot to clone.
\param uniteWithAllChildren List of RobotNodeNames. Each listed robot ndoe is united with all of its children to one fixed RobotNode.
This means that all related coordinate systems and joints will not be present in the clone. The visualizations are united.
*/
static RobotPtr cloneUniteSubsets(RobotPtr robot, const std::string& name, std::vector<std::string> uniteWithAllChildren);
+ /*!
+ * Creates a cloned robot model that only include the given nodes by joining nodes together.
+ * The human mapping is cloned but it is not guranteed that all joints are still contained in the robot.
+ * Physics parameters such as segment inertia or information about whether to ignore collision between nodes are not (yet) updated correctly.
+ * \param robot The original robot
+ * \param actuatedJointNames The actuated joint nodes
+ * \param otherNodeNames Other robot nodes that should be contained with a fixed transformation (given current pose if actuated joint as these are converted to RobotNodeFixed)
+ * \return The reduced robot model or nullptr (if actuatedJointNames contains not only joint nodes that are in robot)
+ */
+ static VirtualRobot::RobotPtr
+ createReducedModel(Robot &robot, const std::vector<std::string>& actuatedJointNames,
+ const std::vector<std::string>& otherNodeNames = std::vector<std::string>());
+
/*!
Creates a clone with changed structure, so that the given robot node is the new root of the resulting kinematic tree.
*/
@@ -124,7 +147,6 @@ namespace VirtualRobot
static bool detach(RobotPtr robot, RobotNodePtr rn);
-
protected:
// some internal stuff
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index b74cee00f51a2669551be1d7d7cf409d59a3de67..c512b5b35cab0470886689b7dba423bd63e542d3 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -987,6 +987,7 @@ namespace VirtualRobot
result->setGlobalPose(getGlobalPose());
result->scaling = scaling;
result->basePath = basePath;
+ result->primitiveApproximation = primitiveApproximation.clone();
return result;
}
@@ -1472,9 +1473,14 @@ namespace VirtualRobot
std::vector<char> cstr(collisionModelXML.size() + 1); // Create char buffer to store string copy
strcpy(cstr.data(), collisionModelXML.c_str()); // Copy string into char buffer
doc.parse<0>(cstr.data());
- collisionModel = BaseIO::processCollisionTag(doc.first_node(), name, basePath);
- if (collisionModel && scaling != 1.0f) {
- collisionModel = collisionModel->clone(collisionChecker, scaling);
+ auto collisionModel = BaseIO::processCollisionTag(doc.first_node(), name, basePath);
+ if (collisionModel && scaling != 1.0f)
+ {
+ setCollisionModel(collisionModel->clone(collisionChecker, scaling));
+ }
+ else
+ {
+ setCollisionModel(collisionModel);
}
reloaded = true;
}
@@ -1485,10 +1491,18 @@ namespace VirtualRobot
strcpy(cstr.data(), visualizationModelXML.c_str()); // Copy string into char buffer
doc.parse<0>(cstr.data());
bool useAsColModel;
- visualizationModel = BaseIO::processVisualizationTag(doc.first_node(), name, basePath, useAsColModel);
- if (visualizationModel && scaling != 1.0f) visualizationModel = visualizationModel->clone(true, scaling);
- if (visualizationModel && collisionModel == nullptr && (useVisAsColModelIfMissing || useAsColModel)) {
- collisionModel.reset(new CollisionModel(visualizationModel->clone(true), getName() + "_VISU_ColModel", CollisionCheckerPtr()));
+ auto visualizationModel = BaseIO::processVisualizationTag(doc.first_node(), name, basePath, useAsColModel);
+ if (visualizationModel && scaling != 1.0f)
+ {
+ setVisualization(visualizationModel->clone(true, scaling));
+ }
+ else
+ {
+ setVisualization(visualizationModel);
+ }
+ if (visualizationModel && collisionModel == nullptr && (useVisAsColModelIfMissing || useAsColModel))
+ {
+ setCollisionModel(std::make_shared<CollisionModel>(visualizationModel->clone(true), getName() + "_VISU_ColModel", CollisionCheckerPtr()));
}
reloaded = true;
}
@@ -1501,4 +1515,149 @@ namespace VirtualRobot
const std::string &SceneObject::getVisualizationModelXML() const {
return visualizationModelXML;
}
+
+ const SceneObject::PrimitiveApproximation &SceneObject::getPrimitiveApproximation() const
+ {
+ return primitiveApproximation;
+ }
+
+ SceneObject::PrimitiveApproximation &SceneObject::getPrimitiveApproximation()
+ {
+ return primitiveApproximation;
+ }
+
+ void SceneObject::setPrimitiveApproximationModel(const std::vector<std::string> &ids, bool loadDefault)
+ {
+ auto primitives = getPrimitiveApproximation().getModels(ids, loadDefault);
+ if (primitives.empty())
+ {
+ setCollisionModel(nullptr);
+ }
+ else
+ {
+ VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
+ setCollisionModel(std::make_shared<CollisionModel>(visualizationFactory->getVisualizationFromPrimitives(primitives),
+ getName() + "_PrimitiveModel", CollisionCheckerPtr()));
+ }
+
+ // recursive
+ for (auto &child : children)
+ {
+ child->setPrimitiveApproximationModel(ids, loadDefault);
+ }
+ }
+
+ void SceneObject::getPrimitiveApproximationIDs(std::set<std::string> &ids) const
+ {
+ primitiveApproximation.getPrimitiveApproximationIDs(ids);
+
+ // recursive
+ for (auto &child : children)
+ {
+ child->getPrimitiveApproximationIDs(ids);
+ }
+ }
+
+
+
+ std::vector<Primitive::PrimitivePtr> SceneObject::PrimitiveApproximation::getModels(const std::vector<std::string> &ids, bool loadDefault) const
+ {
+ std::vector<Primitive::PrimitivePtr> result;
+ if (loadDefault)
+ {
+ result.insert(result.end(), defaultPrimitives.begin(), defaultPrimitives.end());
+ }
+ for (const std::string &id : ids)
+ {
+ if (primitives.count(id) > 0)
+ {
+ result.insert(result.end(), primitives.at(id).begin(), primitives.at(id).end());
+ }
+ }
+ return result;
+ }
+
+
+ void SceneObject::PrimitiveApproximation::addModel(const std::vector<Primitive::PrimitivePtr> &primitives, const std::string &id)
+ {
+ if (id.empty())
+ {
+ defaultPrimitives.insert(defaultPrimitives.end(), primitives.begin(), primitives.end());
+ }
+ else if (this->primitives.count(id) == 0)
+ {
+ this->primitives[id] = primitives;
+ }
+ else
+ {
+ this->primitives[id].insert(this->primitives[id].end(), primitives.begin(), primitives.end());
+ }
+ }
+
+ void SceneObject::PrimitiveApproximation::getPrimitiveApproximationIDs(std::set<std::string> &ids) const
+ {
+ for (const auto& [id, _] : primitives) {
+ ids.insert(id);
+ }
+ }
+
+ SceneObject::PrimitiveApproximation SceneObject::PrimitiveApproximation::clone() const
+ {
+ PrimitiveApproximation cloned;
+ for (const auto &primitive : this->defaultPrimitives)
+ {
+ cloned.defaultPrimitives.push_back(primitive->clone());
+ }
+ for (const auto &[id, primitives] : this->primitives)
+ {
+ std::vector<Primitive::PrimitivePtr> clonedPrimitives;
+ for (const auto &primitive : primitives)
+ {
+ clonedPrimitives.push_back(primitive->clone());
+ }
+ cloned.primitives[id] = clonedPrimitives;
+ }
+ return cloned;
+ }
+
+ SceneObject::PrimitiveApproximation &SceneObject::PrimitiveApproximation::localTransformation(const Eigen::Matrix4f &localTransformation)
+ {
+ for (auto& primitive : this->defaultPrimitives)
+ {
+ primitive->transform = localTransformation * primitive->transform;
+ }
+
+ for (auto& [id, primitives] : this->primitives)
+ {
+ for (auto& primitive : primitives)
+ {
+ primitive->transform = localTransformation * primitive->transform;
+ }
+ }
+ return *this;
+ }
+
+ void SceneObject::PrimitiveApproximation::join(const PrimitiveApproximation &primitiveApproximation, const Eigen::Matrix4f &localTransformation)
+ {
+ auto cloned = primitiveApproximation.clone().localTransformation(localTransformation);
+ this->defaultPrimitives.insert(this->defaultPrimitives.end(), cloned.defaultPrimitives.begin(), cloned.defaultPrimitives.end());
+
+ for (auto& [id, primitives] : cloned.primitives)
+ {
+ if (this->primitives.count(id) > 0)
+ {
+ this->primitives.at(id).insert(this->primitives.at(id).end(), primitives.begin(), primitives.end());
+ }
+ else
+ {
+ this->primitives[id] = primitives;
+ }
+ }
+ }
+
+ bool SceneObject::PrimitiveApproximation::empty() const
+ {
+ return defaultPrimitives.empty() && primitives.empty();
+ }
+
} // namespace
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index ad6bce5d835f3d1cecd452fcf1eeb753b068b434..aefd37fc17badf91da02a82b8691284a7be07e23 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -30,6 +30,9 @@
#include <string>
#include <vector>
#include <filesystem>
+#include <map>
+#include <set>
+#include "Primitive.h"
namespace VirtualRobot
@@ -86,6 +89,27 @@ namespace VirtualRobot
std::vector< std::string > ignoreCollisions; // ignore collisions with other objects (only used within collision engines)
};
+ struct VIRTUAL_ROBOT_IMPORT_EXPORT PrimitiveApproximation
+ {
+ std::vector<Primitive::PrimitivePtr> getModels(const std::vector<std::string> &ids, bool loadDefault = true) const;
+
+ void addModel(const std::vector<Primitive::PrimitivePtr> &primitives, const std::string &id = std::string());
+
+ void getPrimitiveApproximationIDs(std::set<std::string> &ids) const;
+
+ PrimitiveApproximation clone() const;
+
+ PrimitiveApproximation& localTransformation(const Eigen::Matrix4f &localTransformation);
+
+ void join(const PrimitiveApproximation& primitiveApproximation, const Eigen::Matrix4f& localTransformation = Eigen::Matrix4f::Identity());
+
+ bool empty() const;
+
+ private:
+ std::vector<Primitive::PrimitivePtr> defaultPrimitives;
+ std::map<std::string, std::vector<Primitive::PrimitivePtr>> primitives;
+ };
+
/*!
*/
SceneObject(const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(), CollisionModelPtr collisionModel = CollisionModelPtr(), const Physics& p = Physics(), CollisionCheckerPtr colChecker = CollisionCheckerPtr());
@@ -398,6 +422,19 @@ namespace VirtualRobot
const std::string& getVisualizationModelXML() const;
+ const PrimitiveApproximation& getPrimitiveApproximation() const;
+
+ PrimitiveApproximation& getPrimitiveApproximation();
+
+ void setPrimitiveApproximation(const PrimitiveApproximation& primitiveApproximation)
+ {
+ this->primitiveApproximation = primitiveApproximation;
+ }
+
+ void setPrimitiveApproximationModel(const std::vector<std::string> &ids, bool loadDefault = true);
+
+ void getPrimitiveApproximationIDs(std::set<std::string> &ids) const;
+
protected:
virtual SceneObject* _clone(const std::string& name, CollisionCheckerPtr colChecker = CollisionCheckerPtr(), float scaling = 1.0f) const;
@@ -447,6 +484,8 @@ namespace VirtualRobot
CollisionCheckerPtr collisionChecker;
float scaling = 1.0f;
+
+ PrimitiveApproximation primitiveApproximation;
};
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 3e9cd8a83bab578f2ef3d2cf22f17c5542d2b616..5c8f40d91d39e746aa57c1dc7f64e2113e90e554 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -146,14 +146,11 @@ namespace VirtualRobot
SoSeparator* coinVisualization = new SoSeparator();
coinVisualization->ref();
- Eigen::Matrix4f currentTransform = Eigen::Matrix4f::Identity();
-
for (auto p : primitives)
{
- currentTransform *= p->transform;
SoSeparator* soSep = new SoSeparator();
SoNode* pNode = GetNodeFromPrimitive(p, boundingBox, color);
- soSep->addChild(getMatrixTransformScaleMM2M(currentTransform));
+ soSep->addChild(getMatrixTransformScaleMM2M(p->transform));
soSep->addChild(pNode);
coinVisualization->addChild(soSep);
}
@@ -205,6 +202,37 @@ namespace VirtualRobot
soCylinder->height = cylinder->height / 1000.f;
coinVisualization->addChild(soCylinder);
}
+ else if (primitive->type == Primitive::Capsule::TYPE)
+ {
+ // TODO find a better visualization for capsule
+ Primitive::Capsule* cylinder = std::dynamic_pointer_cast<Primitive::Capsule>(primitive).get();
+ SoCylinder* soCylinder = new SoCylinder;
+ soCylinder->radius = cylinder->radius / 1000.f;
+ soCylinder->height = cylinder->height / 1000.f;
+ coinVisualization->addChild(soCylinder);
+
+ {
+ SoSeparator* sep = new SoSeparator();
+ SoTranslation* transl = new SoTranslation;
+ sep->addChild(transl);
+ transl->translation.setValue(0, -cylinder->height / 2000.f, 0.);
+ SoSphere* soSphere = new SoSphere;
+ soSphere->radius = soCylinder->radius;
+ sep->addChild(soSphere);
+ coinVisualization->addChild(sep);
+ }
+
+ {
+ SoSeparator* sep = new SoSeparator();
+ SoTranslation* transl = new SoTranslation;
+ sep->addChild(transl);
+ transl->translation.setValue(0, cylinder->height / 2000.f, 0.);
+ SoSphere* soSphere = new SoSphere;
+ soSphere->radius = soCylinder->radius;
+ sep->addChild(soSphere);
+ coinVisualization->addChild(sep);
+ }
+ }
if (boundingBox && coinVisualization)
{
@@ -4345,7 +4373,6 @@ namespace VirtualRobot
return root;
}
-
SoGroup* CoinVisualizationFactory::convertSoFileChildren(SoGroup* orig)
{
if (!orig)
diff --git a/VirtualRobot/Visualization/VisualizationFactory.cpp b/VirtualRobot/Visualization/VisualizationFactory.cpp
index 3a4f8a5924329ee527e12adc16b0a6898b464225..26cfa61e5fdefefbf0cf719f3cb9354bbd8079fe 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/VisualizationFactory.cpp
@@ -14,7 +14,7 @@ namespace VirtualRobot
void
VisualizationFactory::applyDisplacement(VisualizationNodePtr visu,
- Eigen::Matrix4f& displacement)
+ const Eigen::Matrix4f& displacement)
{
if (visu)
{
diff --git a/VirtualRobot/Visualization/VisualizationFactory.h b/VirtualRobot/Visualization/VisualizationFactory.h
index 94f1142cdbd9b6bcf8cd19c8a6c29fcaf5aed91a..537731321099954000d1609bc441a52d927eee8b 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.h
+++ b/VirtualRobot/Visualization/VisualizationFactory.h
@@ -206,7 +206,7 @@ namespace VirtualRobot
/*!
Move local visualization by homogeneous matrix m. (MM)
*/
- virtual void applyDisplacement(VisualizationNodePtr visu, Eigen::Matrix4f& displacement);
+ virtual void applyDisplacement(VisualizationNodePtr visu, const Eigen::Matrix4f& displacement);
/*!
Create an empty VisualizationNode.
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index fdfd4161c58e646a5a0687aebbfc1975eb7a7626..988ca660b74dd8b791ab359ca9f29020cfc9a8d3 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -1258,6 +1258,22 @@ namespace VirtualRobot
return collisionModel;
}
+ void BaseIO::processPrimitiveModelTag(SceneObject::PrimitiveApproximation& primitiveApproximation,
+ const rapidxml::xml_node<char>* primitiveApproximationXMLNode)
+ {
+ rapidxml::xml_attribute<>* attr = primitiveApproximationXMLNode->first_attribute("enable", 0, false);
+ if (!attr || isTrue(attr->value()))
+ {
+ auto primitives = processPrimitives(primitiveApproximationXMLNode);
+ rapidxml::xml_attribute<>* idAttr = primitiveApproximationXMLNode->first_attribute("id", 0, false);
+ std::string id = idAttr ? idAttr->value() : std::string();
+ if (primitives.size() > 0)
+ {
+ primitiveApproximation.addModel(primitives, id);
+ }
+ }
+ }
+
std::vector<VisualizationNodePtr> BaseIO::processVisuFiles(const rapidxml::xml_node<char>* visualizationXMLNode, const std::string& basePath, std::string& fileType)
{
const rapidxml::xml_node<>* node = visualizationXMLNode;
@@ -1415,6 +1431,13 @@ namespace VirtualRobot
cylinder->height = convertToFloat(primitiveXMLNode->first_attribute("height", 0, false)->value()) * lenFactor;
primitive.reset(cylinder);
}
+ else if (pName == "capsule")
+ {
+ Primitive::Capsule* capsule = new Primitive::Capsule;
+ capsule->radius = convertToFloat(primitiveXMLNode->first_attribute("radius", 0, false)->value()) * lenFactor;
+ capsule->height = convertToFloat(primitiveXMLNode->first_attribute("height", 0, false)->value()) * lenFactor;
+ primitive.reset(capsule);
+ }
else
{
VR_ERROR << "Unknown primitive type \"" << pName << "\"; skipping";
diff --git a/VirtualRobot/XML/BaseIO.h b/VirtualRobot/XML/BaseIO.h
index 41a2d9a2507d686d994d2fe2a7001dd1b6db7bd9..7115303b2d9a88d30a01ef4627f9644092ce2843 100644
--- a/VirtualRobot/XML/BaseIO.h
+++ b/VirtualRobot/XML/BaseIO.h
@@ -98,6 +98,7 @@ namespace VirtualRobot
static VisualizationNodePtr processVisualizationTag(const rapidxml::xml_node<char>* visuXMLNode, const std::string& tagName, const std::string& basePath, bool& useAsColModel);
static CollisionModelPtr processCollisionTag(const rapidxml::xml_node<char>* colXMLNode, const std::string& tagName, const std::string& basePath);
+ static void processPrimitiveModelTag(SceneObject::PrimitiveApproximation& primitiveApproximation, const rapidxml::xml_node<char>* colXMLNode);
static std::vector<Primitive::PrimitivePtr> processPrimitives(const rapidxml::xml_node<char>* primitivesXMLNode);
static void processPhysicsTag(const rapidxml::xml_node<char>* physicsXMLNode, const std::string& nodeName, SceneObject::Physics& physics);
static RobotNodeSetPtr processRobotNodeSet(const rapidxml::xml_node<char>* setXMLNode, RobotPtr robo, const std::string& robotRootNode, int& robotNodeSetCounter);
diff --git a/VirtualRobot/XML/ObjectIO.cpp b/VirtualRobot/XML/ObjectIO.cpp
index c47299a9a8f82827654e26d91a0496d74f20b4ff..25f5c295d61c643a97306c69b4647e1efe4933a1 100644
--- a/VirtualRobot/XML/ObjectIO.cpp
+++ b/VirtualRobot/XML/ObjectIO.cpp
@@ -152,6 +152,7 @@ namespace VirtualRobot
SceneObject::Physics physics;
bool physicsDefined = false;
Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
+ SceneObject::PrimitiveApproximation primitiveApproximation;
// get name
std::string objName = processNameAttribute(objectXMLNode);
@@ -218,6 +219,10 @@ namespace VirtualRobot
collisionModel = processCollisionTag(node, objName, basePath);
colProcessed = true;
}
+ else if (nodeName == "primitivemodel")
+ {
+ processPrimitiveModelTag(primitiveApproximation, node);
+ }
else if (nodeName == "physics")
{
THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in Obstacle '" << objName << "'." << endl);
@@ -239,6 +244,7 @@ namespace VirtualRobot
// build object
ObstaclePtr object(new Obstacle(objName, visualizationNode, collisionModel, physics));
object->setGlobalPose(globalPose);
+ object->setPrimitiveApproximation(primitiveApproximation);
return object;
}
@@ -300,6 +306,7 @@ namespace VirtualRobot
std::vector<GraspSetPtr> graspSets;
Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
std::vector< rapidxml::xml_node<>* > sensorTags;
+ SceneObject::PrimitiveApproximation primitiveApproximation;
// get name
std::string objName = processNameAttribute(objectXMLNode);
@@ -366,6 +373,10 @@ namespace VirtualRobot
collisionModel = processCollisionTag(node, objName, basePath);
colProcessed = true;
}
+ else if (nodeName == "primitivemodel")
+ {
+ processPrimitiveModelTag(primitiveApproximation, node);
+ }
else if (nodeName == "physics")
{
THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in ManipulationObject '" << objName << "'." << endl);
@@ -412,6 +423,7 @@ namespace VirtualRobot
}
object->setGlobalPose(globalPose);
+ object->setPrimitiveApproximation(primitiveApproximation);
return object;
}
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index 4c6e2e61c8a1c1073b329258ee95dd1cc1c38524..6b5808efc0744a4f3cb575a71c5901a2cfc3957e 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -641,14 +641,15 @@ namespace VirtualRobot
// collision information
bool colProcessed = false;
- VisualizationNodePtr visualizationNode;
- CollisionModelPtr collisionModel;
+ VisualizationNodePtr visualizationNode = nullptr;
+ CollisionModelPtr collisionModel = nullptr;
RobotNodePtr robotNode;
std::string visualizationModelXML;
std::string collisionModelXML;
SceneObject::Physics physics;
bool physicsDefined = false;
Eigen::Matrix4f transformMatrix = Eigen::Matrix4f::Identity();
+ SceneObject::PrimitiveApproximation primitiveApproximation;
rapidxml::xml_node<>* node = robotNodeXMLNode->first_node();
rapidxml::xml_node<>* jointNodeXML = nullptr;
@@ -718,6 +719,10 @@ namespace VirtualRobot
collisionModelXML = ss.str();
}
}
+ else if (nodeName == "primitivemodel")
+ {
+ processPrimitiveModelTag(primitiveApproximation, node);
+ }
else if (nodeName == "child")
{
processChildNode(node, childrenNames);
@@ -776,6 +781,7 @@ namespace VirtualRobot
robotNode->basePath = basePath;
robotNode->visualizationModelXML = visualizationModelXML;
robotNode->collisionModelXML = collisionModelXML;
+ robotNode->setPrimitiveApproximation(primitiveApproximation);
// process sensors
for (auto& sensorTag : sensorTags)
diff --git a/VirtualRobot/examples/RobotViewer/RobotViewer.ui b/VirtualRobot/examples/RobotViewer/RobotViewer.ui
index 1b391d13b77fcb3a30ab6173c3cca9511f9124e6..bae7f369227dd85a6b7099c362828aedea30bb7a 100644
--- a/VirtualRobot/examples/RobotViewer/RobotViewer.ui
+++ b/VirtualRobot/examples/RobotViewer/RobotViewer.ui
@@ -15,7 +15,7 @@
</property>
<widget class="QWidget" name="centralwidget">
<layout class="QGridLayout" name="gridLayout">
- <item row="0" column="0">
+ <item row="0" column="1">
<widget class="QSplitter" name="splitter">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -27,6 +27,12 @@
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="minimumSize">
+ <size>
+ <width>600</width>
+ <height>0</height>
+ </size>
+ </property>
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
@@ -49,8 +55,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>841</width>
- <height>1092</height>
+ <width>483</width>
+ <height>1094</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
@@ -293,59 +299,63 @@
<item row="0" column="0" colspan="2">
<widget class="QWidget" name="widget" native="true">
<layout class="QGridLayout" name="gridLayout_5">
- <item row="6" column="0">
- <widget class="QCheckBox" name="checkBoxPhysicsCoM">
- <property name="text">
- <string>CoM</string>
- </property>
+ <item row="2" column="1">
+ <widget class="QComboBox" name="comboBoxPrimitiveModel">
</widget>
</item>
- <item row="6" column="1">
- <widget class="QCheckBox" name="checkBoxPhysicsInertia">
+ <item row="5" column="0">
+ <widget class="QLabel" name="label">
<property name="text">
- <string>Inertia</string>
+ <string>Full model options</string>
</property>
</widget>
</item>
- <item row="4" column="0" colspan="2">
+ <item row="8" column="0">
<widget class="QCheckBox" name="checkBoxRobotCoordSystems">
<property name="text">
<string>Coordinate systems</string>
</property>
</widget>
</item>
- <item row="5" column="0" colspan="2">
+ <item row="9" column="0">
<widget class="QCheckBox" name="checkBoxRobotSensors">
<property name="text">
<string>Sensors</string>
</property>
</widget>
</item>
- <item row="3" column="0" colspan="2">
- <widget class="QCheckBox" name="checkBoxStructure">
+ <item row="2" column="0">
+ <widget class="QCheckBox" name="checkBoxColModel">
<property name="text">
- <string>Robot structure</string>
+ <string>Collision model</string>
</property>
</widget>
</item>
- <item row="2" column="0" colspan="2">
+ <item row="6" column="0">
<widget class="QCheckBox" name="checkBoxFullModel">
<property name="text">
<string>IV Model</string>
</property>
</widget>
</item>
- <item row="1" column="0" colspan="2">
- <widget class="QLabel" name="label">
+ <item row="7" column="0">
+ <widget class="QCheckBox" name="checkBoxStructure">
<property name="text">
- <string>Full model options</string>
+ <string>Robot structure</string>
</property>
</widget>
</item>
- <item row="0" column="0" colspan="2">
- <widget class="QCheckBox" name="checkBoxColModel">
+ <item row="10" column="0">
+ <widget class="QCheckBox" name="checkBoxPhysicsCoM">
<property name="text">
- <string>Collision model</string>
+ <string>CoM</string>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="1">
+ <widget class="QCheckBox" name="checkBoxPhysicsInertia">
+ <property name="text">
+ <string>Inertia</string>
</property>
</widget>
</item>
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
index b3b714c236a9766fff67f0e50f1916800261a449..2326f0b53e784773254c4d3fae3d16dda58bd9dc 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.cpp
@@ -6,6 +6,7 @@
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Import/RobotImporterFactory.h>
#include <VirtualRobot/CollisionDetection/CDManager.h>
+#include <VirtualRobot/RobotFactory.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
@@ -106,7 +107,6 @@ void showRobotWindow::setupUI()
// setup
viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
-
viewer->setGLRenderAction(new SoLineHighlightRenderAction);
viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
viewer->setFeedbackVisibility(true);
@@ -133,6 +133,7 @@ void showRobotWindow::setupUI()
connect(UI.checkBoxPhysicsInertia, SIGNAL(clicked()), this, SLOT(displayPhysics()));
connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(rebuildVisualization()));
+ connect(UI.comboBoxPrimitiveModel, SIGNAL(activated(int)), this, SLOT(selectPrimitiveModel(int)));
connect(UI.checkBoxRobotSensors, SIGNAL(clicked()), this, SLOT(showSensors()));
connect(UI.checkBoxStructure, SIGNAL(clicked()), this, SLOT(robotStructure()));
UI.checkBoxFullModel->setChecked(true);
@@ -240,6 +241,9 @@ void showRobotWindow::robotFullModel()
bool showFullModel = UI.checkBoxFullModel->checkState() == Qt::Checked;
+ if (UI.checkBoxColModel->checkState())
+ rebuildVisualization();
+
robot->setupVisualization(showFullModel, true);
}
@@ -252,29 +256,46 @@ void showRobotWindow::rebuildVisualization()
}
robotSep->removeAllChildren();
- //setRobotModelShape(UI.checkBoxColModel->state() == QCheckBox::On);
- useColModel = UI.checkBoxColModel->checkState() == Qt::Checked;
- //bool sensors = UI.checkBoxRobotSensors->checkState() == Qt::Checked;
- SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked()) ? SceneObject::Collision : SceneObject::Full;
-
- visualization = robot->getVisualization<CoinVisualization>(colModel);
- SoNode* visualisationNode = nullptr;
- if (visualization)
+ if (UI.checkBoxColModel->checkState() == Qt::Checked)
{
- visualisationNode = visualization->getCoinVisualization();
+ visualization = robot->getVisualization<CoinVisualization>(SceneObject::Collision);
+ SoNode* visualisationNode = nullptr;
+
+ if (visualization)
+ {
+ visualisationNode = visualization->getCoinVisualization();
+ }
+
+ if (visualisationNode)
+ {
+ robotSep->addChild(visualisationNode);
+ }
}
- if (visualisationNode)
+ if (UI.checkBoxFullModel->checkState() == Qt::Checked
+ || UI.checkBoxStructure->checkState() == Qt::Checked
+ || UI.checkBoxRobotSensors->checkState() == Qt::Checked
+ || UI.checkBoxRobotCoordSystems->checkState() == Qt::Checked)
{
- robotSep->addChild(visualisationNode);
+ visualization = robot->getVisualization<CoinVisualization>(SceneObject::Full);
+ SoNode* visualisationNode = nullptr;
+
+ if (visualization)
+ {
+ visualisationNode = visualization->getCoinVisualization();
+ }
+
+ if (visualisationNode)
+ {
+ robotSep->addChild(visualisationNode);
+ }
}
selectJoint(UI.comboBoxJoint->currentIndex());
UI.checkBoxStructure->setEnabled(!useColModel);
UI.checkBoxRobotSensors->setEnabled(!useColModel);
- UI.checkBoxFullModel->setEnabled(!useColModel);
UI.checkBoxRobotCoordSystems->setEnabled(!useColModel);
}
@@ -767,8 +788,6 @@ void showRobotWindow::loadRobot()
}
robot = importer->loadFromFile(m_sRobotFilename, RobotIO::eFull);
-
-
}
catch (VirtualRobotException& e)
{
@@ -830,6 +849,16 @@ void showRobotWindow::updatRobotInfo()
displayTriangles();
+ UI.comboBoxPrimitiveModel->clear();
+ std::set<std::string> ids;
+ robot->getPrimitiveApproximationIDs(ids);
+ UI.comboBoxPrimitiveModel->addItem(""); // TODO
+ for (const std::string& id : ids)
+ {
+ UI.comboBoxPrimitiveModel->addItem(QString::fromStdString(id));
+ }
+
+
// build visualization
rebuildVisualization();
robotStructure();
@@ -879,6 +908,15 @@ void showRobotWindow::openHand()
}
}
+
+void showRobotWindow::selectPrimitiveModel(int nr)
+{
+ std::cout << "Selecting primitive model nr " << nr << std::endl;
+ std::vector<std::string> ids = { UI.comboBoxPrimitiveModel->itemText(nr).toStdString() };
+ robot->setPrimitiveApproximationModel(ids, true);
+ rebuildVisualization();
+}
+
void showRobotWindow::selectEEF(int nr)
{
std::cout << "Selecting EEF nr " << nr << std::endl;
diff --git a/VirtualRobot/examples/RobotViewer/showRobotWindow.h b/VirtualRobot/examples/RobotViewer/showRobotWindow.h
index 3b62ad9304fee90a0e8380d1ce2b721160a6e4a1..9ee20da4930cb01c4135ae0a5dcf3f54dc059756 100644
--- a/VirtualRobot/examples/RobotViewer/showRobotWindow.h
+++ b/VirtualRobot/examples/RobotViewer/showRobotWindow.h
@@ -49,6 +49,7 @@ public slots:
void rebuildVisualization();
void showRobot();
void loadRobot();
+ void selectPrimitiveModel(int nr);
void selectJoint(int nr);
void selectRNS(int nr);
void jointValueChanged(int pos);