drawing

source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.cpp

@@ -32,26 +32,35 @@
 #include <VirtualRobot/VirtualRobot.h>
 #include <VirtualRobot/XML/RobotIO.h>
 
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 #include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time.h"
 #include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/Metronome.h"
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
 #include "RobotAPI/components/ArViz/Client/Elements.h"
+#include "RobotAPI/components/ArViz/Client/Layer.h"
+#include "RobotAPI/components/ArViz/Client/elements/Robot.h"
+#include "RobotAPI/components/ArViz/Client/elements/RobotHand.h"
 #include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
 #include "RobotAPI/libraries/ArmarXObjects/forward_declarations.h"
 #include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h"
+#include "RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h"
 
+#include "armarx/manipulation/core/types.h"
 #include "armarx/manipulation/geometric_planning/ArVizDrawer.h"
 #include "armarx/manipulation/geometric_planning/util.h"
 
 
-
 namespace armarx::manipulation::components::articulated_object_augmented_visualization
 {
     Component::Component()
     {
         addPlugin(articulatedObjectReaderPlugin);
+        addPlugin(virtualRobotReaderPlugin);
     }
 
 
@@ -80,8 +89,42 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
     void
     Component::onConnectComponent()
     {
+        // load all robots
+        {
+            const auto descriptions =
+                virtualRobotReaderPlugin->get().queryDescriptions(Clock::Now());
+
+            ARMARX_INFO << "Found the following robot descriptions: ";
+            for (const auto& description : descriptions)
+            {
+                ARMARX_INFO << "Loading robot `" << description.name << "` from "
+                            << description.xml;
+                const auto robot = VirtualRobot::RobotIO::loadRobot(
+                    description.xml.toSystemPath(),
+                    VirtualRobot::RobotIO::RobotDescription::eStructure);
+                ARMARX_CHECK_NOT_NULL(robot);
+                robots.emplace(description.name, robot);
+
+                auto robotNameHelper = RobotNameHelper::Create(description.xml.toSystemPath());
+                ARMARX_CHECK(robotNameHelper);
+                robotNameHelpers.emplace(description.name, robotNameHelper);
+            }
+        }
+
         ARMARX_INFO << "Visualizing paths for nodes";
-        visualizeAllParametricPathsForNodes("handle");
+        Metronome metronome(Frequency::Hertz(2));
+        while (true)
+        {
+            try
+            {
+                visualizeAllParametricPathsForNodes("handle");
+            }
+            catch (...)
+            {
+                ARMARX_WARNING << GetHandledExceptionString();
+            }
+            metronome.waitForNextTick();
+        }
     }
 
 
@@ -251,42 +294,130 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
                 arviz.commit(l);
             }
 
-            if(properties.drawGrasps)
+            if (properties.drawGrasps)
             {
-                // refactor this to retrieve all grasps for specific EEF
-                for(const auto& node: articulatedObject->getRobotNodes())
+                auto layer = arviz.layer("grasps " + articulatedObject->getType() + "/" +
+                                         articulatedObject->getName());
+
+                for (const auto& node : articulatedObject->getRobotNodes())
                 {
                     const auto graspSets = node->getAllGraspSets();
-                    if(not graspSets.empty())
+                    if (not graspSets.empty())
                     {
                         ARMARX_IMPORTANT << "Found grasp sets attached to node " << node->getName();
-                        for(const auto& graspSet : graspSets)
+                        for (const auto& graspSet : graspSets)
                         {
-                            for(const auto& grasp: graspSet->getGrasps())
+                            for (const auto& grasp : graspSet->getGrasps())
                             {
+                                const std::string side = graspSet->getName();
+
+                                ARMARX_TRACE;
                                 ARMARX_INFO << "- " << grasp->getName();
-                                drawGrasp(grasp);
+                                drawGrasp(side, grasp, core::Pose(node->getGlobalPose()), layer);
                             }
                         }
                     }
                 }
+
+                ARMARX_TRACE;
+                ARMARX_INFO << VAROUT(layer.size());
+                arviz.commit(layer);
             }
         }
     }
-    
-    void Component::drawGrasp(const VirtualRobot::GraspPtr& grasp) 
+
+    void
+    Component::drawGrasp(const std::string& side,
+                         const VirtualRobot::GraspPtr& grasp,
+                         const core::Pose& global_T_node,
+                         viz::Layer& layer) const
     {
-        // load robot
-        if(grasp->getRobotType() == "Armar6 RightHand")
+        ARMARX_CHECK_NOT_NULL(grasp);
+
+        if (robots.count(grasp->getRobotType()) == 0)
         {
-            grasp->getTcpPoseGlobal(objectPose)
+            ARMARX_WARNING << "Cannot draw grasp. Unknown robot `" << grasp->getRobotType() << "`";
+            return;
         }
 
+        ARMARX_TRACE;
+        const auto& robotNameHelper = robotNameHelpers.at(grasp->getRobotType());
+
+        ARMARX_TRACE;
+        const auto& robot = robots.at(grasp->getRobotType());
+
+        const auto global_T_tcp = grasp->getTcpPoseGlobal(global_T_node.matrix());
+
+        ARMARX_TRACE;
+
+        struct Foo
+        {
+            Eigen::Isometry3f tcp_T_hand_root;
+            std::map<std::string, float> jointValues;
+
+        };
+
+        // FIXME for both left and right!
+        const Foo foo = [&]
+        (){
+            ARMARX_TRACE;
+            const armarx::RobotNameHelper::Arm arm = robotNameHelper->getArm(side);
+
+            ARMARX_TRACE;
+            const armarx::RobotNameHelper::RobotArm robotArm = arm.addRobot(robot);
+
+            const std::string handRootNodeName = "Hand " + side.substr(0, 1) + " Root";
+
+            const auto eefName = arm.getEndEffector();
+
+            const auto eef = robot->getEndEffector(eefName);
+
+            ARMARX_INFO << "preshape: " << grasp->getPreshapeName();
+
+            const auto preshape = eef->getPreshape(grasp->getPreshapeName());
+
+            Foo foo;
+
+            if (preshape != nullptr)
+            {
+                foo.jointValues = preshape->getRobotNodeJointValueMap();
+                ARMARX_INFO << "preshape with joint values " << foo.jointValues;
+            }
+
+            ARMARX_TRACE;
+            const Eigen::Isometry3f tcp_T_hand_root(
+                robotArm.getTCP()->getPoseInRootFrame().inverse() *
+                robot->getRobotNode(handRootNodeName)->getPoseInRootFrame());
+            foo.tcp_T_hand_root = tcp_T_hand_root;
+
+            return foo;
+        }();
+
+        ARMARX_TRACE;
+        // FIXME this does not work. weak_ptr exception
+        // const auto vizElement = viz::RobotHand(grasp->getName())
+        //                             .fileBySide("Right", robotNameHelper)
+        //                             .tcpPose(global_T_tcp, robot);
+
+        std::string modelFile = "armar6_rt/robotmodel/Armar6-SH/Armar6-" + side + "Hand-v3.xml";
+
+
+        ARMARX_TRACE;
+        const auto vizElement =
+            viz::Robot(side + "/" + grasp->getName())
+                .file("armar6_rt", modelFile)
+                .pose(Eigen::Matrix4f{(global_T_tcp * foo.tcp_T_hand_root).matrix()})
+                .joints(foo.jointValues)
+                .overrideColor(simox::Color::azure());
+
+        ARMARX_TRACE;
+        layer.add(vizElement);
+
+
         // if(grasp->getRobotType() == "Armar6 LeftHand")
         // {
 
         // }
-
     }
 
 

source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.h

@@ -26,15 +26,20 @@
 
 // #include <mutex>
 
+#include <VirtualRobot/Robot.h>
 #include <VirtualRobot/VirtualRobot.h>
 
 #include <ArmarXCore/core/Component.h>
 
 #include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h"
+#include "RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h"
 #include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 #include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h>
 
+#include <armarx/manipulation/core/types.h>
+
 
 namespace armarx::manipulation::components::articulated_object_augmented_visualization
 {
@@ -77,7 +82,10 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
         void visualizeAllParametricPathsForNodes(const std::string& identifier);
 
 
-        void drawGrasp(const VirtualRobot::GraspPtr& grasp);
+        void drawGrasp(const std::string& side,
+                       const VirtualRobot::GraspPtr& grasp,
+                       const armarx::manipulation::core::Pose& global_T_node,
+                       viz::Layer& layer) const;
 
 
     private:
@@ -92,11 +100,16 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
             bool drawGrasps = true;
         };
         Properties properties;
-       
+
+        std::map<std::string, VirtualRobot::RobotPtr> robots;
+        std::map<std::string, RobotNameHelperPtr> robotNameHelpers;
 
         armem::client::plugins::ReaderWriterPlugin<
             ::armarx::armem::articulated_object::ArticulatedObjectReader>*
             articulatedObjectReaderPlugin = nullptr;
+
+        armem::client::plugins::ReaderWriterPlugin<
+            ::armarx::armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin = nullptr;
     };
 
 } // namespace armarx::manipulation::components::articulated_object_augmented_visualization