Implement 'PointAt'-skill

Implements a generalized 'PointAt'-skill to let the robot point somewhere. An ARMAR-6-specific version was previously implemented in oml_demo.

Current state: Skill works. IK has some problems (see below).

Known problems / open questions

• Skill implementation

  • Compiler warning
    Compiler throws Woverloaded-virtual in pointing_skills. This is due to the fact that the compile-options from SimoxControl (pointing_skills depends on pointing_core, which depends on SimoxControl::example_method) are being used to compile pointing_skills. SimoxControl's compile-options enable Woverloaded-virtual on level 2, while usually only level 1 is being used through -Wall. See also the documentation for Woverloaded-virtual. -> ignore for now
  • Getting the endeffector is hardcoded in core/Pointing.cpp -> ac16076f
  • Skill can crash if already pointing at target
  • set logging tag -> 7cc9b4a5
  • Wrist position ignored (i.e. wrist might be angled before, thus robot points in wrong direction) -> 1b907616 zero all wrist joints
  • Cannot abort skill while IK computes trajectory -> 9846e1e3
  • Use FramedPosition for target -> 04528ae6 Segfaults in RobotAPI if invalid frame-string is passed -> 3b3e9e67

• IK -> ignore for now

  • Trajectories and end configurations computed by the IK are not self-collision free (loading collision model doesn't help)
  • If trajectories from IK were self-collision free: would generating a time-optimal trajectory still be self-collision free?
  • Generated trajectories are sometimes very inefficient (trajectories sometimes very shaky)
  • Use wrist or TCP as wrist?
  • Meter/millimeter conversion correct?
  • IK doesn't reliably find a solution
  • Cannot control how far the arm extends (i.e. sometimes the arm is fully extended, sometimes the upper arm stays close to the body and only the forearm points at the target)
  • Should the IK stay in simox::control::method::example?
  • Can we remove the Armar6-specific wrapper simox::control::method::example::PointingRightArm (used by the oml_demo-skill)?

• Other

  • add simox-control to required modules of control axii module -> sw/armarx/meta/axii!754 (merged)

Possible further work

• Implement skill to point and look at target

Needed merge requests:

• Resolve "components/RobotUnitSimulation: Wrong ... (sw/armarx/armarx-simulation!69 - merged)
• Fix Trajectory timestamp generation (sw/armarx/robot-api!430 - merged)
• https://git.h2t.iar.kit.edu/sw/armarx-integration/robots/armar7/models/-/merge_requests/18+
• armarx/skills/control: Add SimoxControl dependency (sw/armarx/meta/axii!754 - merged)
• Fix template ambiguities in aron_conversions (sw/armarx/robot-api!439 - merged)

source/armarx/control/pointing/core/Pointing.cpp

 #include "Pointing.h"
 
+#include <VirtualRobot/RobotNodeSet.h>
+
 #include <ArmarXCore/core/time.h>
 
 #include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
@@ -32,23 +34,18 @@ namespace armarx::control::pointing::core
 
         armarx::TrajectoryPtr traj = computTrajectory(robot, params.side, target);
 
+        addWristTrajectory(robot, params.side, traj);
+
         if (params.handShape)
         {
             auto shapeJointValues = GetShapeJointValues(robot, params.side, *params.handShape);
             addHandTrajectory(robot, traj, shapeJointValues);
         }
 
-        if (traj->dim() > 0)
-        {
-            traj = traj->calculateTimeOptimalTrajectory(
-                params.maxVelocity, params.maxAcceleration, 0.0, IceUtil::Time::milliSeconds(10));
+        traj = traj->calculateTimeOptimalTrajectory(
+            params.maxVelocity, params.maxAcceleration, 0.0, IceUtil::Time::milliSeconds(10));
 
-            playTrajectory(traj);
-        }
-        else
-        {
-            ARMARX_INFO << "Already pointing at target";
-        }
+        playTrajectory(traj);
     }
 
     void
@@ -96,16 +93,37 @@ namespace armarx::control::pointing::core
         }
         ARMARX_VERBOSE << "Done computing trajectory";
 
-        if (traj->size() == 1) // already pointing at target
+        if (traj->size() == 1) // forearm already aligned with target
         {
             return armarx::TrajectoryPtr(new Trajectory);
         }
         return traj;
     }
 
+    void
+    Pointing::addWristTrajectory(VirtualRobot::RobotPtr robot, Side side, TrajectoryPtr traj)
+    {
+        auto humanMapping = robot->getHumanMapping();
+        ARMARX_CHECK(humanMapping) << "Robot '" << robot->getName() << "' has no human mapping";
+
+        auto armDescription = (side == LEFT) ? humanMapping->leftArm : humanMapping->rightArm;
+        auto wrist = armDescription.segments.wrist.nodeName;
+
+        auto nodeSetArm = robot->getRobotNodeSet(armDescription.nodeSet);
+
+        // iterate over all joints, starting from the wrist, to move them into a neutral position
+        std::for_each(nodeSetArm->begin() + nodeSetArm->getRobotNodeIndex(wrist),
+                      nodeSetArm->end(),
+                      [traj](VirtualRobot::RobotNodePtr robotNode) {
+                          traj->addDimension({robotNode->getJointValue(), 0.0},
+                                             Ice::DoubleSeq(),
+                                             robotNode->getName());
+                      });
+    }
+
     void
     Pointing::addHandTrajectory(VirtualRobot::RobotPtr robot,
-                                armarx::TrajectoryPtr& traj,
+                                armarx::TrajectoryPtr traj,
                                 const std::map<std::string, float>& shapeJointValues)
     {
         for (const auto& [jointName, targetValue] : shapeJointValues)