RobotReceiver.cpp

Go to the documentation of this file.

/**
 * 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 "RobotReceiver.h"
 
#include <Eigen/Core>
 
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/view_selection/target_provider/handover/util.h>
 
namespace armarx::view_selection::target_provider::handover
{
    RobotReceiver::RobotReceiver(const std::string& humanTrackingId,
                                 const VirtualRobot::RobotPtr& robot) :
        HandoverTargetProvider(humanTrackingId, robot)
    {
    }
 
    std::vector<gaze_targets::GazeTarget>
    RobotReceiver::updateTargets(const armem::human::HumanPose& human)
    {
        std::vector<gaze_targets::GazeTarget> targets;
 
        switch (phase_)
        {
            case Phase::PreHandover:
                targets = updateTargetsPreHandover(human);
                break;
            case Phase::PostHandover:
                targets = updateTargetsPreHandover(human);
                break;
        }
 
        return targets;
    }
 
    std::vector<gaze_targets::GazeTarget>
    RobotReceiver::updateTargetsPreHandover(const armem::human::HumanPose& human) const
    {
 
        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 handPriorityFromDistance =
            [&](const float handBodyDistance, const float handRobotDistance)
        {
            // f = 1: highest priority
            // f = 0: low prio
            const float normalizedHandBodyPriority =
                std::clamp((handBodyDistance - params.minHandReachoutDistance) /
                               (params.maxHandReachoutDistance - params.minHandReachoutDistance),
                           0.f,
                           1.f);
 
            // f = 1: highest priority
            // f = 0: low prio
            const float normalizedHandRobotPriority = std::clamp(
                (params.maxHandoverInitializationDistance - handRobotDistance) /
                    (params.maxHandoverInitializationDistance - params.minHandoverDistance),
                0.f,
                1.f);
 
            return params.handPriorityScaling * normalizedHandBodyPriority *
                   normalizedHandRobotPriority;
        };
 
        const auto handsPriorityFromDistance = [&](const float handsRobotDistance)
        {
            return handsRobotDistance < params.maxHandsInitializationDistance
                       ? (params.headPriority + 1)
                       : 0;
        };
 
 
        // 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 handRobotDistance = leftWristKp.positionRobot->toEigen().head<2>().norm();
 
            const float priority = handPriorityFromDistance(distanceLeft, handRobotDistance);
            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 handRobotDistance = rightWristKp.positionRobot->toEigen().head<2>().norm();
 
            const float priority = handPriorityFromDistance(distanceRight, handRobotDistance);
 
            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);
        }
 
        // In addition to the individual hand targets, we create one target for the center point of both hands
        // This will lower the gaze target and the head won't need to move too much once an arm is streched out.
        {
            const armem::human::PoseKeypoint leftWristKp =
                human.keypoints.at(leftWristKpName.value());
            const armem::human::PoseKeypoint rightWristKp =
                human.keypoints.at(rightWristKpName.value());
 
            const float alpha = 0.5; // might be improved in the future
 
            const Eigen::Vector3f centerHandsPosition =
                leftWristKp.positionGlobal->toEigen() +
                alpha * (rightWristKp.positionGlobal->toEigen() -
                         leftWristKp.positionGlobal->toEigen());
 
            const float centerHandsRobotDistance =
                (centerHandsPosition - robot_->getGlobalPosition()).head<2>().norm();
 
            const float priority = handsPriorityFromDistance(centerHandsRobotDistance);
 
            const gaze_targets::TargetPriority targetPriority(
                gaze_targets::AttentionType::TaskDriven, priority);
 
            const std::string& agent = leftWristKp.positionGlobal->agent;
 
            gaze_targets::GazeTarget handsTarget(
                "center_hands",
                FramedPosition(centerHandsPosition, GlobalFrame, agent),
                targetPriority,
                Duration::Seconds(1),
                false);
            gazeTargets.push_back(handsTarget);
        }
 
        // combined wrist
 
        // some debugging
        {
            for (const auto& target : gazeTargets)
            {
                ARMARX_VERBOSE << target.name << ": " << target.priority;
            }
        }
 
        return gazeTargets;
    }
 
    std::vector<gaze_targets::GazeTarget>
    RobotReceiver::updateTargetsAfterHandover(const armem::human::HumanPose& human) const
    {
 
        const auto headKpName = util::headKeypoint(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());
 
        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);
        }
 
        return gazeTargets;
    }
 
} // namespace armarx::view_selection::target_provider::handover