diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice index eb23d767e8c64892e9000eec890ef545ac8127d2..0dc650c9b7bb6bf9b139714015ea55120a409f0d 100644 --- a/source/RobotAPI/interface/objectpose/object_pose_types.ice +++ b/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; diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp index b37f432ef60adbca9307f61598bdfc5d05e18eea..51fb57e6896277be3538672fce00c1096bc60951 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp +++ b/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()); diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h index 754c1a1b178c9b88a600b466cb73c5c10a617c5a..ce653e0923ae8fc3bb1f1283f9e905624ce61171 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h +++ b/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; diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml index cc1a3a0467dba42f8bb87a89069b72d8e85e1189..ba26c1a618746e41823850dc99a93b670bc2a7e9 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml +++ b/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 /> diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp index 26f07a947f311676af0a746156b2ac0fb255b417..a639223a050b5473c69302816ee1a46adceb505a 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp +++ b/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; diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp index d491662edda9ff00244178735c0c84c26a923688..d3ece87d9746198af2f5b9acc5b81cce38d189aa 100644 --- a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp +++ b/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); + } } diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h index add5f1f183d15b6ec7c0588a5d4a9abc7e558d82..3e24156490544ebc537372a0d204c052ae8b2428 100644 --- a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h +++ b/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; }; diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp index 36ff86743abbbae569d43b44aff1e92a25896b19..425b5af02da13e58f4a654a0784029f5fbe9a9e0 100644 --- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp +++ b/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; diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp index 13c76d0e2700e73bdf788b39eb10696098801836..31f25a68ad359505e52ea3928c16bde8fb10385b 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp +++ b/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(); diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp index 6f337c8240be05af068c69b50c6703cbd63da576..a0c8812a6751f0612ffb254a2cb43bdd714e812d 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp +++ b/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(); } } diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h index 19dbeccd5addc942e4c417aa6505210ac6d82e92..01e7f059817d4050fc9728a4b09d95d22c9a572f 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h +++ b/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); }; diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp index 601f17d656dad19c150d7f36487e45900b102a57..da72cc16f995f239c207dfb38dcaa6cf533fb74b 100644 --- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp +++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp @@ -1,9 +1,15 @@ #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 diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h index 70e06ce8eedb7de9538f5249b3eedf797aec6060..9c124d758a9189b1ec709cca9808b430bbc63a94 100644 --- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h +++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h @@ -1,11 +1,15 @@ #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