Merge branch 'armem/articulated-instances' into 'armem/dev'

Object memory: articulated objects no longer as separate class and instance segments

See merge request ArmarX/RobotAPI!175

source/RobotAPI/interface/objectpose/object_pose_types.ice

@@ -29,6 +29,10 @@
 
 module armarx
 {
+    // Originally defined in <RobotAPI/interface/units/KinematicUnitInterface.ice>
+    dictionary<string, float> NameValueMap;
+
+
     // A struct's name cannot cannot differ only in capitalization from its immediately enclosing module name.
     module objpose
     {
@@ -74,6 +78,9 @@ module armarx
                 PoseBase objectPose;
                 string objectPoseFrame;
 
+                /// The object's joint values if it is articulated.
+                NameValueMap objectJointValues;
+
                 /// Confidence in [0, 1] (1 = full, 0 = none).
                 float confidence = 0;
                 /// Source timestamp.
@@ -105,6 +112,10 @@ module armarx
                 PoseBase objectPoseOriginal;
                 string objectPoseOriginalFrame;
 
+                /// The object's joint values if it is articulated.
+                NameValueMap objectJointValues;
+
+
                 StringFloatDictionary robotConfig;
                 PoseBase robotPose;
 

source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp

@@ -37,6 +37,8 @@ namespace armarx::objpose
         objectPoseOriginal = ::armarx::fromIce(ice.objectPoseOriginal);
         objectPoseOriginalFrame = ice.objectPoseOriginalFrame;
 
+        objectJointValues = ice.objectJointValues;
+
         robotConfig = ice.robotConfig;
         robotPose = ::armarx::fromIce(ice.robotPose);
 
@@ -66,6 +68,7 @@ namespace armarx::objpose
         ice.objectPoseGlobal = new Pose(objectPoseGlobal);
         ice.objectPoseOriginal = new Pose(objectPoseOriginal);
         ice.objectPoseOriginalFrame = objectPoseOriginalFrame;
+        ice.objectJointValues = objectJointValues;
 
         ice.robotConfig = robotConfig;
         ice.robotPose = new Pose(robotPose);
@@ -87,6 +90,7 @@ namespace armarx::objpose
 
         objectPoseOriginal = ::armarx::fromIce(provided.objectPose);
         objectPoseOriginalFrame = provided.objectPoseFrame;
+        objectJointValues = provided.objectJointValues;
 
         armarx::FramedPose framed(objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
         framed.changeFrame(robot, robot->getRootNode()->getName());

source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h

@@ -61,6 +61,9 @@ namespace armarx::objpose
         Eigen::Matrix4f objectPoseOriginal;
         std::string objectPoseOriginalFrame;
 
+        /// The object's joint values if it is articulated.
+        std::map<std::string, float> objectJointValues;
+
         std::map<std::string, float> robotConfig;
         Eigen::Matrix4f robotPose;
 

source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml

@@ -62,6 +62,13 @@ ARON DTO of armarx::objpose::ObjectPose.
                 <string />
             </ObjectChild>
 
+            <!-- The object's joint values if it is articulated. -->
+            <ObjectChild key='objectJointValues'>
+                <Dict>
+                    <Float />
+                </Dict>
+            </ObjectChild>
+
             <ObjectChild key='robotConfig'>
                <Dict>
                     <Float />

source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp

@@ -69,6 +69,7 @@ void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
     bo.objectPoseGlobal = dto.objectPoseGlobal;
     bo.objectPoseOriginal = dto.objectPoseOriginal;
     bo.objectPoseOriginalFrame = dto.objectPoseOriginalFrame;
+    bo.objectJointValues = dto.objectJointValues;
 
     bo.robotConfig = dto.robotConfig;
     bo.robotPose = dto.robotPose;
@@ -110,6 +111,7 @@ void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
     dto.objectPoseGlobal = bo.objectPoseGlobal;
     dto.objectPoseOriginal = bo.objectPoseOriginal;
     dto.objectPoseOriginalFrame = bo.objectPoseOriginalFrame;
+    dto.objectJointValues = bo.objectJointValues;
 
     dto.robotConfig = bo.robotConfig;
     dto.robotPose = bo.robotPose;

source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp

@@ -30,6 +30,18 @@ namespace armarx::armem::obj
         return id;
     }
 
+
+    ObjectID SceneSnapshot::Object::getObjectID() const
+    {
+        return getClassID().withInstanceName(instanceName);
+    }
+
+
+    ObjectID SceneSnapshot::Object::getObjectID(ObjectFinder& finder) const
+    {
+        return getClassID(finder).withInstanceName(instanceName);
+    }
+
 }
 
 
@@ -37,9 +49,11 @@ void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot::Object&
 {
     //     j["instanceID"] = rhs.instanceID;
     j["class"] = rhs.className;
+    j["instanceName"] = rhs.instanceName;
     j["collection"] = rhs.collection;
     j["position"] = rhs.position;
     j["orientation"] = rhs.orientation;
+    j["jointValues"] = rhs.jointValues;
 }
 
 
@@ -47,9 +61,17 @@ void armarx::armem::obj::from_json(const nlohmann::json& j, SceneSnapshot::Objec
 {
     //     j.at("instanceID").get_to(rhs.instanceID);
     j.at("class").get_to(rhs.className);
+    if (j.count("instanceName"))
+    {
+        j["instanceName"].get_to(rhs.instanceName);
+    }
     j.at("collection").get_to(rhs.collection);
     j.at("position").get_to(rhs.position);
     j.at("orientation").get_to(rhs.orientation);
+    if (j.count("jointValues"))
+    {
+        j.at("jointValues").get_to(rhs.jointValues);
+    }
 }
 
 

source/RobotAPI/libraries/armem_objects/SceneSnapshot.h

@@ -39,13 +39,18 @@ namespace armarx::armem::obj
         struct Object
         {
             std::string className;
+            std::string instanceName;
             std::string collection;
 
             Eigen::Vector3f position = Eigen::Vector3f::Zero();
             Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
 
+            std::map<std::string, float> jointValues;
+
             ObjectID getClassID() const;
             ObjectID getClassID(ObjectFinder& finder) const;
+            ObjectID getObjectID() const;
+            ObjectID getObjectID(ObjectFinder& finder) const;
         };
         std::vector<Object> objects;
     };

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

@@ -89,7 +89,7 @@ namespace armarx::armem::server::obj::articulated_object_class
             for (const armem::articulated_object::ArticulatedObjectDescription& desc : descriptions)
             {
                 EntityUpdate& update = commit.updates.emplace_back();
-                update.entityID = providerID.withEntityName(datasetName + "/" + desc.name);
+                update.entityID = providerID.withEntityName(desc.name);
                 update.timeArrived = update.timeCreated = update.timeSent = now;
 
                 arondto::RobotDescription aronRobotDescription;

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

@@ -863,13 +863,19 @@ namespace armarx::armem::server::obj::instance
             pose.providerName = sceneName;
             pose.objectType = objpose::ObjectTypeEnum::KnownObject;
             pose.isStatic = true;  // Objects loaded from prior knowledge are considerd static to exclude them from decay.
-            pose.objectID = classID.withInstanceName(std::to_string(idCounters[classID]++));
+            pose.objectID = classID.withInstanceName(
+                                object.instanceName.empty()
+                                ? std::to_string(idCounters[classID]++)
+                                : object.instanceName
+                            );
 
             pose.objectPoseGlobal = simox::math::pose(object.position, object.orientation);
             pose.objectPoseRobot = pose.objectPoseGlobal;
             pose.objectPoseOriginal = pose.objectPoseGlobal;
             pose.objectPoseOriginalFrame = armarx::GlobalFrame;
 
+            pose.objectJointValues = object.jointValues;
+
             pose.robotConfig = {};
             pose.robotPose = Eigen::Matrix4f::Identity();
 

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

@@ -28,6 +28,8 @@ namespace armarx::armem::server::obj::instance
                        "Enable showing object frames.");
         defs->optional(objectFramesScale, prefix + "objectFramesScale",
                        "Scaling of object frames.");
+        defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
+                       "Prefer articulated object models if available.");
     }
 
 
@@ -111,25 +113,44 @@ namespace armarx::armem::server::obj::instance
 
         const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
         {
-            viz::Object object(key);
-            object.pose(pose);
-            if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
-            {
-                object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
-            }
-            else
-            {
-                object.fileByObjectFinder(id);
-            }
-            if (alphaByConfidence && objectPose.confidence < 1.0f)
+            std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
+
+            bool done = false;
+            if (useArticulatedModels && objectInfo)
             {
-                object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
+                if (std::optional<PackageFileLocation> model = objectInfo->getArticulatedModel())
+                {
+                    viz::Robot robot(key);
+                    robot.pose(pose);
+                    robot.file(model->package, model->relativePath);
+                    robot.joints(objectPose.objectJointValues);
+
+                    layer.add(robot);
+                    done = true;
+                }
             }
-            else if (alpha < 1)
+            if (!done)
             {
-                object.overrideColor(simox::Color::white().with_alpha(alpha));
+                viz::Object object(key);
+                object.pose(pose);
+                if (objectInfo)
+                {
+                    object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
+                }
+                else
+                {
+                    object.fileByObjectFinder(id);
+                }
+                if (alphaByConfidence && objectPose.confidence < 1.0f)
+                {
+                    object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
+                }
+                else if (alpha < 1)
+                {
+                    object.overrideColor(simox::Color::white().with_alpha(alpha));
+                }
+                layer.add(object);
             }
-            layer.add(object);
         }
 
         if (oobbs && objectPose.localOOBB)
@@ -164,6 +185,7 @@ namespace armarx::armem::server::obj::instance
             objectFramesScale.setSteps(int(10 * max));
             objectFramesScale.setValue(visu.objectFramesScale);
         }
+        useArticulatedModels.setValue(visu.useArticulatedModels);
 
         GridLayout grid;
         int row = 0;
@@ -180,6 +202,8 @@ namespace armarx::armem::server::obj::instance
         grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
         grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
         row++;
+        grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
+        row++;
 
         group.setLabel("Visualization");
         group.addChild(grid);
@@ -194,6 +218,7 @@ namespace armarx::armem::server::obj::instance
         visu.oobbs = oobbs.getValue();
         visu.objectFrames = objectFrames.getValue();
         visu.objectFramesScale = objectFramesScale.getValue();
+        visu.useArticulatedModels = useArticulatedModels.getValue();
     }
 
 }

source/RobotAPI/libraries/armem_objects/server/instance/Visu.h

@@ -76,6 +76,9 @@ namespace armarx::armem::server::obj::instance
         bool objectFrames = false;
         float objectFramesScale = 1.0;
 
+        /// Prefer articulated models if available.
+        bool useArticulatedModels = true;
+
         SimpleRunningTask<>::pointer_type updateTask;
 
 
@@ -92,6 +95,9 @@ namespace armarx::armem::server::obj::instance
             armarx::RemoteGui::Client::CheckBox objectFrames;
             armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
 
+            armarx::RemoteGui::Client::CheckBox useArticulatedModels;
+
+
             void setup(const Visu& visu);
             void update(Visu& visu);
         };

source/RobotAPI/libraries/armem_robot/aron_conversions.cpp

 #include "aron_conversions.h"
 
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+
 #include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
 
+
 namespace armarx::armem::robot
 {
 
@@ -59,3 +65,47 @@ namespace armarx::armem::robot
     }
 
 }  // namespace armarx::armem::robot
+
+
+namespace armarx::armem
+{
+
+    void robot::fromAron(const arondto::ObjectClass& dto, RobotDescription& bo)
+    {
+        bo.name = aron::fromAron<armarx::ObjectID>(dto.id).str();
+        fromAron(dto.articulatedSimoxXmlPath, bo.xml);
+    }
+
+    void robot::toAron(arondto::ObjectClass& dto, const RobotDescription& bo)
+    {
+        toAron(dto.id, ObjectID(bo.name));
+        toAron(dto.articulatedSimoxXmlPath, bo.xml);
+    }
+
+
+    void robot::fromAron(const arondto::ObjectInstance& dto, RobotState& bo)
+    {
+        fromAron(dto.pose, bo);
+    }
+
+    void robot::toAron(arondto::ObjectInstance& dto, const RobotState& bo)
+    {
+        toAron(dto.pose, bo);
+    }
+
+
+    void robot::fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo)
+    {
+        bo.timestamp = dto.timestamp;
+        bo.globalPose = dto.objectPoseGlobal;
+        bo.jointMap = dto.objectJointValues;
+    }
+
+    void robot::toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo)
+    {
+        dto.timestamp = bo.timestamp;
+        dto.objectPoseGlobal = bo.globalPose.matrix();
+        dto.objectJointValues = bo.jointMap;
+    }
+
+}  // namespace armarx::armem

source/RobotAPI/libraries/armem_robot/aron_conversions.h

 #pragma once
 
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
 
+#include <RobotAPI/libraries/armem_robot/types.h>
 #include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
 
+
 namespace armarx::armem::robot
 {
     // TODO move the following
@@ -22,4 +26,14 @@ namespace armarx::armem::robot
     void fromAron(const arondto::RobotState& dto, RobotState& bo);
     void toAron(arondto::RobotState& dto, const RobotState& bo);
 
+
+    void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
+    void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
+
+    void fromAron(const arondto::ObjectInstance& dto, RobotState& bo);
+    void toAron(arondto::ObjectInstance& dto, const RobotState& bo);
+
+    void fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo);
+    void toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo);
+
 }  // namespace armarx::armem::robot