Merge branch 'feature/motion-planning' into 'main'

Riemannian motion planning

See merge request !14

source/armarx/manipulation/skills/ReachGrasp.cpp

@@ -160,6 +160,10 @@ namespace armarx::manipulation::skills
 
         draw(in.params);
 
+        ARMARX_CHECK(ctx.robot.synchronize(Clock::Now()));
+        const core::Pose global_T_tcp_desired(grasp->getTcpPoseGlobal(graspNode->getGlobalPose()));
+        initializeControllerTcpTarget(global_T_tcp_desired, in.params);
+
         return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
     }
 

source/armarx/manipulation/skills/ReachGrasp.h

@@ -103,6 +103,12 @@ namespace armarx::manipulation::skills
             return true;
         };
 
+        virtual bool
+        initializeControllerTcpTarget(const core::Pose& global_T_tcp, const ParamsT& params)
+        {
+            return true;
+        };
+
         virtual bool
         cleanupController()
         {