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

@@ -14,6 +14,7 @@ namespace armarx::control::pointing::core
 {
     Pointing::Pointing(const Remote& remote) : remote_(remote)
     {
+        setTag("Pointing");
     }
 
     void
@@ -29,16 +30,16 @@ namespace armarx::control::pointing::core
 
         VirtualRobot::RobotPtr robot = getRobot();
 
-        Eigen::Vector3f target = params.target.toGlobalEigen(robot);
+        Eigen::Vector3f target = toGlobal(params.target, robot);
         visualizeTarget(target);
 
-        armarx::TrajectoryPtr traj = computTrajectory(robot, params.side, target);
+        armarx::TrajectoryPtr traj = computeArmTrajectory(robot, params.side, target);
 
         addWristTrajectory(robot, params.side, traj);
 
         if (params.handShape)
         {
-            auto shapeJointValues = GetShapeJointValues(robot, params.side, *params.handShape);
+            auto shapeJointValues = getShapeJointValues(robot, params.side, *params.handShape);
             addHandTrajectory(robot, traj, shapeJointValues);
         }
 
@@ -69,6 +70,18 @@ namespace armarx::control::pointing::core
         return robot;
     }
 
+    Eigen::Vector3f
+    Pointing::toGlobal(const FramedPosition& framedPosition, VirtualRobot::RobotPtr robot)
+    {
+        if (not(framedPosition.frame == armarx::GlobalFrame ||
+                robot->hasRobotNode(framedPosition.frame)))
+        {
+            ARMARX_ERROR << "'" << framedPosition.frame << "' is not a valid frame name";
+            throw armarx::exceptions::local::PointingFailedException();
+        }
+        return framedPosition.toGlobalEigen(robot);
+    }
+
     void
     Pointing::visualizeTarget(const Eigen::Vector3f& target)
     {
@@ -81,7 +94,7 @@ namespace armarx::control::pointing::core
     }
 
     TrajectoryPtr
-    Pointing::computTrajectory(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f target)
+    Pointing::computeArmTrajectory(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f target)
     {
         ARMARX_VERBOSE << "Computing trajectory...";
         auto ik = PointingIK(robot, side, target);
@@ -100,11 +113,13 @@ namespace armarx::control::pointing::core
             }
             if (aborted)
             {
-                ARMARX_INFO << "Pointing aborted.";
+                ARMARX_INFO << "Pointing aborted";
                 throw armarx::exceptions::local::PointingAbortedException();
             }
         } while (not ik.isTargetReached());
 
+        ARMARX_VERBOSE << "Done computing trajectory";
+
         return ik.getTrajectory();
     }
 
@@ -129,6 +144,33 @@ namespace armarx::control::pointing::core
                       });
     }
 
+    std::map<std::string, float>
+    Pointing::getShapeJointValues(VirtualRobot::RobotPtr robot,
+                                  Side side,
+                                  const std::string& shapeName)
+    {
+        auto humanMapping = robot->getHumanMapping();
+        ARMARX_CHECK(humanMapping) << "Robot '" << robot->getName() << "' has no human mapping";
+        auto armDescription = (side == LEFT) ? humanMapping->leftArm : humanMapping->rightArm;
+        auto tcp = armDescription.segments.tcp.nodeName;
+
+        // get endeffector whichs tcp-name matches the tcp-name of the according hand
+        auto eefs = robot->getEndEffectors();
+        auto eef = std::find_if(eefs.begin(),
+                                eefs.end(),
+                                [&tcp](VirtualRobot::EndEffectorPtr eef)
+                                { return eef->getTcpName() == tcp; });
+        ARMARX_CHECK(eef != eefs.end());
+
+        auto shape = (*eef)->getPreshape(shapeName);
+        if (not shape)
+        {
+            ARMARX_ERROR << "Hand '" << (*eef)->getName() << "' has no shape '" << shapeName << "'";
+            throw armarx::exceptions::local::PointingFailedException();
+        }
+        return shape->getRobotNodeJointValueMap();
+    }
+
     void
     Pointing::addHandTrajectory(VirtualRobot::RobotPtr robot,
                                 armarx::TrajectoryPtr traj,
@@ -167,21 +209,4 @@ namespace armarx::control::pointing::core
         ARMARX_INFO << "Pointing target reached";
     }
 
-    std::map<std::string, float>
-    Pointing::GetShapeJointValues(VirtualRobot::RobotPtr robot,
-                                  Side side,
-                                  const std::string& shapeName)
-    {
-        auto eefName = std::string("Hand_") + (side == Side::LEFT ? "L" : "R") + "_EEF"; // TODO
-        auto eef = robot->getEndEffector(eefName);
-
-        auto shape = eef->getPreshape(shapeName);
-        if (not shape)
-        {
-            ARMARX_ERROR << "Hand '" << eefName << "' has no shape '" << shape << "'";
-            throw armarx::exceptions::local::PointingFailedException();
-        }
-        return shape->getRobotNodeJointValueMap();
-    }
-
 } // namespace armarx::control::pointing::core