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 "HandOverGroupStatechartContext.h"
 
#include <ArmarXCore/core/ManagedIceObject.h>
 
//#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
#include <ArmarXCore/observers/filters/AverageFilter.h>
 
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
 
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h>
#include <RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
#include <RobotAPI/components/ArViz/Client/elements/RobotHand.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();
        }
        //        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;
        }
        float distance = calculateMedian(keypointMap).norm();
        if (distance < minDistance)
        {
            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));
}