diff --git a/source/armarx/manipulation/njoint_arm/CMakeLists.txt b/source/armarx/manipulation/njoint_arm/CMakeLists.txt
index c8881e71ab478bf5c39e4e311c10ad824f8a88ef..5ae79932d612d0a72bae55b93f92703aabcc6106 100644
--- a/source/armarx/manipulation/njoint_arm/CMakeLists.txt
+++ b/source/armarx/manipulation/njoint_arm/CMakeLists.txt
@@ -1,10 +1,10 @@
armarx_add_library(njoint_arm
- DEPENDENCIES
+ DEPENDENCIES
ArmarXCoreInterfaces ArmarXCore
RobotAPIInterfaces RobotAPICore RobotAPIComponentPlugins
armarx_control::deprecated_njoint_mp_controller
armarx_manipulation::core armarx_manipulation::util
- SOURCES
+ SOURCES
NJointArmArbiter.cpp
MovementInstructions.cpp
HandControllerPolicyBase.cpp
@@ -13,7 +13,7 @@ armarx_add_library(njoint_arm
impl/NJointTaskSpaceImpedanceControl.cpp
impl/CartesianNaturalPositionController.cpp
impl/NJointTaskSpaceImpedanceDMPController.cpp
- HEADERS
+ HEADERS
NJointArmArbiter.h
NJointArm.h
ControllerVariants.h
diff --git a/source/armarx/manipulation/util/CMakeLists.txt b/source/armarx/manipulation/util/CMakeLists.txt
index 026a61d7845a9aecc003f8a23fbb1c1505c14c6b..ef3747ad44c0dc4a34dd6a88b52acfa10d52b162 100644
--- a/source/armarx/manipulation/util/CMakeLists.txt
+++ b/source/armarx/manipulation/util/CMakeLists.txt
@@ -1,14 +1,24 @@
+armarx_add_aron_library(util_aron
+ ARON_FILES
+ aron/VisualizationConfig.xml
+ DEPENDENCIES
+ armarx_manipulation::core_aron
+ )
+
armarx_add_library(util
DEPENDENCIES
- ArmarXCore RobotAPICore GraspingUtility
+ ArmarXCore RobotAPICore GraspingUtility armarx_manipulation::core armarx_manipulation::util_aron
+ aron_component_config
DEPENDENCIES_LEGACY
manif
SOURCES
BimanualTransformationHelper.cpp
LieHelpers.cpp
Utilities.cpp
+ VisualizationHelper.cpp
HEADERS
BimanualTransformationHelper.h
LieHelpers.h
Utilities.h
+ VisualizationHelper.h
)
diff --git a/source/armarx/manipulation/util/VisualizationHelper.cpp b/source/armarx/manipulation/util/VisualizationHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..96cce8a634ebea8de840c3da33c892507be9cd71
--- /dev/null
+++ b/source/armarx/manipulation/util/VisualizationHelper.cpp
@@ -0,0 +1,187 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package armarx_manipulation
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 27.03.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "VisualizationHelper.h"
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
+
+#include <RobotAPI/interface/core/NameValueMap.h>
+
+#include <armarx/manipulation/core/ActionHypothesis.h>
+#include <armarx/manipulation/core/ExecutableAction.h>
+
+namespace armarx::manipulation::util
+{
+ void
+ VisualizationHelper::visualizePlatformPlacement(const armarx::FramedPose& platformPose,
+ const VirtualRobot::RobotPtr& robot)
+ {
+ const auto& config = configPlugin_->config_.getUpToDateReadBuffer();
+ auto platform = viz::Robot("Platform")
+ .file(config.robot_project_dir, config.robot_path)
+ .pose(platformPose.toGlobalEigen(robot))
+ .overrideColor(simox::Color::green());
+ arviz_.commitLayerContaining("Robot", platform);
+ }
+
+ void
+ VisualizationHelper::visualizeHypothesis(
+ const armarx::manipulation::core::executable_action::Unimanual& unimanual,
+ const VirtualRobot::RobotPtr& robot,
+ bool isReachable,
+ std::optional<simox::Color> color,
+ const ConfigT& config)
+ {
+ auto layer = arviz_.layer("Hypotheses");
+ armarx::NameValueMap joints;
+ std::string path;
+ if (unimanual.handedness.value() == armarx::manipulation::core::arondto::Handedness::Left)
+ {
+ joints = config.left_hand_opened_joints;
+ path = config.left_hand_path;
+ }
+ else if (unimanual.handedness.value() ==
+ armarx::manipulation::core::arondto::Handedness::Right)
+ {
+ joints = config.right_hand_opened_joints;
+ path = config.right_hand_path;
+ }
+ else
+ {
+ return;
+ }
+ simox::Color default_color = isReachable ? simox::Color::green() : simox::Color::red();
+ if (color)
+ {
+ default_color = *color;
+ }
+ const Eigen::Matrix4f rootToGlobalTcp =
+ robot->getRobotNode(unimanual.getHandRootNode().value())
+ ->toLocalCoordinateSystem(
+ robot->getRobotNode(unimanual.getTCPName().value())->getGlobalPose());
+ layer.add(armarx::viz::Robot(std::to_string(rand()) + "exec_pose")
+ .file("", path)
+ .joints(joints)
+ .pose(unimanual.execPose->toGlobalEigen(robot) * rootToGlobalTcp.inverse())
+ .overrideColor(default_color));
+ if (config.visualize_preposes)
+ {
+ layer.add(armarx::viz::Robot(std::to_string(rand()) + "pre_pose")
+ .file("", path)
+ .joints(joints)
+ .pose(unimanual.prePose->toGlobalEigen(robot) * rootToGlobalTcp.inverse())
+ .overrideColor(default_color.with_alpha(0.2)));
+ }
+ // viz_robots.push_back(visu->prePose());
+ arviz_.commit(layer);
+ }
+
+ void
+ VisualizationHelper::visualizeRobot(const VirtualRobot::RobotPtr& robot)
+ {
+ const auto& config = configPlugin_->config_.getUpToDateReadBuffer();
+ auto vis_robot = armarx::viz::Robot("Robot")
+ .file(config.robot_project_dir, config.robot_path)
+ .joints(robot->getConfig()->getRobotNodeJointValueMap())
+ .pose(robot->getGlobalPose())
+ .overrideColor(simox::Color::blue());
+ arviz_.commitLayerContaining("Robot", vis_robot);
+ }
+
+ void
+ VisualizationHelper::visualizeTrajectory(const std::vector<armarx::FramedPose>& trajectory,
+ const VirtualRobot::RobotPtr& robot,
+ const std::string& tcpName)
+ {
+ Eigen::Vector3f last_position = robot->getRobotNode(tcpName)->getGlobalPosition();
+ auto layer = arviz_.layer("Trajectory");
+ for (const auto& i : trajectory)
+ {
+ const std::string id = std::to_string(std::rand());
+ Eigen::Matrix4f current_pose = i.toGlobalEigen(robot);
+ layer.add(armarx::viz::Pose("pose_" + id).pose(current_pose));
+ layer.add(armarx::viz::Arrow("link_" + id)
+ .fromTo(last_position, current_pose.block<3, 1>(0, 3))
+ .color(simox::Color::green())
+ .width(3.0));
+ last_position = current_pose.block<3, 1>(0, 3);
+ }
+ arviz_.commit(layer);
+ }
+
+ void
+ VisualizationHelper::clearVisualizationLayers()
+ {
+ arviz_.commitDeleteLayer("MovementInstructions");
+ arviz_.commitDeleteLayer("Robot");
+ arviz_.commitDeleteLayer("Hypotheses");
+ arviz_.commitDeleteLayer("Trajectory");
+ }
+
+ VisualizationHelper::VisualizationHelper(armarx::ManagedIceObject& parent,
+ const std::string& prefix) :
+ ComponentPlugin(parent, prefix)
+ {
+ addPluginAndDependency(arvizPlugin_);
+ addPluginAndDependency(configPlugin_);
+ }
+
+ void
+ VisualizationHelper::postOnConnectComponent()
+ {
+ arviz_ = arvizPlugin_->createClient();
+ }
+
+ armarx::viz::Client&
+ VisualizationHelper::getArvizClient()
+ {
+ return arviz_;
+ }
+
+ void
+ VisualizationHelper::visualizeAction(const armarx::manipulation::core::ExecutableAction& action,
+ const VirtualRobot::RobotPtr& robot,
+ std::optional<simox::Color> color,
+ const VisualizationHelper::ConfigT& config)
+ {
+ for (const auto& um : action.unimanual)
+ {
+ if (um.hypothesis.has_value())
+ {
+ visualizeHypothesis(
+ um, robot, not action.planningInformation.isDiscarded, color, config);
+ }
+ }
+ }
+
+ template <typename PluginT, typename... ParamsT>
+ void
+ VisualizationHelper::addPluginAndDependency(std::experimental::observer_ptr<PluginT>& plugin,
+ ParamsT&&... params)
+ {
+ PluginT* tmp{nullptr};
+ addPlugin(tmp, std::forward<ParamsT>(params)...);
+ addPluginDependency(tmp);
+ plugin.reset(tmp);
+ }
+} // namespace armarx::manipulation::util
\ No newline at end of file
diff --git a/source/armarx/manipulation/util/VisualizationHelper.h b/source/armarx/manipulation/util/VisualizationHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..255661921fcddee6c56f2186854a2998590c1cf4
--- /dev/null
+++ b/source/armarx/manipulation/util/VisualizationHelper.h
@@ -0,0 +1,104 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package armarx_manipulation
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 27.03.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <experimental/memory>
+
+#include <SimoxUtility/color/Color.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/ComponentPlugin.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/aron_component_config/ComponentPlugin.h>
+
+#include <armarx/manipulation/core/forward_declarations.h>
+#include <armarx/manipulation/util/aron/VisualizationConfig.aron.generated.h>
+
+namespace armarx::manipulation::util
+{
+ class VisualizationHelper : public armarx::ComponentPlugin
+ {
+ template <typename T>
+ using observer_ptr = std::experimental::observer_ptr<T>;
+ using ConfigT = armarx::manipulation::util::arondto::Visualization;
+
+ public:
+ VisualizationHelper(armarx::ManagedIceObject& parent, const std::string& prefix);
+
+ void postOnConnectComponent() override;
+
+ /**
+ * /brief Visualizes a single Platform Placement with committing the ArViz Layer
+ * @param platformPose Framed Pose of the Platform root
+ */
+ void visualizePlatformPlacement(const armarx::FramedPose& platformPose,
+ const VirtualRobot::RobotPtr& robot);
+
+ /**
+ * /brief Visualizes a single affordance (after checking that an AffordanceVisualization exists) without commiting the ArViz Layer
+ * @param affordance
+ */
+ void visualizeHypothesis(
+ const armarx::manipulation::core::executable_action::Unimanual& unimanual,
+ const VirtualRobot::RobotPtr& robot,
+ bool isReachable,
+ std::optional<simox::Color> color,
+ const ConfigT& config);
+ void visualizeAction(const armarx::manipulation::core::ExecutableAction& action,
+ const VirtualRobot::RobotPtr& robot,
+ std::optional<simox::Color> color,
+ const ConfigT& config);
+
+ /**
+ * \brief Visualizes the local robot clone (with commiting the Layer)
+ */
+ void visualizeRobot(const VirtualRobot::RobotPtr& robot);
+
+ /**!
+ * \brief visualize the trajectoy without synching the robot first
+ *
+ * @param trajectory
+ * @param robot
+ */
+ void visualizeTrajectory(const std::vector<armarx::FramedPose>& trajectory,
+ const VirtualRobot::RobotPtr& robot,
+ const std::string& tcpName);
+
+
+ void clearVisualizationLayers();
+
+
+ armarx::viz::Client& getArvizClient();
+
+ private:
+ template <typename PluginT, typename... ParamsT>
+ void addPluginAndDependency(observer_ptr<PluginT>& plugin, ParamsT&&... params);
+
+ observer_ptr<armarx::plugins::ArVizComponentPlugin> arvizPlugin_;
+ observer_ptr<armarx::plugins::AronComponentConfigPlugin<ConfigT>> configPlugin_;
+ armarx::viz::Client arviz_;
+ };
+
+} // namespace armarx::manipulation::util
diff --git a/source/armarx/manipulation/util/aron/VisualizationConfig.xml b/source/armarx/manipulation/util/aron/VisualizationConfig.xml
new file mode 100644
index 0000000000000000000000000000000000000000..6fc1f1b3dedd0f49d1079fe1f76ee086d113d153
--- /dev/null
+++ b/source/armarx/manipulation/util/aron/VisualizationConfig.xml
@@ -0,0 +1,54 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+ <Object name='armarx::manipulation::util::arondto::Visualization'>
+ <ObjectChild key='robot_project_dir'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='robot_path'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='right_hand_path'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='right_hand_root_node_name'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='left_hand_path'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='left_hand_root_node_name'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='head_path'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='head_root_node_name'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='right_hand_closed_joints'>
+ <Dict>
+ <float/>
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='left_hand_closed_joints'>
+ <Dict>
+ <float/>
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='right_hand_opened_joints'>
+ <Dict>
+ <float/>
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='left_hand_opened_joints'>
+ <Dict>
+ <float/>
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='visualize_preposes'>
+ <bool/>
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>