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 284e4e57904d69f4122bd7a6576e4321976b7e7e..c8ecce937fe31c10d018550bf11bf5de3be62116 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -611,6 +611,7 @@ namespace VirtualRobot
}
result->basePath = basePath;
result->setScaling(actualScaling);
+ result->primitiveApproximation = primitiveApproximation.clone();
return result;
}
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..dd3c97134ccf6e52754ded8ef45d83b74a165659 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,11 @@ namespace VirtualRobot
float height;
float depth;
std::string toXMLString(int tabs = 0) override;
+
+ std::unique_ptr<Primitive> clone() const final
+ {
+ return std::make_unique<Box>(width, height, depth);
+ }
};
class VIRTUAL_ROBOT_IMPORT_EXPORT Sphere : public Primitive
@@ -48,6 +55,11 @@ 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
+ {
+ return std::make_unique<Sphere>(radius);
+ }
};
class VIRTUAL_ROBOT_IMPORT_EXPORT Cylinder : public Primitive
@@ -59,6 +71,30 @@ namespace VirtualRobot
float radius;
float height;
std::string toXMLString(int tabs = 0) override;
+
+ std::unique_ptr<Primitive> clone() const final
+ {
+ return std::make_unique<Cylinder>(radius, height);
+ }
+ };
+
+ /**
+ * @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
+ {
+ return std::make_unique<Capsule>(radius, height);
+ }
};
typedef std::shared_ptr<Primitive> PrimitivePtr;
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index a5b3652b05ce4f6852e82a75db699af4d762b6a0..0310836a129da4565007c25597504ba7436c8488 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;
}
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index af42dc2bbd2a9dd82dc4e439f6a4ddb39d00be97..4a5abee893c8ac1be62e455ca5a989dfc2415190 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -985,6 +985,7 @@ namespace VirtualRobot
result->setGlobalPose(getGlobalPose());
result->scaling = scaling;
result->basePath = basePath;
+ result->primitiveApproximation = primitiveApproximation.clone();
return result;
}
@@ -1470,9 +1471,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;
}
@@ -1483,10 +1489,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;
}
@@ -1499,4 +1513,109 @@ 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;
+ }
+
} // namespace
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index ad6bce5d835f3d1cecd452fcf1eeb753b068b434..1832ffe6f23d02983e4f888b55afa785e8db13ea 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,21 @@ 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;
+
+ 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 +416,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 +478,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 fe1f660317cbf154589b0043317103130b54a625..466541151c7872ff286175bcc0284b893b12eb4d 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -205,6 +205,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)
{
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..01b3ec6bbd087ba004eca36a54a55bf639851791 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -649,6 +649,8 @@ namespace VirtualRobot
SceneObject::Physics physics;
bool physicsDefined = false;
Eigen::Matrix4f transformMatrix = Eigen::Matrix4f::Identity();
+ SceneObject::PrimitiveApproximation primitiveApproximation;
+ //primitiveApproximation.addModel({ std::make_shared<Primitive::Cylinder>(10, 10) }, "test");
rapidxml::xml_node<>* node = robotNodeXMLNode->first_node();
rapidxml::xml_node<>* jointNodeXML = nullptr;
@@ -718,6 +720,10 @@ namespace VirtualRobot
collisionModelXML = ss.str();
}
}
+ else if (nodeName == "primitivemodel")
+ {
+ processPrimitiveModelTag(primitiveApproximation, node);
+ }
else if (nodeName == "child")
{
processChildNode(node, childrenNames);
@@ -776,6 +782,7 @@ namespace VirtualRobot
robotNode->basePath = basePath;
robotNode->visualizationModelXML = visualizationModelXML;
robotNode->collisionModelXML = collisionModelXML;
+ robotNode->setPrimitiveApproximation(primitiveApproximation);
// process sensors
for (auto& sensorTag : sensorTags)