Pointing.cpp

Go to the documentation of this file.

#include "Pointing.h"
 
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/time.h>
 
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
 
#include <armarx/control/pointing/constants/constants.h>
 
#include "PointingIK.h"
 
namespace armarx::control::pointing::core
{
    Pointing::Pointing(const Remote& remote) : remote_(remote)
    {
        setTag("Pointing");
    }
 
    void
    Pointing::execute(const Pointing::Parameters params)
    {
        aborted = false;
 
        ARMARX_INFO << "Pointing with " << ((params.side == Side::LEFT) ? "left" : "right")
                    << " arm at " << params.target
                    << (params.handShape ? " with hand shape '" + *params.handShape + "'" : "");
 
        remote_.trajectoryPlayer->stopTrajectoryPlayer();
 
        VirtualRobot::RobotPtr robot = getRobot();
 
        Eigen::Vector3f target = toGlobal(params.target, robot);
        visualizeTarget(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);
            addHandTrajectory(robot, traj, shapeJointValues);
        }
 
        traj = traj->calculateTimeOptimalTrajectory(
            params.maxVelocity, params.maxAcceleration, 0.0, IceUtil::Time::milliSeconds(10));
 
        playTrajectory(traj);
    }
 
    void
    Pointing::abort()
    {
        aborted = true;
    }
 
    VirtualRobot::RobotPtr
    Pointing::getRobot()
    {
        armarx::armem::robot_state::VirtualRobotReader robotReader;
        robotReader.connect(remote_.memoryNameSystem);
 
        auto robot = robotReader.getSynchronizedRobot(remote_.robotName);
        if (not robot)
        {
            ARMARX_ERROR << "Could not get synchronized robot '" << remote_.robotName << "'";
            throw armarx::exceptions::local::PointingFailedException();
        }
        return robot;
    }
 
    Eigen::Vector3f
    Pointing::toGlobal(const FramedPosition& framedPosition, VirtualRobot::RobotPtr robot)
    {
        if (not(framedPosition.frame == armarx::GlobalFrame ||
                robot->hasRobotNode(framedPosition.frame)))
        {
            ARMARX_ERROR << "'" << framedPosition.frame << "' is not a valid frame name";
            throw armarx::exceptions::local::PointingFailedException();
        }
        return framedPosition.toGlobalEigen(robot);
    }
 
    void
    Pointing::visualizeTarget(const Eigen::Vector3f& target)
    {
        auto layer = remote_.arviz.layer(constants::ARVIZ_LAYER_NAME);
        layer.add(armarx::viz::Sphere("Pointing target")
                      .radius(50)
                      .color(armarx::viz::Color::orange())
                      .position(target));
        remote_.arviz.commit(layer);
    }
 
    TrajectoryPtr
    Pointing::computeArmTrajectory(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f target)
    {
        ARMARX_VERBOSE << "Computing trajectory...";
        auto ik = PointingIK(robot, side, target);
 
        if (ik.isTargetReached()) // forearm already aligned with target
        {
            return armarx::TrajectoryPtr(new Trajectory);
        }
 
        do
        {
            if (ik.optimize() < 1e-6)
            {
                ARMARX_WARNING << "PointingIK didn't find a solution";
                throw armarx::exceptions::local::PointingFailedException();
            }
            if (aborted)
            {
                ARMARX_INFO << "Pointing aborted";
                throw armarx::exceptions::local::PointingAbortedException();
            }
        } while (not ik.isTargetReached());
 
        ARMARX_VERBOSE << "Done computing trajectory";
 
        return ik.getTrajectory();
    }
 
    void
    Pointing::addWristTrajectory(VirtualRobot::RobotPtr robot, Side side, TrajectoryPtr traj)
    {
        auto humanMapping = robot->getHumanMapping();
        ARMARX_CHECK(humanMapping) << "Robot '" << robot->getName() << "' has no human mapping";
 
        auto armDescription = (side == LEFT) ? humanMapping->leftArm : humanMapping->rightArm;
        auto wrist = armDescription.segments.wrist.nodeName;
 
        auto nodeSetArm = robot->getRobotNodeSet(armDescription.nodeSet);
 
        // iterate over all joints, starting from the wrist, to move them into a neutral position
        std::for_each(nodeSetArm->begin() + nodeSetArm->getRobotNodeIndex(wrist),
                      nodeSetArm->end(),
                      [traj](VirtualRobot::RobotNodePtr robotNode) {
                          traj->addDimension({robotNode->getJointValue(), 0.0},
                                             Ice::DoubleSeq(),
                                             robotNode->getName());
                      });
    }
 
    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,
                                const std::map<std::string, float>& shapeJointValues)
    {
        for (const auto& [jointName, targetValue] : shapeJointValues)
        {
            traj->addDimension({robot->getRobotNode(jointName)->getJointValue(), targetValue},
                               Ice::DoubleSeq(),
                               jointName);
        }
    }
 
    void
    Pointing::playTrajectory(armarx::TrajectoryPtr traj)
    {
        ARMARX_INFO << "Playing trajectory (" << traj->getTimeLength() << "s)";
        remote_.trajectoryPlayer->loadJointTraj(traj);
        remote_.trajectoryPlayer->startTrajectoryPlayer();
 
 
        using namespace armarx::core::time;
        DateTime waitUnil = DateTime::Now() + Duration::Seconds(traj->getTimeLength());
 
        Clock clock;
        while (DateTime::Now() < waitUnil)
        {
            if (aborted)
            {
                ARMARX_INFO << "Pointing aborted. Stopping trajectory";
                remote_.trajectoryPlayer->stopTrajectoryPlayer();
                throw armarx::exceptions::local::PointingAbortedException();
            }
            clock.waitFor(Duration::MilliSeconds(10));
        }
        ARMARX_INFO << "Pointing target reached";
    }
 
} // namespace armarx::control::pointing::core