Added <PrimitiveModel>-tag to scene objects; Added capsule primitive

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);

VirtualRobot/Nodes/RobotNode.cpp

@@ -611,6 +611,7 @@ namespace VirtualRobot
         }
         result->basePath = basePath;
         result->setScaling(actualScaling);
+        result->primitiveApproximation = primitiveApproximation.clone();
         return result;
     }
 

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);

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

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;

VirtualRobot/Robot.cpp

@@ -923,6 +923,7 @@ namespace VirtualRobot
         {
             result->registerHumanMapping(getHumanMapping().value());
         }
+        result->primitiveApproximation = primitiveApproximation.clone();
 
         return result;
     }

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

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;
     };
 
 

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)
         {

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";

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);

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;
     }

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)