diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp index d491662edda9ff00244178735c0c84c26a923688..2fcfbb4ae129edf59214cc8a398d535d95a2e86e 100644 --- a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp +++ b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp @@ -40,6 +40,7 @@ void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot::Object& j["collection"] = rhs.collection; j["position"] = rhs.position; j["orientation"] = rhs.orientation; + j["jointValues"] = rhs.jointValues; } @@ -50,6 +51,10 @@ void armarx::armem::obj::from_json(const nlohmann::json& j, SceneSnapshot::Objec 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..30e144b6300327b969904a662fd1bd3466dc467b 100644 --- a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h +++ b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h @@ -44,6 +44,8 @@ namespace armarx::armem::obj 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; }; diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp index 13c76d0e2700e73bdf788b39eb10696098801836..5c76dfb7289ba6f503dfd17eaa7bda26af0e9e2c 100644 --- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp +++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp @@ -870,6 +870,8 @@ namespace armarx::armem::server::obj::instance 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); };