Add visualization helper component plugin

source/armarx/manipulation/njoint_arm/CMakeLists.txt

 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

source/armarx/manipulation/util/CMakeLists.txt

+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
 )

source/armarx/manipulation/util/VisualizationHelper.cpp

+/*
+ * 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

source/armarx/manipulation/util/VisualizationHelper.h

+/*
+ * 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

source/armarx/manipulation/util/aron/VisualizationConfig.xml

+<?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>