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
@@ -32,13 +33,13 @@ namespace armarx::control::pointing::core
         Eigen::Vector3f target = params.target.toGlobalEigen(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);
         }
 
@@ -64,7 +65,7 @@ namespace armarx::control::pointing::core
         if (not robot)
         {
             ARMARX_ERROR << "Could not get synchronized robot '" << remote_.robotName << "'";
-            throw PointingFailedException();
+            throw armarx::exceptions::local::PointingFailedException();
         }
         return robot;
     }
@@ -81,7 +82,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);
@@ -96,15 +97,17 @@ namespace armarx::control::pointing::core
             if (ik.optimize() < 1e-6)
             {
                 ARMARX_WARNING << "PointingIK didn't find a solution";
-                throw PointingFailedException();
+                throw armarx::exceptions::local::PointingFailedException();
             }
             if (aborted)
             {
-                ARMARX_INFO << "Pointing aborted.";
-                throw PointingAbortedException();
+                ARMARX_INFO << "Pointing aborted";
+                throw armarx::exceptions::local::PointingAbortedException();
             }
         } while (not ik.isTargetReached());
 
+        ARMARX_VERBOSE << "Done computing trajectory";
+
         return ik.getTrajectory();
     }
 
@@ -129,6 +132,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,
@@ -160,28 +190,11 @@ namespace armarx::control::pointing::core
             {
                 ARMARX_INFO << "Pointing aborted. Stopping trajectory";
                 remote_.trajectoryPlayer->stopTrajectoryPlayer();
-                throw PointingAbortedException();
+                throw armarx::exceptions::local::PointingAbortedException();
             }
             clock.waitFor(Duration::MilliSeconds(10));
         }
         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 PointingFailedException();
-        }
-        return shape->getRobotNodeJointValueMap();
-    }
-
 } // namespace armarx::control::pointing::core