update to target provider/handover lib

source/armarx/view_selection/target_provider/CMakeLists.txt

@@ -2,28 +2,21 @@ armarx_add_library(target_provider
     SOURCES
         target_provider.cpp
         handover/HandoverTargetProvider.cpp
-        handover/HumanGiver.cpp
-        handover/HumanReceiver.cpp
+        handover/RobotGiver.cpp
+        handover/RobotReceiver.cpp
+        handover/util.cpp
     HEADERS
         target_provider.h
         handover/HandoverTargetProvider.h
-        handover/HumanGiver.h
-        handover/HumanReceiver.h
+        handover/RobotGiver.h
+        handover/RobotReceiver.h
+        handover/util.h
     DEPENDENCIES_PUBLIC
         ArmarXCoreInterfaces
         ArmarXCore
         armem_human
         Simox::VirtualRobot
-    # DEPENDENCIES_PRIVATE
-        # ...
-    # DEPENDENCIES_INTERFACE
-        # ...
-    # DEPENDENCIES_LEGACY_PUBLIC
-        # ...
-    # DEPENDENCIES_LEGACY_PRIVATE
-        # ...
-    # DEPENDENCIES_LEGACY_INTERFACE
-        # ...
+        armarx_view_selection::gaze_targets
 )
 
  

source/armarx/view_selection/target_provider/handover/HandoverTargetProvider.h

@@ -39,8 +39,7 @@ namespace armarx::view_selection::target_provider::handover
         virtual ~HandoverTargetProvider() = default;
 
         virtual std::vector<gaze_targets::GazeTarget>
-        updateTargets(const std::vector<armem::human::HumanPose>&
-                          humans) = 0; // TODO(fabian.reister): object poses
+        updateTargets(const armem::human::HumanPose& human) = 0;
 
     protected:
         const std::string humanTrackingId_;

source/armarx/view_selection/target_provider/handover/RobotGiver.cpp

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "RobotGiver.h"
+#include <VirtualRobot/Robot.h>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Duration.h"
+
+#include "armarx/view_selection/gaze_targets/AttentionType.h"
+#include "armarx/view_selection/gaze_targets/GazeTarget.h"
+#include "armarx/view_selection/target_provider/handover/util.h"
+
+namespace armarx::view_selection::target_provider::handover
+{
+    std::vector<gaze_targets::GazeTarget>
+    RobotGiver::updateTargets(const armem::human::HumanPose& human)
+    {
+
+        const auto headKpName = util::headKeypoint(human.poseModelId);
+        const auto pelvisKpName = util::pelvisKeypoint(human.poseModelId);
+        const auto leftWristKpName = util::leftWristKeypoint(human.poseModelId);
+        const auto rightWristKpName = util::rightWristKeypoint(human.poseModelId);
+
+        ARMARX_CHECK(headKpName.has_value());
+        ARMARX_CHECK(leftWristKpName.has_value());
+        ARMARX_CHECK(rightWristKpName.has_value());
+        ARMARX_CHECK(pelvisKpName.has_value());
+
+
+        std::vector<gaze_targets::GazeTarget> gazeTargets;
+
+        if (human.keypoints.count(headKpName.value()) == 0)
+        {
+            ARMARX_WARNING << deactivateSpam(1) << "Head keypoint `" << headKpName.value()
+                           << "` not available!";
+            return {};
+        };
+
+        // head target
+        {
+            const armem::human::PoseKeypoint headKp = human.keypoints.at(headKpName.value());
+
+            const gaze_targets::TargetPriority targetPriority(
+                gaze_targets::AttentionType::TaskDriven, params.headPriority);
+
+            gaze_targets::GazeTarget headTarget(
+                "head", headKp.positionGlobal.value(), targetPriority, Duration::Seconds(1), false);
+            gazeTargets.push_back(headTarget);
+        }
+
+        const armem::human::PoseKeypoint pelvisKp = human.keypoints.at(pelvisKpName.value());
+
+        const float distanceHumanRobot = (pelvisKp.positionGlobal.value().toEigen() - robot_->getGlobalPosition()).head<2>().norm();
+        ARMARX_VERBOSE << VAROUT(distanceHumanRobot);
+
+        if(distanceHumanRobot > params.maxHandoverInitializationDistance)
+        {
+            return gazeTargets;
+        }
+
+
+        // the priority to look at the humans hands is defined by the distance to the pelvis (2d projection)
+
+        const auto priorityFromDistance = [&](const float distance)
+        {
+            // f = 1: highest priority
+            // f = 0: low prio
+            const float normalizedPriority =
+                (distance - params.minHandReachoutDistance) /
+                (params.maxHandReachoutDistance - params.minHandReachoutDistance);
+            return params.handPriorityScaling * normalizedPriority;
+        };
+
+        // left wrist target
+        if (human.keypoints.count(leftWristKpName.value()) > 0)
+        {
+            const armem::human::PoseKeypoint leftWristKp =
+                human.keypoints.at(leftWristKpName.value());
+
+            const float distanceLeft =
+                (leftWristKp.positionGlobal->toEigen() - pelvisKp.positionGlobal->toEigen()).norm();
+
+            const float priority = priorityFromDistance(distanceLeft);
+            const gaze_targets::TargetPriority targetPriority(
+                gaze_targets::AttentionType::TaskDriven, priority);
+
+            gaze_targets::GazeTarget handTarget("left_hand",
+                                                leftWristKp.positionGlobal.value(),
+                                                targetPriority,
+                                                Duration::Seconds(1),
+                                                false);
+            gazeTargets.push_back(handTarget);
+        }
+
+        // right wrist target
+        if (human.keypoints.count(rightWristKpName.value()) > 0)
+        {
+            const armem::human::PoseKeypoint rightWristKp =
+                human.keypoints.at(rightWristKpName.value());
+
+            const float distanceRight =
+                (rightWristKp.positionGlobal->toEigen() - pelvisKp.positionGlobal->toEigen())
+                    .norm();
+            const float priority = priorityFromDistance(distanceRight);
+
+            const gaze_targets::TargetPriority targetPriority(
+                gaze_targets::AttentionType::TaskDriven, priority);
+
+            gaze_targets::GazeTarget handTarget("right_hand",
+                                                rightWristKp.positionGlobal.value(),
+                                                targetPriority,
+                                                Duration::Seconds(1),
+                                                false);
+            gazeTargets.push_back(handTarget);
+        }
+
+        // combined wrist
+
+        // some debugging
+        {
+            for(const auto& target: gazeTargets)
+            {
+                ARMARX_VERBOSE << target.name << ": " << target.priority;
+            }
+        }
+
+        return gazeTargets;
+    }
+
+
+} // namespace armarx::view_selection::target_provider::handover

source/armarx/view_selection/target_provider/handover/HumanReceiver.h → source/armarx/view_selection/target_provider/handover/RobotGiver.h

@@ -29,16 +29,34 @@ namespace armarx::view_selection::target_provider::handover
     /**
     * @brief The robot is acting as the giver
     * 
+    * The giver will first look at the receiver's face during reach. At a certain distance to the receiver, it will then look at the receiver's hand.
+    * Once the object is exchanged, the giver will then look at the receivers face.
     */
-    class HumanReceiver : public HandoverTargetProvider
+    class RobotGiver : public HandoverTargetProvider
     {
     public:
         using HandoverTargetProvider::HandoverTargetProvider;
 
+        struct Params
+        {
+            float maxHandoverInitializationDistance = 2500;
+            
+            float minHandReachoutDistance = 300; 
+            float maxHandReachoutDistance = 1000;
+
+            float handPriorityScaling = 350;
+
+            float headPriority = 100;
+
+        };
+
+        
+
         std::vector<gaze_targets::GazeTarget>
-        updateTargets(const std::vector<armem::human::HumanPose>& humans) override;
+        updateTargets(const armem::human::HumanPose& human) override;
 
     protected:
     private:
+        const Params params;
     };
 } // namespace armarx::view_selection::target_provider::handover

source/armarx/view_selection/target_provider/handover/HumanGiver.cpp → source/armarx/view_selection/target_provider/handover/RobotReceiver.cpp

@@ -19,12 +19,13 @@
  *             GNU General Public License
  */
 
-#include "HumanGiver.h"
+#include "RobotReceiver.h"
 
 namespace armarx::view_selection::target_provider::handover
 {
+
     std::vector<gaze_targets::GazeTarget>
-    HumanGiver::updateTargets(const std::vector<armem::human::HumanPose>& humans)
+    RobotReceiver::updateTargets(const armem::human::HumanPose& human)
     {
         // look at the giver's hand until object is received
         // then, look at the giver's face

source/armarx/view_selection/target_provider/handover/HumanGiver.h → source/armarx/view_selection/target_provider/handover/RobotReceiver.h

@@ -30,13 +30,13 @@ namespace armarx::view_selection::target_provider::handover
     * @brief The robot is acting as the receiver
     * 
     */
-    class HumanGiver : public HandoverTargetProvider
+    class RobotReceiver : public HandoverTargetProvider
     {
     public:
         using HandoverTargetProvider::HandoverTargetProvider;
 
         std::vector<gaze_targets::GazeTarget>
-        updateTargets(const std::vector<armem::human::HumanPose>& humans) override;
+        updateTargets(const armem::human::HumanPose& human) override;
 
     protected:
     private:

source/armarx/view_selection/target_provider/handover/HumanReceiver.cpp → source/armarx/view_selection/target_provider/handover/types.h

@@ -18,16 +18,16 @@
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
+ 
+ #pragma once
+ 
+ namespace armarx::view_selection::target_provider::handover
+ {
+ 
+  enum class Phase
+  {
+    PreHandover,
+    PostHandover
+  };
 
-#include "HumanReceiver.h"
-
-namespace armarx::view_selection::target_provider::handover
-{
-    std::vector<gaze_targets::GazeTarget>
-    HumanReceiver::updateTargets(const std::vector<armem::human::HumanPose>& humans)
-    {
-        return {}; // FIXME implement
-    }
-
-
-} // namespace armarx::view_selection::target_provider::handover
+ } // namespace armarx::view_selection::target_provider::handover

source/armarx/view_selection/target_provider/handover/util.cpp

+#include "util.h"
+
+#include <cstddef>
+#include <optional>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "VisionX/libraries/human/pose/model/k4a_bt_body_32.h"
+#include "VisionX/libraries/human/pose/model/openpose_body_25.h"
+
+namespace armarx::view_selection::target_provider::handover::util
+{
+    std::optional<std::string>
+    headKeypoint(const std::string& poseModelId)
+    {
+        namespace k4a_bt_body_32 = human::pose::model::k4a_bt_body_32;
+        namespace openpose_body_25 = human::pose::model::openpose_body_25;
+
+        if (poseModelId == openpose_body_25::ModelId)
+        {
+            return openpose_body_25::JointNames.to_name(openpose_body_25::Joints::Nose);
+        }
+
+        if (poseModelId == k4a_bt_body_32::ModelId)
+        {
+            return k4a_bt_body_32::JointNames.to_name(k4a_bt_body_32::Joints::Head);
+        }
+
+        ARMARX_ERROR << "Unknown pose model ID '" << poseModelId << "'.";
+        return std::nullopt;
+    }
+
+    std::optional<std::string>
+    leftWristKeypoint(const std::string& poseModelId)
+    {
+        namespace k4a_bt_body_32 = human::pose::model::k4a_bt_body_32;
+        namespace openpose_body_25 = human::pose::model::openpose_body_25;
+
+        if (poseModelId == openpose_body_25::ModelId)
+        {
+            return openpose_body_25::JointNames.to_name(openpose_body_25::Joints::WristLeft);
+        }
+
+        if (poseModelId == k4a_bt_body_32::ModelId)
+        {
+            return k4a_bt_body_32::JointNames.to_name(k4a_bt_body_32::Joints::WristLeft);
+        }
+
+        ARMARX_ERROR << "Unknown pose model ID '" << poseModelId << "'.";
+        return std::nullopt;
+    }
+
+
+    std::optional<std::string>
+    rightWristKeypoint(const std::string& poseModelId)
+    {
+        namespace k4a_bt_body_32 = human::pose::model::k4a_bt_body_32;
+        namespace openpose_body_25 = human::pose::model::openpose_body_25;
+
+        if (poseModelId == openpose_body_25::ModelId)
+        {
+            return openpose_body_25::JointNames.to_name(openpose_body_25::Joints::WristRight);
+        }
+
+        if (poseModelId == k4a_bt_body_32::ModelId)
+        {
+            return k4a_bt_body_32::JointNames.to_name(k4a_bt_body_32::Joints::WristRight);
+        }
+
+        ARMARX_ERROR << "Unknown pose model ID '" << poseModelId << "'.";
+        return std::nullopt;
+    }
+
+
+std::optional<std::string> pelvisKeypoint(const std::string& poseModelId)
+{
+    namespace k4a_bt_body_32 = human::pose::model::k4a_bt_body_32;
+        namespace openpose_body_25 = human::pose::model::openpose_body_25;
+
+        if (poseModelId == openpose_body_25::ModelId)
+        {
+            return openpose_body_25::JointNames.to_name(openpose_body_25::Joints::Pelvis);
+        }
+
+        if (poseModelId == k4a_bt_body_32::ModelId)
+        {
+            return k4a_bt_body_32::JointNames.to_name(k4a_bt_body_32::Joints::Pelvis);
+        }
+
+        ARMARX_ERROR << "Unknown pose model ID '" << poseModelId << "'.";
+        return std::nullopt;
+    }
+
+
+} // namespace armarx::view_selection::target_provider::handover::util

source/armarx/view_selection/target_provider/handover/util.h

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <optional>
+#include <string>
+
+namespace armarx::view_selection::target_provider::handover::util
+{
+    std::optional<std::string> headKeypoint(const std::string& poseModelId);
+
+    std::optional<std::string> leftWristKeypoint(const std::string& poseModelId);
+    std::optional<std::string> rightWristKeypoint(const std::string& poseModelId);
+
+    std::optional<std::string> pelvisKeypoint(const std::string& poseModelId);
+
+} // namespace armarx::view_selection::target_provider::handover::util