Pointing.h

Go to the documentation of this file.

#pragma once
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Trajectory.h>
 
#include "Side.h"
 
namespace armarx::exceptions::local
{
    class PointingException : public armarx::LocalException
    {
    };
 
    class PointingFailedException : public PointingException
    {
    };
 
    class PointingAbortedException : public PointingException
    {
    };
} // namespace armarx::exceptions::local
 
namespace armarx::control::pointing::core
{
 
    /**
     * @brief Implementation of a 'Pointing'-gesture.
     *
     * Lets the robot point somewhere by aligning the forearm with the passed target, aligning the
     * wrist, and setting a hand shape.
     *
     * @ingroup Library-pointing
     */
    class Pointing : virtual public armarx::Logging
    {
    public:
        struct Remote
        {
            std::string robotName;
            armarx::viz::Client arviz;
            armarx::armem::client::MemoryNameSystem& memoryNameSystem;
            armarx::TrajectoryPlayerInterfacePrx trajectoryPlayer;
        };
 
        struct Parameters
        {
            /** Side of the arm to point with. */
            Side side;
 
            /** Position to point at. */
            armarx::FramedPosition target;
 
            /** Name of the hand preshape to use. If none is provided, the hand will be ignored. */
            std::optional<std::string> handShape;
 
            /** Maximum velocity */
            double maxVelocity;
 
            /** Maximum acceleration */
            double maxAcceleration;
        };
 
    public:
        Pointing(const Remote& remote);
 
        /**
         * @brief Executes the pointing gesture.
         *
         * Let's the robot point at the passed target. Blocks unitl the target configuration is
         * reached or the execution is aborted with @ref abort.
         *
         * @param params Parameterization of the pointing gesture.
         *
         * @throws armarx::exceptions::local::PointingFailedException Thrown if the pointing gesture
         *         could not be executed.
         * @throws armarx::exceptions::local::PointingAbortedException Thrown when the execution is
         *         aborted with @ref abort() before reaching the target configuration.
         */
        void execute(const Parameters params);
 
        /**
         * @brief Aborts a running execution.
         */
        void abort();
 
 
    private:
        VirtualRobot::RobotPtr getRobot();
 
        Eigen::Vector3f toGlobal(const armarx::FramedPosition&, VirtualRobot::RobotPtr);
 
        void visualizeTarget(const Eigen::Vector3f& target);
 
        armarx::TrajectoryPtr
        computeArmTrajectory(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f target);
 
        void
        addWristTrajectory(VirtualRobot::RobotPtr robot, Side side, armarx::TrajectoryPtr traj);
 
        std::map<std::string, float>
        getShapeJointValues(VirtualRobot::RobotPtr robot, Side side, const std::string& shapeName);
 
        void addHandTrajectory(VirtualRobot::RobotPtr robot,
                               armarx::TrajectoryPtr traj,
                               const std::map<std::string, float>& shapeJointValues);
 
        void playTrajectory(armarx::TrajectoryPtr traj);
 
    private:
        Remote remote_;
        std::atomic_bool aborted;
    };
 
} // namespace armarx::control::pointing::core