Merge branch 'master' into feat/hand-controls

source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp

@@ -18,12 +18,13 @@
 #include <ArmarXCore/core/time/Metronome.h>
 #include <ArmarXCore/core/time/forward_declarations.h>
 
-#include "RobotAPI/components/ArViz/Client/elements/Robot.h"
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/components/ArViz/Client/elements/Color.h>
+#include <RobotAPI/components/ArViz/Client/elements/Robot.h>
 #include <RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h>
 #include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
 #include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
 #include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
 #include <RobotAPI/libraries/armem/core/error.h>
 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
 #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
@@ -48,6 +49,11 @@ namespace armarx::armem::server::grasp
 
         defs->optional(enableVisualizeKnownGraspCandidates,
                        "p.enableVisualizeKnownGraspCandidates");
+        defs->optional(enableVisualizeKnownGraspCandidatesPreposes,
+                       "p.enableVisualizeKnownGraspCandidatesPreposes");
+        defs->optional(enableVisualizeKnownGraspCandidatesApproach,
+                       "p.enableVisualizeKnownGraspCandidatesApproach");
+
         defs->optional(frequencyHzVisualizeKnownGraspCandidates,
                        "p.frequencyHzVisualizeKnownGraspCandidates");
 
@@ -1016,16 +1022,55 @@ namespace armarx::armem::server::grasp
                                 const Eigen::Isometry3f eef_T_hand_root{
                                     handRootNode->getPoseInFrame(tcp)};
 
-                                const Eigen::Isometry3f pose =
+                                const Eigen::Isometry3f global_T_grasp_pose =
                                     Eigen::Isometry3f{object.objectPoseGlobal} *
-                                    Eigen::Isometry3f{grasp.pose}.inverse() * eef_T_hand_root;
+                                    Eigen::Isometry3f{grasp.pose}.inverse();
+
+                                // visualize grasp pose
+                                {
+                                    viz::Robot graspHandVisu(visuName + "_grasp");
+                                    graspHandVisu.useCollisionModel()
+                                        .file(visuRobotPartXML)
+                                        .pose(
+                                            Eigen::Isometry3f{global_T_grasp_pose * eef_T_hand_root}
+                                                .matrix());
+
+                                    layer.add(graspHandVisu);
+                                }
+
+                                // visualize prepose if available
+                                if (grasp.prepose.has_value())
+                                {
+                                    const Eigen::Isometry3f global_T_prepose =
+                                        Eigen::Isometry3f{object.objectPoseGlobal} *
+                                        Eigen::Isometry3f{grasp.prepose.value()}.inverse();
 
-                                viz::Robot handVisu(visuName);
-                                handVisu.useCollisionModel()
-                                    .file(visuRobotPartXML)
-                                    .pose(pose.matrix());
+                                    // visualize as prepose as hand
+                                    if (enableVisualizeKnownGraspCandidatesPreposes)
+                                    {
+                                        viz::Robot graspHandVisuPrepose(visuName + "_prepose");
+                                        graspHandVisuPrepose.useCollisionModel()
+                                            .file(visuRobotPartXML)
+                                            .pose(Eigen::Isometry3f{global_T_prepose *
+                                                                    eef_T_hand_root}
+                                                      .matrix())
+                                            .overrideColor(viz::Color::blue());
+
+                                        layer.add(graspHandVisuPrepose);
+                                    }
+
+                                    // visualize approach direction (arrow between poses)
+                                    if (enableVisualizeKnownGraspCandidatesApproach)
+                                    {
+                                        viz::Arrow arrow(visuName + "_approach");
+                                        arrow
+                                            .fromTo(global_T_prepose.translation(),
+                                                    global_T_grasp_pose.translation())
+                                            .color(viz::Color::green());
 
-                                layer.add(handVisu);
+                                        layer.add(arrow);
+                                    }
+                                }
                             }
                         }
                     }

source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h

@@ -120,6 +120,10 @@ namespace armarx::armem::server::grasp
         bool enableRemoteGui{true};
 
         bool enableVisualizeKnownGraspCandidates = false;
+        bool enableVisualizeKnownGraspCandidatesPreposes = false;
+        bool enableVisualizeKnownGraspCandidatesApproach = false;
+
+
         std::size_t frequencyHzVisualizeKnownGraspCandidates = 5;
 
         void visualizeGraspCandidates();

source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp

 #include <VirtualRobot/XML/ObjectIO.h>
 
-#include <set>
-
 #include <SimoxUtility/algorithm/string.h>
 #include <SimoxUtility/filesystem/list_directory.h>
 

source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp

@@ -185,7 +185,7 @@ namespace armarx
             {
                 if (_last_iteration_id != -1 && _last_iteration_id + 1 != step.iterationId)
                 {
-                    ARMARX_INFO << deactivateSpam(10)
+                    ARMARX_INFO << deactivateSpam(60)
                                 << "Missing Iterations or iterations out of order! "
                                 << "This should not happen. " << VAROUT(_last_iteration_id) << ", "
                                 << VAROUT(step.iterationId);

source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.cpp

@@ -3,6 +3,7 @@
 // ArmarX
 #include <opencv2/imgcodecs.hpp>
 #include <opencv2/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
 #include <opencv2/opencv.hpp>
 
 #include <RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h>

source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml

@@ -21,6 +21,10 @@
             <ObjectChild key='pose'>
                 <Pose />
             </ObjectChild>
+
+            <ObjectChild key='prepose'>
+                <Pose optional="true" />
+            </ObjectChild>
         </Object>
 
         <!-- A Grasp set is a set of grasps that need to be executed to grasp an object. The order is important! -->

source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp

 #include "KnownGraspProviderSegment.h"
 
+#include <SimoxUtility/algorithm/string/string_tools.h>
 #include <VirtualRobot/Grasping/GraspSet.h>
 #include <VirtualRobot/XML/ObjectIO.h>
 
@@ -63,9 +64,46 @@ namespace armarx::armem::grasping::segment
                     arondto::KnownGraspSet retGraspSet;
 
                     retGraspSet.name = graspSet->getName();
-                    retGraspSet.robot = graspSet->getRobotType();
+
+                    retGraspSet.robot = simox::alg::split(graspSet->getRobotType(), " ").front();
+
                     retGraspSet.endeffector = graspSet->getEndEffector();
 
+                    VirtualRobot::GraspSetPtr preGraspSet = [&graspSet]()
+                    {
+                        std::vector<VirtualRobot::GraspPtr> preGrasps;
+                        for (const auto& grasp : graspSet->getGrasps())
+                        {
+                            // check if grasp is a prepose by checking if grasp name ends with "_Prepose"
+                            if (simox::alg::ends_with(grasp->getName(), PREPOSE_SUFFIX))
+                            {
+                                preGrasps.emplace_back(grasp);
+                            }
+
+                            // everything else is assumed to be a grasp
+                        }
+
+                        // create a new grasp set with the preposes
+                        VirtualRobot::GraspSetPtr preGraspSet = graspSet->clone();
+                        preGraspSet->removeAllGrasps(); // just keep the EEF info etc
+                        for (const auto& preGrasp : preGrasps)
+                        {
+                            preGraspSet->addGrasp(preGrasp);
+                        }
+
+                        return preGraspSet;
+                    }();
+
+
+                    ARMARX_DEBUG << VAROUT(preGraspSet->getSize());
+
+                    // remove all preposes from the grasp set
+                    for (const auto& preGrasp : preGraspSet->getGrasps())
+                    {
+                        graspSet->removeGrasp(preGrasp);
+                    }
+
+
                     for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
                     {
                         ARMARX_CHECK_NOT_NULL(grasp);
@@ -76,8 +114,57 @@ namespace armarx::armem::grasping::segment
                         retGrasp.quality = grasp->getQuality();
                         retGrasp.creator = grasp->getCreationMethod();
                         retGrasp.pose = grasp->getTransformation();
-
-                        ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '"
+                        retGrasp.prepose.reset();
+
+                        // check if grasp has a prepose by checking if grasp name ends with "_Prepose"
+                        {
+                            const std::string prePoseName = retGrasp.name + PREPOSE_SUFFIX;
+                            
+                            ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
+                            
+                            if (preGraspSet->hasGrasp(prePoseName))
+                            {
+                                retGrasp.prepose =
+                                    preGraspSet->getGrasp(prePoseName)->getTransformation();
+
+                                // remove the prepose from the set as it found its match
+                                preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
+
+
+                                ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '" << retGrasp.name << "' in set '"
+                                           << retGraspSet.name << "' for obj '" << objectClassName
+                                           << "' with pose \n"
+                                           << retGrasp.prepose.value();
+                            }
+                        }
+
+                        // check if grasp has a prepose for a grasp with a specific name, e.g., "XY_Grasp" (GRASP_OPTIONAL_SUFFIX)
+                        if (// not retGrasp.prepose.has_value() and
+                            simox::alg::ends_with(retGrasp.name, GRASP_OPTIONAL_SUFFIX))
+                        {
+                            const std::string prePoseName =
+                                retGrasp.name.substr(
+                                    0, retGrasp.name.size() - std::strlen(GRASP_OPTIONAL_SUFFIX)) +
+                                PREPOSE_SUFFIX;
+
+                            ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
+
+                            if (preGraspSet->hasGrasp(prePoseName))
+                            {
+                                retGrasp.prepose =
+                                    preGraspSet->getGrasp(prePoseName)->getTransformation();
+
+                                // remove the prepose from the set as it found its match
+                                preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
+
+                                ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '" << retGrasp.name << "' in set '"
+                                           << retGraspSet.name << "' for obj '" << objectClassName
+                                           << "' with pose \n"
+                                           << retGrasp.prepose.value();
+                            }
+                        }
+
+                        ARMARX_DEBUG << "Found grasp '" << retGrasp.name << "' in set '"
                                        << retGraspSet.name << "' for obj '" << objectClassName
                                        << "' with pose \n"
                                        << retGrasp.pose;
@@ -85,6 +172,19 @@ namespace armarx::armem::grasping::segment
                         retGraspSet.grasps.push_back(retGrasp);
                     }
 
+                    // Now, check if there are any preposes left in the set. This should not have happened.
+                    if (preGraspSet->getSize() > 0)
+                    {
+                        ARMARX_WARNING << "Found " << preGraspSet->getSize()
+                                       << " preposes in the grasp set '" << retGraspSet.name
+                                       << "' for obj '" << objectClassName
+                                       << "' that do not have a corresponding grasp!";
+                        for(const auto& preGrasp : preGraspSet->getGrasps())
+                        {
+                            ARMARX_WARNING << "Prepose '" << preGrasp->getName();
+                        }
+                    }
+
                     ARMARX_CHECK(ret.graspSets.count(retGraspSet.robot + "/" + retGraspSet.name) ==
                                  0)
                         << "The grasp set `" << retGraspSet.robot + "/" + retGraspSet.name
@@ -132,13 +232,13 @@ namespace armarx::armem::grasping::segment
 
                 update.instancesData = {knownGraspCandidate->toAron()};
 
-                ARMARX_VERBOSE << VAROUT(knownGraspCandidate->graspSets.size());
+                ARMARX_DEBUG << VAROUT(knownGraspCandidate->graspSets.size());
                 for (const auto& gs : knownGraspCandidate->graspSets)
                 {
-                    ARMARX_VERBOSE << VAROUT(gs.second.grasps.size());
+                    ARMARX_DEBUG << VAROUT(gs.second.grasps.size());
                     for (const auto& grasp : gs.second.grasps)
                     {
-                        ARMARX_VERBOSE << VAROUT(grasp.name);
+                        ARMARX_DEBUG << VAROUT(grasp.name);
                     }
                 }
             }

source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h

@@ -23,5 +23,9 @@ namespace armarx::armem::grasping::segment
     public:
         static const constexpr char* CORE_SEGMENT_NAME = "KnownGraspCandidate";
         static const constexpr char* PROVIDER_SEGMENT_NAME = "PriorKnowledgeData";
+
+    private:
+        static const constexpr char* PREPOSE_SUFFIX = "_Prepose";
+        static const constexpr char* GRASP_OPTIONAL_SUFFIX = "_Grasp";
     };
 }

source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp

@@ -7,6 +7,7 @@
 #include <VirtualRobot/XML/RobotIO.h>
 
 #include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 
@@ -32,37 +33,27 @@ namespace armarx::armem::articulated_object
                                                   const std::string& instanceName,
                                                   VirtualRobot::RobotIO::RobotDescription loadMode)
     {
-        const auto descriptions = queryDescriptions(timestamp, providerName);
+        ARMARX_VERBOSE << "Getting `" << typeName + "/" + instanceName << "`";
 
-        ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+        const std::optional<ArticulatedObject> articulatedObjectDescription =
+            get(typeName + "/" + instanceName, timestamp, providerName);
+        ARMARX_CHECK_NOT_NULL(articulatedObjectDescription)
+            << "Failed to get articulated object `" << typeName << "/" << instanceName << "`";
 
-        const auto it = std::find_if(
-            descriptions.begin(),
-            descriptions.end(),
-            [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
-            { return desc.name == typeName; });
-
-        if (it == descriptions.end())
-        {
-            ARMARX_WARNING << "Description for articulate object with type <" << typeName
-                           << "> not (yet) available!";
-            return nullptr;
-        }
-
-        ARMARX_DEBUG << "Description for articulate object with type <" << typeName
-                     << "> available!";
-
-        auto obj = VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(), loadMode);
+        auto obj = VirtualRobot::RobotIO::loadRobot(
+            articulatedObjectDescription->description.xml.toSystemPath(), loadMode);
 
         if (not obj)
         {
-            ARMARX_WARNING << "Failed to load description for articulated object <" << typeName
-                           << ">!";
+            ARMARX_WARNING << "Failed to load articulated object `" << typeName << "/"
+                           << instanceName << "` from file `"
+                           << articulatedObjectDescription->description.xml.toSystemPath() << "`.";
+
             return nullptr;
         }
 
         obj->setName(instanceName);
-        obj->setType(it->name);
+        obj->setType(typeName);
 
         return obj;
     }

source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp

@@ -51,7 +51,8 @@ namespace armarx::armem::articulated_object
 
     bool
     ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
-                                                    const armem::Time& timestamp)
+                                                    const armem::Time& timestamp,
+                                                    const bool isStatic)
     {
 
         ARMARX_CHECK_NOT_NULL(articulatedObject);
@@ -59,6 +60,6 @@ namespace armarx::armem::articulated_object
         armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
             convert(*articulatedObject, Time::Now());
 
-        return store(armemArticulatedObject);
+        return store(armemArticulatedObject, isStatic);
     }
 } // namespace armarx::armem::articulated_object

source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h

@@ -14,8 +14,8 @@ namespace armarx::armem::articulated_object
     public:
         using Writer::Writer;
 
-        bool
-        storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
-                               const armem::Time& timestamp);
+        bool storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+                                    const armem::Time& timestamp,
+                                    bool isStatic = false);
     };
 } // namespace armarx::armem::articulated_object

source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp

@@ -171,6 +171,7 @@ namespace armarx::armem::articulated_object
 
         if (providerName.has_value()) // query single provider
         {
+            ARMARX_VERBOSE << "Single provider query";
             ARMARX_CHECK_NOT_EMPTY(providerName.value());
 
             // clang-format off
@@ -183,6 +184,8 @@ namespace armarx::armem::articulated_object
         }
         else // query all providers
         {
+            ARMARX_VERBOSE << "All provider query";
+
             // clang-format off
             qb
             .coreSegments().withName(objects::constants::CoreClassSegmentName)
@@ -192,7 +195,9 @@ namespace armarx::armem::articulated_object
             // clang-format on
         }
 
+        ARMARX_VERBOSE << "Before query";
         const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+        ARMARX_VERBOSE << "After query";
 
         ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
@@ -201,6 +206,8 @@ namespace armarx::armem::articulated_object
             return {};
         }
 
+        ARMARX_TRACE;
+        ARMARX_VERBOSE << "getRobotDescriptions";
         return getRobotDescriptions(qResult.memory);
     }
 
@@ -380,20 +387,18 @@ namespace armarx::armem::articulated_object
             memory.getCoreSegment(objects::constants::CoreClassSegmentName);
 
         std::vector<robot_state::description::RobotDescription> descriptions;
-        coreSegment.forEachEntity(
-            [&descriptions](const wm::Entity& entity)
+        coreSegment.forEachInstance(
+            [&descriptions](const wm::EntityInstance& instance)
             {
-                if (not entity.empty())
+                ARMARX_VERBOSE << "Converting ...";
+                if (const auto robotDescription = convertRobotDescription(instance))
                 {
-                    const auto robotDescription =
-                        convertRobotDescription(entity.getFirstSnapshot().getInstance(0));
-                    if (robotDescription)
-                    {
-                        descriptions.push_back(*robotDescription);
-                    }
+                    descriptions.push_back(*robotDescription);
                 }
             });
 
+        ARMARX_VERBOSE << descriptions.size() << " descriptions";
+
         return descriptions;
     }
 

source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp

@@ -15,11 +15,11 @@
 #include <RobotAPI/libraries/armem/core/operations.h>
 #include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
 #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
 #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
-#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
 
 #include "utils.h"
 
@@ -191,7 +191,7 @@ namespace armarx::armem::articulated_object
     }
 
     bool
-    Writer::storeInstance(const ArticulatedObject& obj) const
+    Writer::storeInstance(const ArticulatedObject& obj, const bool isStatic) const
     {
         std::lock_guard g{memoryWriterMutex};
 
@@ -218,7 +218,7 @@ namespace armarx::armem::articulated_object
         arondto::ObjectInstance objectInstance;
         toAron(objectInstance, obj.config);
 
-        const auto classId = storeOrGetClass(obj);
+        const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
 
         if (not classId)
         {
@@ -229,10 +229,17 @@ namespace armarx::armem::articulated_object
         // install memory link
         toAron(objectInstance.classID, *classId);
 
+        // set object instance id
+        const MemoryID memoryInstanceId = classId->withEntityName(entityName);
+
         armem::MemoryID id;
-        id.setEntityID(classId->getEntityID());
+        id.setEntityID(memoryInstanceId.getEntityID());
 
         armarx::ObjectID objectId(id.entityName);
+        ARMARX_INFO << "Object ID: " << objectId;
+
+        ARMARX_CHECK_NOT_EMPTY(objectId.instanceName())
+            << "An object instance name must be provided!";
 
         armarx::arondto::ObjectID cs;
         cs.className = objectId.className();
@@ -243,6 +250,8 @@ namespace armarx::armem::articulated_object
         objectInstance.pose.providerName = properties.providerName;
         objectInstance.pose.attachmentValid = false;
 
+        objectInstance.pose.isStatic = isStatic;
+
         update.instancesData = {objectInstance.toAron()};
         update.referencedTime = timestamp;
 
@@ -260,7 +269,7 @@ namespace armarx::armem::articulated_object
     }
 
     bool
-    Writer::store(const ArticulatedObject& obj) const
+    Writer::store(const ArticulatedObject& obj, const bool isStatic) const
     {
         const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
 
@@ -271,7 +280,7 @@ namespace armarx::armem::articulated_object
             return false;
         }
 
-        return storeInstance(obj);
+        return storeInstance(obj, isStatic);
     }
 
     // TODO this is a duplicate

source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h

@@ -45,9 +45,9 @@ namespace armarx::armem::articulated_object
         void connect(armem::client::MemoryNameSystem& memoryNameSystem);
 
 
-        bool store(const ArticulatedObject& obj) const override;
+        bool store(const ArticulatedObject& obj, bool isStatic) const override;
 
-        bool storeInstance(const ArticulatedObject& obj) const;
+        bool storeInstance(const ArticulatedObject& obj, bool isStatic) const;
         std::optional<armem::MemoryID> storeClass(const ArticulatedObject& obj) const;
 
         // const std::string& getPropertyPrefix() const override;

source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h

@@ -30,7 +30,7 @@ namespace armarx::armem::articulated_object
     public:
         virtual ~WriterInterface() = default;
 
-        virtual bool store(const ArticulatedObject& obj) const = 0;
+        virtual bool store(const ArticulatedObject& obj, bool isStatic) const = 0;
     };
 
 } // namespace armarx::armem::articulated_object

source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp

 #include "Segment.h"
 
-#include <filesystem>
 #include <sstream>
 
 #include <sys/inotify.h>

source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp

@@ -3,6 +3,7 @@
 #include <Eigen/Geometry>
 
 #include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/color/cmaps/colormaps.h>
 #include <SimoxUtility/math/pose.h>
 #include <SimoxUtility/math/rescale.h>
 
@@ -72,6 +73,9 @@ namespace armarx::armem::server::obj::familiar_object_instance
             auto layerPointCloud = arviz.layer("familiar_objects/points/" + providerName);
             auto layerBox = arviz.layer("familiar_objects/box/" + providerName);
 
+            auto confidenceCmap = simox::color::cmaps::viridis();
+            confidenceCmap.set_vlimits(0, 1);
+
             for (const auto& familiarObject : familiarObjects)
             {
                 if (not isWithinTimeFrame(familiarObject))
@@ -120,7 +124,11 @@ namespace armarx::armem::server::obj::familiar_object_instance
                         auto box = viz::Box(objectId.str());
                         box.pose(global_T_bb.matrix());
                         box.size(familiarObject.bounding_box.extents);
-                        box.color(simox::Color::gray(128, alpha));
+
+                        auto color = confidenceCmap.at(familiarObject.confidence);
+                        color.a = alpha;
+
+                        box.color(color);
                         layerBox.add(box);
                     }
                 }

source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp

@@ -62,7 +62,7 @@ namespace armarx::obstacle_avoidance
             if (auto obstacle = finder.loadManipulationObject(objectPose))
             {
                 obstacle->setGlobalPose(objectPose.objectPoseGlobal);
-                obstacle->setName(objectPose.objectID.instanceName());
+                obstacle->setName(objectPose.objectID.str());
                 sceneObjects->addSceneObject(obstacle);
             }
         }
@@ -104,7 +104,6 @@ namespace armarx::obstacle_avoidance
 
                     Eigen::Affine3f world_T_obj = world_T_map * map_T_obj;
 
-                    // ARMARX_INFO << world_T_obj.translation();
 
                     auto cube = factory.createBox(boxSize, boxSize, boxSize);
 
@@ -177,4 +176,4 @@ namespace armarx::obstacle_avoidance
         arviz.commitLayerContaining("CollisionModel", mesh);
     }
 
-} // namespace armarx::obstacle_avoidance
+} // namespace armarx::obstacle_avoidance
\ No newline at end of file

source/RobotAPI/libraries/skills/core/Skill.cpp

 #include "Skill.h"
 
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
+
 namespace armarx
 {
     namespace skills

source/RobotAPI/libraries/skills/core/Skill.h

@@ -2,7 +2,6 @@
 
 #include <functional>
 #include <mutex>
-#include <queue>
 #include <thread>
 
 #include <ArmarXCore/core/logging/Logging.h>
@@ -14,10 +13,8 @@
 
 #include "SkillDescription.h"
 #include "SkillID.h"
-#include "SkillPreparationInput.h"
 #include "SkillProxy.h"
 #include "SkillStatusUpdate.h"
-#include "error/Exception.h"
 
 namespace armarx
 {