From 98deb92f736552b88262c9b93d7b9f154d050a48 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 29 Jun 2021 18:02:45 +0200
Subject: [PATCH] Extend snapshot loading and visualization to handle
articulated objects
---
.../libraries/armem_objects/SceneSnapshot.cpp | 5 ++
.../libraries/armem_objects/SceneSnapshot.h | 2 +
.../armem_objects/server/instance/Segment.cpp | 2 +
.../armem_objects/server/instance/Visu.cpp | 55 ++++++++++++++-----
.../armem_objects/server/instance/Visu.h | 6 ++
5 files changed, 55 insertions(+), 15 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
index d491662ed..2fcfbb4ae 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 add5f1f18..30e144b63 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 13c76d0e2..5c76dfb72 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 6f337c824..a0c8812a6 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 19dbeccd5..01e7f0598 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);
};
--
GitLab