ICartesianControl.h

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __I_CARTESIAN_CONTROL__
#define __I_CARTESIAN_CONTROL__
 
#include <algorithm> // std::all_of
#include <map>
#include <vector>
 
#include <yarp/os/Vocab.h>
#include <yarp/dev/ReturnValue.h>
 
#include <ICartesianSolver.h>
 
namespace roboticslab
{
 
class ICartesianControl
{
public:
    enum class Vocabs
    {
        OK = yarp::os::createVocab32('o','k'),             
        FAILED = yarp::os::createVocab32('f','a','i','l'), 
        SET = yarp::os::createVocab32('s','e','t'),        
        GET = yarp::os::createVocab32('g','e','t'),        
        NOT_SET = yarp::os::createVocab32('n','s','e','t') 
    };
 
    enum class RPC
    {
        STATE = yarp::os::createVocab32('s','t','a','t'), 
        INV = yarp::os::createVocab32('i','n','v'),       
        MOVEJ = yarp::os::createVocab32('m','o','v','j'), 
        MOVEL = yarp::os::createVocab32('m','o','v','l'), 
        MOVEV = yarp::os::createVocab32('m','o','v','v'), 
        GCMP = yarp::os::createVocab32('g','c','m','p'),  
        FORCE = yarp::os::createVocab32('f','o','r','c'), 
        STOP = yarp::os::createVocab32('s','t','o','p'),  
        TOOL = yarp::os::createVocab32('t','o','o','l'),  
        ACT = yarp::os::createVocab32('a','c','t')        
    };
 
    enum class Streaming
    {
        POSE = yarp::os::createVocab32('p','o','s','e'),  
        TWIST = yarp::os::createVocab32('t','w','s','t'), 
        WRENCH = yarp::os::createVocab32('w','r','n','c') 
    };
 
    enum class Mode
    {
        NONE = yarp::os::createVocab32('n','c','t','l'),  
        MOVEJ = yarp::os::createVocab32('m','o','v','j'), 
        MOVEL = yarp::os::createVocab32('m','o','v','l'), 
        MOVEV = yarp::os::createVocab32('m','o','v','v'), 
        GCMP = yarp::os::createVocab32('g','c','m','p'),  
        FORCE = yarp::os::createVocab32('f','o','r','c')  
    };
 
    enum class Actuator
    {
        NONE = yarp::os::createVocab32('a','c','n'),      
        CLOSE = yarp::os::createVocab32('a','c','c','g'), 
        OPEN = yarp::os::createVocab32('a','c','o','g'),  
        STOP = yarp::os::createVocab32('a','c','s','g'),  
        GENERIC = yarp::os::createVocab32('a','c','g')    
    };
 
    enum class Config
    {
        GAIN = yarp::os::createVocab32('c','p','c','g'),          
        TRAJ_DURATION = yarp::os::createVocab32('c','p','t','d'), 
        TRAJ_REF_SPD = yarp::os::createVocab32('c','p','t','s'),  
        TRAJ_REF_ACC = yarp::os::createVocab32('c','p','t','a'),  
        CMC_PERIOD = yarp::os::createVocab32('c','p','c','p'),    
        FRAME = yarp::os::createVocab32('c','p','f'),             
        STREAMING_CMD = yarp::os::createVocab32('c','p','s','c')  
    };
 
    struct ControllerState
    {
        std::vector<double> x; 
        Mode mode;             
        double timestamp;      
        double duration;       
        float progress;        
        bool success;          
    };
 
    virtual ~ICartesianControl() = default;
 
    //--------------------- RPC commands ---------------------
 
    virtual yarp::dev::ReturnValue getState(ControllerState & state) = 0;
 
    virtual yarp::dev::ReturnValue solvePose(const std::vector<double> & xd, std::vector<double> & q) = 0;
 
    virtual yarp::dev::ReturnValue moveJoint(const std::vector<double> & xd) = 0;
 
    virtual yarp::dev::ReturnValue moveLinear(const std::vector<double> & xd) = 0;
 
    virtual yarp::dev::ReturnValue moveVelocity(const std::vector<double> & xdotd) = 0;
 
    virtual yarp::dev::ReturnValue gravityCompensation() = 0;
 
    virtual yarp::dev::ReturnValue forceControl(const std::vector<double> & fd) = 0;
 
    virtual yarp::dev::ReturnValue stopControl() = 0;
 
    virtual yarp::dev::ReturnValue changeTool(const std::vector<double> & x) = 0;
 
    virtual yarp::dev::ReturnValue actuateTool(Actuator command) = 0;
 
    //--------------------- Streaming commands ---------------------
 
    virtual void pose(const std::vector<double> & x) = 0;
 
    virtual void twist(const std::vector<double> & xdot) = 0;
 
    virtual void wrench(const std::vector<double> & w) = 0;
 
    //--------------------- Configuration accessors ---------------------
 
    virtual yarp::dev::ReturnValue setParameter(Config vocab, double value) = 0;
 
    virtual yarp::dev::ReturnValue getParameter(Config vocab, double * value) = 0;
 
    virtual yarp::dev::ReturnValue setParameters(const std::map<Config, double> & params) = 0;
 
    virtual yarp::dev::ReturnValue getParameters(std::map<Config, double> & params) = 0;
 
#ifndef SWIG_PREPROCESSOR_SHOULD_SKIP_THIS
    [[deprecated("use `ICartesianControl::getState` instead")]]
    virtual bool stat(std::vector<double> & x, int * state = nullptr, double * timestamp = nullptr)
    {
        ControllerState controllerState;
        auto ret = getState(controllerState);
        x = controllerState.x;
        if (state != nullptr) *state = static_cast<int>(controllerState.mode);
        if (timestamp != nullptr) *timestamp = controllerState.timestamp;
        return ret;
    }
 
    [[deprecated("use `ICartesianControl::solvePose` instead")]]
    virtual bool inv(const std::vector<double> & xd, std::vector<double> & q)
    { return solvePose(xd, q); }
 
    [[deprecated("use `ICartesianControl::moveJoint` instead")]]
    virtual bool movj(const std::vector<double> & xd)
    { return moveJoint(xd); }
 
    [[deprecated("use `ICartesianControl::getState and moveJoint` instead")]]
    virtual bool relj(const std::vector<double> & xd)
    { return false; }
 
    [[deprecated("use `ICartesianControl::moveLinear` instead")]]
    virtual bool movl(const std::vector<double> & xd)
    { return moveLinear(xd); }
 
    [[deprecated("use `ICartesianControl::moveVelocity` instead")]]
    virtual bool movv(const std::vector<double> & xdotd)
    { return moveVelocity(xdotd); }
 
    [[deprecated("use `ICartesianControl::gravityCompensation` instead")]]
    virtual bool gcmp()
    { return gravityCompensation(); }
 
    [[deprecated("use `ICartesianControl::forceControl` instead")]]
    virtual bool forc(const std::vector<double> & fd)
    { return forceControl(fd); }
 
    [[deprecated("use `ICartesianControl::getState` instead")]]
    virtual bool wait(double timeout = 0.0)
    { return false; }
 
    [[deprecated("use `ICartesianControl::changeTool` instead")]]
    virtual bool tool(const std::vector<double> & x)
    { return changeTool(x); }
 
    [[deprecated("use `ICartesianControl::Actuator` signature instead")]]
    virtual bool act(int command)
    { return actuateTool(static_cast<Actuator>(command)); }
 
    [[deprecated("use `ICartesianControl::Config` signature instead")]]
    virtual bool setParameter(int vocab, double value)
    { return setParameter(static_cast<Config>(vocab), value); }
 
    [[deprecated("use `ICartesianControl::Config` signature instead")]]
    virtual bool getParameter(int vocab, double * value)
    { return getParameter(static_cast<Config>(vocab), value); }
 
    [[deprecated("use `ICartesianControl::Config` signature instead")]]
    virtual bool setParameters(const std::map<int, double> & params)
    { return std::all_of(params.begin(), params.end(), [this](const auto & kv) { return setParameter(static_cast<Config>(kv.first), kv.second); }); }
 
    [[deprecated("use `ICartesianControl::Config` signature instead")]]
    virtual bool getParameters(std::map<int, double> & params)
    { return std::all_of(params.begin(), params.end(), [this](auto & kv) { return getParameter(static_cast<Config>(kv.first), &kv.second); }); }
#endif // SWIG_PREPROCESSOR_SHOULD_SKIP_THIS
};
 
} // namespace roboticslab
 
 
#ifndef SWIG_PREPROCESSOR_SHOULD_SKIP_THIS
[[deprecated("use `ICartesianControl::Vocabs::OK` instead")]]
constexpr auto VOCAB_CC_OK = static_cast<int>(roboticslab::ICartesianControl::Vocabs::OK);
 
[[deprecated("use `ICartesianControl::Vocabs::FAILED` instead")]]
constexpr auto VOCAB_CC_FAILED = static_cast<int>(roboticslab::ICartesianControl::Vocabs::FAILED);
 
[[deprecated("use `ICartesianControl::Vocabs::SET` instead")]]
constexpr auto VOCAB_CC_SET = static_cast<int>(roboticslab::ICartesianControl::Vocabs::SET);
 
[[deprecated("use `ICartesianControl::Vocabs::GET` instead")]]
constexpr auto VOCAB_CC_GET = static_cast<int>(roboticslab::ICartesianControl::Vocabs::GET);
 
[[deprecated("use `ICartesianControl::Vocabs::NOT_SET` instead")]]
constexpr auto VOCAB_CC_NOT_SET = static_cast<int>(roboticslab::ICartesianControl::Vocabs::NOT_SET);
 
[[deprecated("use `ICartesianControl::RPC::STATE` instead")]]
constexpr auto VOCAB_CC_STAT = static_cast<int>(roboticslab::ICartesianControl::RPC::STATE);
 
[[deprecated("use `ICartesianControl::RPC::INV` instead")]]
constexpr auto VOCAB_CC_INV = static_cast<int>(roboticslab::ICartesianControl::RPC::INV);
 
[[deprecated("use `ICartesianControl::RPC::MOVEJ` instead")]]
constexpr auto VOCAB_CC_MOVJ = static_cast<int>(roboticslab::ICartesianControl::RPC::MOVEJ);
 
[[deprecated("use `ICartesianControl::RPC::MOVJ` instead")]]
constexpr auto VOCAB_CC_RELJ = 0;
 
[[deprecated("use `ICartesianControl::RPC::MOVEL` instead")]]
constexpr auto VOCAB_CC_MOVL = static_cast<int>(roboticslab::ICartesianControl::RPC::MOVEL);
 
[[deprecated("use `ICartesianControl::RPC::MOVEV` instead")]]
constexpr auto VOCAB_CC_MOVV = static_cast<int>(roboticslab::ICartesianControl::RPC::MOVEV);
 
[[deprecated("use `ICartesianControl::RPC::GCMP` instead")]]
constexpr auto VOCAB_CC_GCMP = static_cast<int>(roboticslab::ICartesianControl::RPC::GCMP);
 
[[deprecated("use `ICartesianControl::RPC::FORCE` instead")]]
constexpr auto VOCAB_CC_FORC = static_cast<int>(roboticslab::ICartesianControl::RPC::FORCE);
 
[[deprecated("use `ICartesianControl::RPC::STOP` instead")]]
constexpr auto VOCAB_CC_STOP = static_cast<int>(roboticslab::ICartesianControl::RPC::STOP);
 
[[deprecated("use `ICartesianControl::RPC::STATE` instead")]]
constexpr auto VOCAB_CC_WAIT = 0;
 
[[deprecated("use `ICartesianControl::RPC::TOOL` instead")]]
constexpr auto VOCAB_CC_TOOL = static_cast<int>(roboticslab::ICartesianControl::RPC::TOOL);
 
[[deprecated("use `ICartesianControl::RPC::ACT` instead")]]
constexpr auto VOCAB_CC_ACT = static_cast<int>(roboticslab::ICartesianControl::RPC::ACT);
 
[[deprecated("use `ICartesianControl::Streaming::POSE` instead")]]
constexpr auto VOCAB_CC_POSE = static_cast<int>(roboticslab::ICartesianControl::Streaming::POSE);
 
[[deprecated("use `ICartesianControl::Streaming::TWIST` instead")]]
constexpr auto VOCAB_CC_TWIST = static_cast<int>(roboticslab::ICartesianControl::Streaming::TWIST);
 
[[deprecated("use `ICartesianControl::Streaming::WRENCH` instead")]]
constexpr auto VOCAB_CC_WRENCH = static_cast<int>(roboticslab::ICartesianControl::Streaming::WRENCH);
 
[[deprecated("use `ICartesianControl::State::NONE` instead")]]
constexpr auto VOCAB_CC_NOT_CONTROLLING = static_cast<int>(roboticslab::ICartesianControl::Mode::NONE);
 
[[deprecated("use `ICartesianControl::State::MOVEJ` instead")]]
constexpr auto VOCAB_CC_MOVJ_CONTROLLING = static_cast<int>(roboticslab::ICartesianControl::Mode::MOVEJ);
 
[[deprecated("use `ICartesianControl::State::MOVEL` instead")]]
constexpr auto VOCAB_CC_MOVL_CONTROLLING = static_cast<int>(roboticslab::ICartesianControl::Mode::MOVEL);
 
[[deprecated("use `ICartesianControl::State::MOVEV` instead")]]
constexpr auto VOCAB_CC_MOVV_CONTROLLING = static_cast<int>(roboticslab::ICartesianControl::Mode::MOVEV);
 
[[deprecated("use `ICartesianControl::State::GCMP` instead")]]
constexpr auto VOCAB_CC_GCMP_CONTROLLING = static_cast<int>(roboticslab::ICartesianControl::Mode::GCMP);
 
[[deprecated("use `ICartesianControl::State::FORCE` instead")]]
constexpr auto VOCAB_CC_FORC_CONTROLLING = static_cast<int>(roboticslab::ICartesianControl::Mode::FORCE);
 
[[deprecated("use `ICartesianControl::Actuator::NONE` instead")]]
constexpr auto VOCAB_CC_ACTUATOR_NONE = static_cast<int>(roboticslab::ICartesianControl::Actuator::NONE);
 
[[deprecated("use `ICartesianControl::Actuator::CLOSE` instead")]]
constexpr auto VOCAB_CC_ACTUATOR_CLOSE_GRIPPER = static_cast<int>(roboticslab::ICartesianControl::Actuator::CLOSE);
 
[[deprecated("use `ICartesianControl::Actuator::OPEN` instead")]]
constexpr auto VOCAB_CC_ACTUATOR_OPEN_GRIPPER = static_cast<int>(roboticslab::ICartesianControl::Actuator::OPEN);
 
[[deprecated("use `ICartesianControl::Actuator::STOP` instead")]]
constexpr auto VOCAB_CC_ACTUATOR_STOP_GRIPPER = static_cast<int>(roboticslab::ICartesianControl::Actuator::STOP);
 
[[deprecated("use `ICartesianControl::Actuator::GENERIC` instead")]]
constexpr auto VOCAB_CC_ACTUATOR_GENERIC = static_cast<int>(roboticslab::ICartesianControl::Actuator::GENERIC);
 
[[deprecated("unused")]]
constexpr auto VOCAB_CC_CONFIG_PARAMS = 0;
 
[[deprecated("use `ICartesianControl::Config::GAIN` instead")]]
constexpr auto VOCAB_CC_CONFIG_GAIN = static_cast<int>(roboticslab::ICartesianControl::Config::GAIN);
 
[[deprecated("use `ICartesianControl::Config::TRAJ_DURATION` instead")]]
constexpr auto VOCAB_CC_CONFIG_TRAJ_DURATION = static_cast<int>(roboticslab::ICartesianControl::Config::TRAJ_DURATION);
 
[[deprecated("use `ICartesianControl::Config::TRAJ_REF_SPD` instead")]]
constexpr auto VOCAB_CC_CONFIG_TRAJ_REF_SPD = static_cast<int>(roboticslab::ICartesianControl::Config::TRAJ_REF_SPD);
 
[[deprecated("use `ICartesianControl::Config::TRAJ_REF_ACC` instead")]]
constexpr auto VOCAB_CC_CONFIG_TRAJ_REF_ACC = static_cast<int>(roboticslab::ICartesianControl::Config::TRAJ_REF_ACC);
 
[[deprecated("use `ICartesianControl::Config::CMC_PERIOD` instead")]]
constexpr auto VOCAB_CC_CONFIG_CMC_PERIOD = static_cast<int>(roboticslab::ICartesianControl::Config::CMC_PERIOD);
 
[[deprecated("unused")]]
constexpr auto VOCAB_CC_CONFIG_WAIT_PERIOD = 0;
 
[[deprecated("use `ICartesianControl::Config::FRAME` instead")]]
constexpr auto VOCAB_CC_CONFIG_FRAME = static_cast<int>(roboticslab::ICartesianControl::Config::FRAME);
 
[[deprecated("use `ICartesianControl::Config::STREAMING_CMD` instead")]]
constexpr auto VOCAB_CC_CONFIG_STREAMING_CMD = static_cast<int>(roboticslab::ICartesianControl::Config::STREAMING_CMD);
#endif // SWIG_PREPROCESSOR_SHOULD_SKIP_THIS
 
#endif // __I_CARTESIAN_CONTROL__