ReachOutToHumanPose.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/>.
 *
 * @package    RobotSkillTemplates::HandOverGroup
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ReachOutToHumanPose.h"
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/ManagedIceObject.h>
 
#include "HandOverGroupStatechartContext.h"
 
//#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/observers/filters/AverageFilter.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
#include <RobotAPI/components/ArViz/Client/elements/RobotHand.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h>
#include <RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h>
 
 
using namespace armarx;
using namespace visionx;
using namespace HandOverGroup;
 
// DO NOT EDIT NEXT LINE
ReachOutToHumanPose::SubClassRegistry
    ReachOutToHumanPose::Registry(ReachOutToHumanPose::GetName(),
                                  &ReachOutToHumanPose::CreateInstance);
 
#define ENABLE_DEBUG_DRAWER 0
 
void
ReachOutToHumanPose::onEnter()
{
    // put your user code for the enter-point here
    // execution time should be short (<100ms)
    //    ManagedIceObjectPtr obj = ManagedIceObjectPtr::dynamicCast(getContext());
    //    obj->getIceManager()->subscribeTopic()
}
 
void
ReachOutToHumanPose::run()
{
    viz::ScopedClient arviz = viz::Client::createFromTopic("ReachOutToHumanPose", getArvizTopic());
    arviz.commit({arviz.layer("HandOverState")}); // Clear layer.
 
    HandOverGroupStatechartContextExtension* c =
        getContext<HandOverGroupStatechartContextExtension>();
    c->getOpenPoseEstimation()->start3DPoseEstimation();
 
    ARMARX_CHECK_EXPRESSION(c);
    CycleUtil cycle(10);
    filters::PoseMedianFilter lwristFilter(in.getPoseMedianFilterSize()),
        rwristFilter(in.getPoseMedianFilterSize());
    filters::PoseAverageFilter headIKTargetFilter(50);
    std::vector<filters::AverageFilter> targetPositionFilter(3), targetVelocityFilter(3);
    for (auto& filter : targetPositionFilter)
    {
        filter.windowFilterSize = in.getTargetPositionAverageFilterSize();
    }
    for (auto& filter : targetVelocityFilter)
    {
        filter.windowFilterSize = in.getTargetPositionAverageFilterSize();
    }
    float maxTCPVelocity = in.getMaxTCPVelocity();
    float considerForceDistanceThreshold = in.getConsiderForceDistanceThreshold();
    auto robot = getLocalRobot();
 
    {
        auto node = robot->getRobotNode("ArmR3_Sho2");
        node->setJointLimits(node->getJointLimitLow(), VirtualRobot::MathTools::deg2rad(90));
    }
    {
        auto node = robot->getRobotNode("ArmR5_Elb1");
        node->setJointLimits(VirtualRobot::MathTools::deg2rad(5), node->getJointLimitHigh());
    }
    {
        auto node = robot->getRobotNode("ArmR1_Cla1");
        node->setJointLimits(VirtualRobot::MathTools::deg2rad(-45),
                             VirtualRobot::MathTools::deg2rad(45));
    }
 
    {
        auto node = robot->getRobotNode("ArmL3_Sho2");
        node->setJointLimits(node->getJointLimitLow(), VirtualRobot::MathTools::deg2rad(90));
    }
    {
        auto node = robot->getRobotNode("ArmL5_Elb1");
        node->setJointLimits(VirtualRobot::MathTools::deg2rad(5), node->getJointLimitHigh());
    }
    {
        auto node = robot->getRobotNode("ArmL1_Cla1");
        node->setJointLimits(VirtualRobot::MathTools::deg2rad(-45),
                             VirtualRobot::MathTools::deg2rad(45));
    }
 
    auto cameraRobot = getLocalRobot(); // receives new copy
    ARMARX_CHECK_EXPRESSION(robot.get() != cameraRobot.get());
    filters::AverageFilter forceMagnitudeAvgFilter;
    forceMagnitudeAvgFilter.windowFilterSize = 10;
 
    getHeadIKUnit()->request();
 
    // look straight ahead
    ARMARX_INFO << "InitialHeadIKTarget(): " << in.getInitialHeadIKTarget()->output();
    getHeadIKUnit()->setHeadTarget(in.getHeadIKNodeSet(), in.getInitialHeadIKTarget());
    TimeUtil::MSSleep(1000);
 
    auto kinematicChainName = in.getKinematicChainName();
    MultiDimPIDController pid(in.getKp(), 0, 0);
    IceUtil::Time lastTimestamp = armarx::TimeUtil::GetTime();
    ARMARX_CHECK_EXPRESSION(robot);
    auto nodeset = robot->getRobotNodeSet(kinematicChainName);
    auto tcp = nodeset->getTCP();
    auto kinematicRootName = nodeset->getKinematicRoot()->getName();
 
    ARMARX_INFO << VAROUT(kinematicRootName)
                << " position in root: " << nodeset->getKinematicRoot()->getPositionInRootFrame();
    FramedDirectionPtr targetVelocity;
    DatafieldRefPtr forceTorqueRef = DatafieldRefPtr::dynamicCast(
        getForceTorqueObserver()->getDataFieldRef(new DataFieldIdentifier(
            getForceTorqueObserver()->getObserverName(), in.getFTSensorName(), "forces")));
    ARMARX_CHECK_EXPRESSION(forceTorqueRef);
    DatafieldRefPtr nulledForceTorqueRef;
    NJointCartesianVelocityControllerWithRampConfigPtr ctrlCfg =
        new NJointCartesianVelocityControllerWithRampConfig();
    ctrlCfg->KpJointLimitAvoidance = 0;
    ctrlCfg->jointLimitAvoidanceScale = 1;
    ctrlCfg->mode = CartesianSelectionMode::eAll;
    ctrlCfg->nodeSetName = kinematicChainName;
    ctrlCfg->tcpName = tcp->getName();
    ctrlCfg->maxNullspaceAcceleration = 2;
    ctrlCfg->maxPositionAcceleration = in.getTCPAcceleration();
    ctrlCfg->maxOrientationAcceleration = 1;
 
    CartesianPositionController posController(tcp);
    posController.maxOriVel = in.getMaxOrientationVelocity();
    posController.maxPosVel = in.getMaxTCPVelocity();
    posController.KpOri = in.getKpOrientation();
    posController.KpPos = in.getKp();
 
    auto ctrlName = "ReachOutToHumanTCPCtrl";
    TIMING_START(NJointCtrlCreation);
    ARMARX_INFO << deactivateSpam(1) << "Getting old controller";
    if (auto oldCtrl = getRobotUnit()->getNJointController(ctrlName))
    {
        ARMARX_INFO << deactivateSpam(1) << "got old controller";
        while (oldCtrl->isControllerActive())
        {
            oldCtrl->deactivateController();
            ARMARX_INFO << deactivateSpam(1) << "Waiting for old controller to be deactivated";
            usleep(30000);
        }
        ARMARX_INFO << deactivateSpam(1) << "deleting old controller";
        getRobotUnit()->deleteNJointController(ctrlName);
        usleep(100000);
    }
    ARMARX_INFO << deactivateSpam(1) << "Getting new controller";
    auto baseCtrl = getRobotUnit()->createNJointController(
        "NJointCartesianVelocityControllerWithRamp", ctrlName, ctrlCfg);
    ARMARX_CHECK_EXPRESSION(baseCtrl);
    NJointCartesianVelocityControllerWithRampInterfacePrx ctrl =
        NJointCartesianVelocityControllerWithRampInterfacePrx::checkedCast(baseCtrl);
    TIMING_END(NJointCtrlCreation);
    ARMARX_CHECK_EXPRESSION(ctrl) << baseCtrl->ice_ids();
    ctrl->activateController();
 
    auto targetOrientation = in.getHandOrientation();
    IceUtil::Time startTime = TimeUtil::GetTime();
 
    RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
 
    const Eigen::Vector3f offset{0, 0, 300};
    Eigen::Vector3f idlePosi = tcp->getPositionInRootFrame() + offset;
    FramedPositionPtr idlePosition =
        new FramedPosition(idlePosi, robot->getRootNode()->getName(), robot->getName());
    //    in.getIdlePosition();
 
    bool drawDebugInfos = in.getDrawDebugInfos();
    FramedPositionPtr lastIKTarget;
    while (!isRunningTaskStopped()) // stop run function if returning true
    {
        viz::Layer vizLayer = arviz.layer("HandOverState");
 
        auto now = armarx::TimeUtil::GetTime();
        auto timeSinceSinceStart = now - startTime;
        IceUtil::Time duration = now - lastTimestamp;
        lastTimestamp = now;
 
        c->getImageSourceSelectionTopic()->setImageSource("OpenPoseEstimationResult", 5000);
 
        // do your calculations
        std::vector<Keypoint3DMap> keypointList;
        IceUtil::Time timestamp;
        std::tie(timestamp, keypointList) = c->getPoseData()->getLatestData();
        RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
        RemoteRobot::synchronizeLocalCloneToTimestamp(
            cameraRobot, getRobotStateComponent(), timestamp.toMicroSeconds());
 
        Keypoint3DMap poseData = getClosestPerson(keypointList);
        int validKeypoints = 0;
        for (auto& pair : poseData)
        {
            if (isKeypointValid(pair.second))
            {
                validKeypoints++;
            }
        }
 
 
        Keypoint3D rwrist = poseData["RWrist"];
        Vector3Ptr rwristPosition = new Vector3(rwrist.x, rwrist.y, rwrist.z);
        bool validTarget = false;
        if (isKeypointValid(rwrist))
        {
            rwristFilter.update(now.toMicroSeconds(), new Variant(rwristPosition));
        }
        auto tmp = rwristFilter.getValue();
        if (tmp)
        {
            Vector3Ptr filteredRWristPosition = VariantPtr::dynamicCast(tmp)->get<Vector3>();
            ARMARX_CHECK_EXPRESSION(filteredRWristPosition);
            FramedPositionPtr globalrWristPosition =
                FramedPosition(
                    filteredRWristPosition->toEigen(), in.getCameraFrameName(), robot->getName())
                    .toGlobal(robot);
            //            ARMARX_INFO << deactivateSpam(1) << VAROUT(*globalrWristPosition);
            if (drawDebugInfos)
            {
#if ENABLE_DEBUG_DRAWER
                this->getDebugDrawerTopic()->setSphereVisu(
                    "HandOverState",
                    "rwrist",
                    globalrWristPosition,
                    DrawColor{1.0f, 0.0f, 0.0f, rwrist.confidence},
                    30);
#else
                vizLayer.add(viz::Sphere("rwrist")
                                 .position(globalrWristPosition->toEigen())
                                 .color(simox::Color::red(255, 255 * rwrist.confidence))
                                 .radius(30));
#endif
                getDebugObserver()->setDebugChannel(
                    "HandOverState",
                    {
                        {"rwristpoint_z", new Variant(rwristPosition->z)},
                        {"rwristpoint_z_filtered", new Variant(filteredRWristPosition->z)},
                    });
            }
 
            rwristPosition = filteredRWristPosition;
            validTarget = true;
        }
        Keypoint3D lwrist = poseData["LWrist"];
        Vector3Ptr lwristPosition = new Vector3(lwrist.x, lwrist.y, lwrist.z);
        if (isKeypointValid(lwrist))
        {
            lwristFilter.update(now.toMicroSeconds(), new Variant(lwristPosition));
        }
        tmp = lwristFilter.getValue();
        if (tmp)
        {
            lwristPosition = VariantPtr::dynamicCast(tmp)->get<Vector3>();
            ARMARX_CHECK_EXPRESSION(lwristPosition);
            FramedPositionPtr globallWristPosition =
                FramedPosition(lwristPosition->toEigen(), in.getCameraFrameName(), robot->getName())
                    .toGlobal(robot);
            //            ARMARX_INFO << deactivateSpam(1) << VAROUT(*globallWristPosition) << VAROUT(robot->getGlobalPose());
            if (drawDebugInfos)
            {
#if ENABLE_DEBUG_DRAWER
                this->getDebugDrawerTopic()->setSphereVisu(
                    "HandOverState",
                    "lwrist",
                    globallWristPosition,
                    DrawColor{0.0f, 1.0f, 0.0f, lwrist.confidence},
                    30);
#else
                vizLayer.add(viz::Sphere("lwrist")
                                 .position(globallWristPosition->toEigen())
                                 .color(simox::Color::green(255, 255 * lwrist.confidence))
                                 .radius(30));
#endif
            }
            validTarget = true;
        }
        //        ARMARX_INFO << deactivateSpam(1) << "Filtered wrist data";
        float leftHandDistance = lwristPosition && isKeypointValid(lwrist)
                                     ? lwristPosition->toEigen().norm()
                                     : std::numeric_limits<float>::max();
        float rightHandDistance = rwristPosition && isKeypointValid(rwrist)
                                      ? rwristPosition->toEigen().norm()
                                      : std::numeric_limits<float>::max();
 
        Vector3Ptr currentHandPosition =
            leftHandDistance < rightHandDistance ? lwristPosition : rwristPosition;
 
        if (drawDebugInfos)
        {
            getDebugObserver()->setDebugChannel(
                "ReachOutToHuman",
                {{"zDiffLShoulder",
                  new Variant(std::abs(poseData["LWrist"].z - poseData["LShoulder"].z))},
                 {"zDiffNeckR", new Variant(std::abs(poseData["RWrist"].z - poseData["Neck"].z))},
                 {"zDiffNeckL", new Variant(std::abs(poseData["LWrist"].z - poseData["Neck"].z))}});
        }
 
 
        Eigen::Vector3f headIkTarget = getHeadIKTarget(poseData);
        FramedPositionPtr globalHeadIkTarget =
            FramedPosition(headIkTarget, in.getCameraFrameName(), robot->getName())
                .toGlobal(cameraRobot);
        globalHeadIkTarget->z = std::max(in.getMinimumGazeZPosition(), globalHeadIkTarget->z);
 
        if (validKeypoints > 3 && !in.getHeadIKNodeSet().empty() && headIkTarget.norm() > 0)
        {
            if (drawDebugInfos)
            {
#if ENABLE_DEBUG_DRAWER
                this->getDebugDrawerTopic()->setSphereVisu("HandOverState",
                                                           "Median",
                                                           globalHeadIkTarget,
                                                           DrawColor{1.0f, 1.0f, 0.0f, 1.0f},
                                                           50);
#else
                vizLayer.add(viz::Sphere("Median")
                                 .position(globalHeadIkTarget->toEigen())
                                 .color(simox::Color::yellow())
                                 .radius(50));
#endif
            }
            headIKTargetFilter.update(now.toMicroSeconds(), new Variant(globalHeadIkTarget));
            auto tmp = headIKTargetFilter.getValue();
            ARMARX_CHECK_EXPRESSION(tmp);
            FramedPositionPtr globalFilteredMedianKeypoint =
                VariantPtr::dynamicCast(tmp)->get<FramedPosition>();
            ARMARX_CHECK_EXPRESSION(globalFilteredMedianKeypoint);
            if ((TimeUtil::GetTime() - startTime).toSecondsDouble() > 1.3)
            {
                if (!lastIKTarget ||
                    (lastIKTarget->toEigen() - globalFilteredMedianKeypoint->toEigen()).norm() >
                        100)
                {
                    getHeadIKUnit()->setHeadTarget(in.getHeadIKNodeSet(),
                                                   globalFilteredMedianKeypoint);
                    lastIKTarget = globalFilteredMedianKeypoint;
                    //                    ARMARX_INFO << deactivateSpam(1) << "HeadIK Target: " << *globalFilteredMedianKeypoint;
                }
            }
        }
        else
        {
#if ENABLE_DEBUG_DRAWER
            this->getDebugDrawerTopic()->removeSphereVisu("HandOverState", "Median");
#else
            // Don't add Median to the layer.
#endif
        }
 
 
        float currentHumanReachDistance =
            headIkTarget.norm() == 0 && currentHandPosition->toEigen().norm() > 100
                ? 0
                : std::abs((currentHandPosition->toEigen() - headIkTarget)(2));
        bool humanIsReachingOut = currentHumanReachDistance > in.getHumanMinimumReachOutDistance();
        //        ARMARX_INFO << deactivateSpam(1) << VAROUT(currentHumanReachDistance) << VAROUT(humanIsReachingOut);
        Eigen::Vector3f currentTargetVec = currentHandPosition->toEigen();
        for (int i = 0; i < 3; ++i)
        {
            targetPositionFilter.at(i).update(now.toMicroSeconds(),
                                              new Variant(currentTargetVec(i)));
        }
        Eigen::Vector3f currentFilteredTargetPositionVec;
        for (int i = 0; i < 3; ++i)
        {
            currentFilteredTargetPositionVec(i) = targetPositionFilter.at(i).getValue()->getFloat();
        }
        //        ARMARX_INFO << deactivateSpam(1) << "Filtered target position ";
        FramedPositionPtr targetPositioninRootFrame = new FramedPosition(
            currentFilteredTargetPositionVec, in.getCameraFrameName(), cameraRobot->getName());
        targetPositioninRootFrame->changeFrame(robot, robot->getRootNode()->getName());
 
        //        FramedPositionPtr localFramedPosition;
        ARMARX_CHECK_EXPRESSION(tcp);
        //        float currentReachDistance = tcp->getTransformationFrom(nodeset->getKinematicRoot()).block<3,1>(0,3).norm();
        FramedPositionPtr targetInKinematicRootFrame =
            new FramedPosition(*targetPositioninRootFrame);
        targetInKinematicRootFrame->changeFrame(robot, kinematicRootName);
        float reachDistanceToTarget = targetInKinematicRootFrame->toEigen().norm();
        ARMARX_INFO << deactivateSpam(1) << VAROUT(reachDistanceToTarget);
        getDebugObserver()->setDebugDatafield(
            "ReachOutToHuman", "reachDistanceToTarget", new Variant(reachDistanceToTarget));
        float reachDistanceToTargetClamped = reachDistanceToTarget;
        if (reachDistanceToTarget > in.getDistanceThresholdSoft() &&
            reachDistanceToTarget < in.getDistanceThreshold())
        {
            Eigen::Vector3f targetPositioninKinematicRootFrameClamped =
                targetInKinematicRootFrame->toEigen().normalized() * in.getDistanceThresholdSoft();
            targetPositioninRootFrame =
                new FramedPosition(targetPositioninKinematicRootFrameClamped,
                                   targetInKinematicRootFrame->frame,
                                   targetInKinematicRootFrame->agent);
            targetPositioninRootFrame->changeFrame(robot, robot->getRootNode()->getName());
            reachDistanceToTargetClamped = targetPositioninKinematicRootFrameClamped.norm();
        }
        if (drawDebugInfos)
        {
            getDebugObserver()->setDebugChannel(
                "ReachOutToHuman",
                {{"targetPositioninRootFrameX", new Variant(targetPositioninRootFrame->x)},
                 {"targetPositioninRootFrameY", new Variant(targetPositioninRootFrame->y)},
                 {"targetPositioninRootFrameZ", new Variant(targetPositioninRootFrame->z)},
                 {"targetPositioninRootNorm",
                  new Variant(targetPositioninRootFrame->toEigen().norm())},
                 {"reachDistanceToTargetClamped", new Variant(reachDistanceToTargetClamped)}});
        }
 
        float distanceToTarget =
            (tcp->getPositionInRootFrame() - targetPositioninRootFrame->toEigen()).norm();
 
        bool returnToIdlePose =
            reachDistanceToTarget > in.getDistanceThreshold() || !validTarget ||
            !humanIsReachingOut || validKeypoints < 5 ||
            timeSinceSinceStart.toMilliSeconds() < in.getMinWaitBeforeReachTime();
        if (returnToIdlePose)
        {
            targetPositioninRootFrame = in.getIdlePosition();
        }
        else
        {
            idlePosition = in.getIdlePosition();
            targetPositioninRootFrame->y -= in.getTargetYOffset();
            targetPositioninRootFrame->z += in.getTargetZOffset();
        }
        //        ARMARX_CHECK_EXPRESSION(lwristPosition);
        //        ARMARX_CHECK_EXPRESSION(lwristPosition);
        ARMARX_CHECK_EXPRESSION(lwristPosition);
 
        FramedPositionPtr globalTargetPosition = targetPositioninRootFrame->toGlobal(robot);
        if (drawDebugInfos)
        {
#if ENABLE_DEBUG_DRAWER
            getDebugDrawerTopic()->setSphereVisu("HandOverState",
                                                 "TargetPosition",
                                                 globalTargetPosition,
                                                 DrawColor{0.0f, 1.0f, 1.0f, 1.0},
                                                 30);
            getDebugDrawerTopic()->setLineVisu("HandOverState",
                                               "ReachOutLine",
                                               globalTargetPosition,
                                               globalHeadIkTarget,
                                               5,
                                               humanIsReachingOut ? DrawColor{0.0f, 1.0f, 0.0f, 1.0}
                                                                  : DrawColor{1.0f, 0, 0, 1.0});
#else
            vizLayer.add(viz::Sphere("TargetPosition")
                             .position(globalTargetPosition->toEigen())
                             .color(simox::Color::cyan())
                             .radius(30));
            vizLayer.add(
                viz::Cylinder("ReachOutLine")
                    .fromTo(globalTargetPosition->toEigen(), globalHeadIkTarget->toEigen())
                    .radius(5)
                    .color(humanIsReachingOut ? simox::Color::green() : simox::Color::red()));
#endif
        }
 
 
        pid.update(duration.toMicroSecondsDouble(),
                   tcp->getPositionInRootFrame(),
                   targetPositioninRootFrame->toEigen());
        Eigen::Vector3f targetVelocityVec = pid.getControlValue();
        if (targetVelocityVec.norm() > maxTCPVelocity)
        {
            targetVelocityVec = targetVelocityVec.normalized() * maxTCPVelocity;
        }
        if (distanceToTarget < considerForceDistanceThreshold)
        {
            targetVelocityVec = Eigen::Vector3f::Zero();
            if (!nulledForceTorqueRef)
            {
                nulledForceTorqueRef = DatafieldRefPtr::dynamicCast(
                    getForceTorqueObserver()->createNulledDatafield(forceTorqueRef));
            }
            FramedDirectionPtr nulledFT =
                nulledForceTorqueRef->getDataField()->get<FramedDirection>();
            float forceMagnitude = nulledFT->toEigen().norm();
            if (drawDebugInfos)
            {
                getDebugObserver()->setDebugChannel(
                    "ReachOutToHuman",
                    {{"ForceX", new Variant(nulledFT->x)},
                     {"ForceZ", new Variant(nulledFT->z)},
                     {"ForceMagnitude", new Variant(forceMagnitude)}});
            }
            forceMagnitudeAvgFilter.update(now.toMicroSeconds(), new Variant(forceMagnitude));
            float forceMagnitudeFiltered = forceMagnitudeAvgFilter.getValue()->getFloat();
            if (drawDebugInfos)
            {
                getDebugObserver()->setDebugDatafield("ReachOutToHuman",
                                                      "forceMagnitudeFiltered",
                                                      new Variant(forceMagnitudeFiltered));
            }
 
            ARMARX_INFO << deactivateSpam(1) << VAROUT(forceMagnitude)
                        << " threshold: " << in.getForceThreshold();
            if (forceMagnitude > in.getForceThreshold())
            {
                ARMARX_IMPORTANT << "ForceThresholdReached: " << forceMagnitude;
                this->emitForceThresholdReached();
                break;
            }
        }
        else
        {
            nulledForceTorqueRef = nullptr;
        }
        //        Eigen::Vector3f filteredTargetVelocityVec;
        //        for (int i = 0; i < 3; ++i)
        //        {
        //            targetVelocityFilter.at(i).update(now.toMicroSeconds(), new Variant(targetVelocityVec(i)));
        //            filteredTargetVelocityVec(i) = targetVelocityFilter.at(i).getValue()->getFloat();
        //        }
        //        ARMARX_INFO << deactivateSpam(1) << VAROUT(*targetPositioninRootFrame) << VAROUT(*lwristPosition) << VAROUT(*rwristPosition);
        //        ARMARX_INFO << deactivateSpam(1) << VAROUT(distanceToTarget) << VAROUT(targetVelocityVec);
        //        Eigen::AngleAxisf aaOriErr(targetOrientation->toEigenQuaternion() * Eigen::Quaternionf(tcp->getPoseInRootFrame().block<3,3>(0,0)).inverse());
        //        Eigen::Vector3f angleVel = aaOriErr.axis() * VirtualRobot::MathTools::angleModPI(aaOriErr.angle());
        //        angleVel *= in.getKpOrientation();
 
        Eigen::VectorXf cv = posController.calculate(
            Pose(targetOrientation->toEigen(), targetPositioninRootFrame->toEigen()).toEigen(),
            VirtualRobot::IKSolver::All);
        ctrl->setTargetVelocity(cv(0), cv(1), cv(2), cv(3), cv(4), cv(5));
 
 
        //        targetVelocity = new FramedDirection(filteredTargetVelocityVec, robot->getRootNode()->getName(), robot->getName());
        //        getTcpControlUnit()->setTCPVelocity(kinematicChainName, "", targetVelocity, nullptr);
 
        //        ctrl->setTargetVelocity(targetVelocityVec(0),
        //                                targetVelocityVec(1),
        //                                targetVelocityVec(2),
        //                                angleVel(0), angleVel(1), angleVel(2));
 
        arviz.commit({vizLayer});
        cycle.waitForCycleDuration();
    }
    ctrl->deactivateAndDeleteController();
    //    c->getOpenPoseEstimation()->stop();
    //    usleep(100000);
    //    c->getOpenPoseEstimation()->stop();
 
    if (in.isObjectIDSet() && in.getObjectID().find("/") != std::string::npos)
    {
        objpose::DetachObjectFromRobotNodeInput input;
        input.objectID = toIce(armarx::ObjectID(in.getObjectID()));
        ARMARX_IMPORTANT << "Detaching object '" << input.objectID << "' in ObjectPoseStorage.";
        objpose::DetachObjectFromRobotNodeOutput result =
            getObjectPoseStorage()->detachObjectFromRobotNode(input);
        (void)result;
    }
    else if (in.isObjectIDSet())
    {
        ARMARX_IMPORTANT << "Not detaching object '" << in.getObjectID()
                         << " because it does not look like an ObjectID.";
    }
    else
    {
        ARMARX_IMPORTANT
            << "Not detaching object in ObjectPoseStorage because 'ObjectID' is not set.";
    }
}
 
//void ReachOutToHumanPose::onBreak()
//{
//    // put your user code for the breaking point here
//    // execution time should be short (<100ms)
//}
 
void
ReachOutToHumanPose::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
#if ENABLE_DEBUG_DRAWER
    getDebugDrawerTopic()->removeLayer("HandOverState");
#endif
    if (in.getStopOpenPoseAfterRun())
    {
        ARMARX_INFO << "Stopping OpenPose...";
        getOpenPoseEstimation()->stop();
    }
    //    getHeadIKUnit()->release();
 
    //    FramedDirectionPtr targetVelocity = new FramedDirection(Eigen::Vector3f::Zero(), getLocalRobot()->getRootNode()->getName(), getLocalRobot()->getName());
    //    getTcpControlUnit()->setTCPVelocity(in.getKinematicChainName(), "", targetVelocity, targetVelocity);
}
 
Eigen::Vector3f
ReachOutToHumanPose::getHeadIKTarget(const Keypoint3DMap& keypointMap)
{
    //LAnkle -- -510.596 -1043.04 3889 0.267718
    //LEar -- 0 0 0 0
    //LElbow -- 0 0 0 0
    //LEye -- 0 0 0 0
    //LHip -- -657.189 -1591.2 3889 0.0951842
    //LKnee -- -556.669 -1228.13 3604 0.281559
    //LShoulder -- 0 0 0 0
    //LWrist -- 0 0 0 0
    //Neck -- 0 0 0 0
    //Nose -- 0 0 0 0
    //RAnkle -- 0 0 0 0
    //REar -- 0 0 0 0
    //RElbow -- 0 0 0 0
    //REye -- 0 0 0 0
    //RHip -- 0 0 0 0
    //RKnee -- 0 0 0 0
    //RShoulder -- 0 0 0 0
    //RWrist -- 0 0 0 0
 
    std::vector<std::pair<std::string, float>> keypointPriorityOrderWithOffset = {
        {"Neck", 20},
        {"RShoulder", 20},
        {"LShoulder", 20},
        {"Nose", 0},
        {"RElbow", 300},
        {"LElbow", 300},
        {"RWrist", 500},
        {"LWrist", 500},
        {"LHip", 600},
        {"RHip", 600},
        {"RKnee", 1100},
        {"LKnee", 1100},
    };
    for (auto& pair : keypointPriorityOrderWithOffset)
    {
        auto it = keypointMap.find(pair.first);
        if (it != keypointMap.end() && isKeypointValid(it->second))
        {
            ARMARX_INFO << "Selected node for head ik target is: " << it->first
                        << deactivateSpam(3, it->first);
            Eigen::Vector3f result(it->second.x, it->second.y - pair.second, it->second.z);
            return result;
        }
    }
 
    return Eigen::Vector3f::Zero();
}
 
Eigen::VectorXf
ReachOutToHumanPose::calculateMedian(const Keypoint3DMap& keypointMap)
{
    if (keypointMap.empty())
    {
        return Eigen::Vector3f::Zero();
    }
    Eigen::MatrixXf keypointPositions(keypointMap.size(), 3);
    std::vector<std::vector<float>> values(3, Ice::FloatSeq(keypointMap.size(), 0.0));
    int i = 0;
    for (auto& pair : keypointMap)
    {
        values.at(0).at(i) = pair.second.x;
        values.at(1).at(i) = pair.second.y;
        values.at(2).at(i) = pair.second.z;
        i++;
    }
 
    std::sort(values.at(0).begin(), values.at(0).end());
    std::sort(values.at(1).begin(), values.at(1).end());
    std::sort(values.at(2).begin(), values.at(2).end());
 
    size_t center = keypointMap.size() / 2;
    return Eigen::Vector3f(
        values.at(0).at(center), values.at(1).at(center), values.at(2).at(center));
}
 
Eigen::VectorXf
ReachOutToHumanPose::calculateCentroid(const Keypoint3DMap& keypointMap)
{
    Eigen::MatrixXf keypointPositions(keypointMap.size(), 3);
    int i = 0;
    for (auto& pair : keypointMap)
    {
        keypointPositions(i, 0) = pair.second.x;
        keypointPositions(i, 1) = pair.second.y;
        keypointPositions(i, 2) = pair.second.z;
        i++;
    }
    return keypointPositions.colwise().mean();
}
 
Keypoint3DMap
ReachOutToHumanPose::getClosestPerson(const std::vector<Keypoint3DMap>& trackingData)
{
    float minDistance = std::numeric_limits<float>::max();
    Keypoint3DMap result;
    for (auto& keypointMap : trackingData)
    {
        if (keypointMap.empty())
        {
            continue;
        }
        int validKeypoints = 0;
        for (auto& pair : keypointMap)
        {
            if (isKeypointValid(pair.second))
            {
                validKeypoints++;
            }
        }
 
        float distance = calculateMedian(keypointMap).norm();
        if (distance < minDistance && validKeypoints > 5)
        {
            minDistance = distance;
            result = keypointMap;
        }
    }
    return result;
}
 
bool
ReachOutToHumanPose::isKeypointValid(const Keypoint3D& point) const
{
    if (point.confidence < in.getMinimumKeypointConfidence())
    {
        return false;
    }
    //if values directly at the camera -> not valid
    return Eigen::Vector3f(point.x, point.y, point.z).norm() > 100;
    //    return !(point.x == 0.0f && point.y == 0.0f && point.z == 0.0f);
}
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr
ReachOutToHumanPose::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new ReachOutToHumanPose(stateData));
}