BasicCartesianControl.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __BASIC_CARTESIAN_CONTROL_HPP__
#define __BASIC_CARTESIAN_CONTROL_HPP__
 
#include <atomic>
#include <functional>
#include <memory>
#include <utility>
#include <vector>
 
#include <yarp/conf/version.h>
 
#include <yarp/os/PeriodicThread.h>
 
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/IPreciselyTimed.h>
 
#include <kdl/trajectory.hpp>
 
#include "ICartesianSolver.h"
#include "ICartesianControl.h"
#include "BasicCartesianControl_ParamsParser.h"
 
class BasicCartesianControl : public yarp::dev::DeviceDriver,
                              public yarp::os::PeriodicThread,
                              public roboticslab::ICartesianControl,
                              public BasicCartesianControl_ParamsParser
{
public:
    BasicCartesianControl() : yarp::os::PeriodicThread(1.0, yarp::os::PeriodicThreadClock::Absolute)
    {}
 
    // -- ICartesianControl declarations. Implementation in ICartesianControlImpl.cpp --
 
    // RPC commands
    yarp::dev::ReturnValue getState(roboticslab::ICartesianControl::ControllerState & state) override;
    yarp::dev::ReturnValue solvePose(const std::vector<double> & xd, std::vector<double> & q) override;
    yarp::dev::ReturnValue moveJoint(const std::vector<double> & xd) override;
    yarp::dev::ReturnValue moveLinear(const std::vector<double> & xd) override;
    yarp::dev::ReturnValue moveVelocity(const std::vector<double> & xdotd) override;
    yarp::dev::ReturnValue gravityCompensation() override;
    yarp::dev::ReturnValue forceControl(const std::vector<double> & fd) override;
    yarp::dev::ReturnValue stopControl() override;
    yarp::dev::ReturnValue changeTool(const std::vector<double> & x) override;
    yarp::dev::ReturnValue actuateTool(roboticslab::ICartesianControl::Actuator command) override;
 
    // streaming commands
    void pose(const std::vector<double> & x) override;
    void twist(const std::vector<double> & xdot) override;
    void wrench(const std::vector<double> & w) override;
 
    // configuration getters/setters
    yarp::dev::ReturnValue setParameter(roboticslab::ICartesianControl::Config vocab, double value) override;
    yarp::dev::ReturnValue getParameter(roboticslab::ICartesianControl::Config vocab, double * value) override;
    yarp::dev::ReturnValue setParameters(const std::map<roboticslab::ICartesianControl::Config, double> & params) override;
    yarp::dev::ReturnValue getParameters(std::map<roboticslab::ICartesianControl::Config, double> & params) override;
 
    // -------- PeriodicThread declarations. Implementation in PeriodicThreadImpl.cpp --------
    void run() override;
 
    // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp --------
    bool open(yarp::os::Searchable & config) override;
    bool close() override;
 
private:
    class StateWatcher
    {
    public:
        template <typename Fn>
        StateWatcher(Fn && fn) : handler(std::move(fn))
        {}
 
        ~StateWatcher()
        { if (handler) handler(); }
 
        void suppress() const
        { handler = nullptr; }
 
    private:
        mutable std::function<void()> handler;
    };
 
    double getTimestamp();
 
    bool checkJointLimits(const std::vector<double> & q);
    bool checkJointLimits(const std::vector<double> & q, const std::vector<double> & qdot);
    bool checkJointVelocities(const std::vector<double> & qdot);
    bool doFailFastChecks(const std::vector<double> & initialQ);
#if YARP_VERSION_COMPARE(>=, 4,0,0)
    bool checkControlModes(yarp::dev::ControlModeEnum mode);
    bool setControlModes(yarp::dev::SelectableControlModeEnum mode);
#else
    bool checkControlModes(int mode);
    bool setControlModes(int mode);
#endif
    bool presetStreamingCommand(roboticslab::ICartesianControl::Streaming command);
    bool computeIsocronousSpeeds(const std::vector<double> & q, const std::vector<double> & qd, std::vector<double> & qdot);
 
    void handleMovj(const std::vector<double> & q, const StateWatcher & watcher);
    void handleMovelVel(const std::vector<double> & q, const StateWatcher & watcher);
    void handleMovelPosd(const std::vector<double> & q, const StateWatcher & watcher);
    void handleMovv(const std::vector<double> & q, const StateWatcher & watcher);
    void handleGcmp(const std::vector<double> & q, const StateWatcher & watcher);
    void handleForc(const std::vector<double> & q, const std::vector<double> & qdot, const std::vector<double> & qdotdot, const StateWatcher & watcher);
 
    yarp::dev::PolyDriver solverDevice;
    roboticslab::ICartesianSolver * iCartesianSolver {nullptr};
 
    yarp::dev::PolyDriver robotDevice;
    yarp::dev::IControlMode * iControlMode {nullptr};
    yarp::dev::IEncoders * iEncoders {nullptr};
    yarp::dev::IPositionControl * iPositionControl {nullptr};
    yarp::dev::IPositionDirect *  iPositionDirect {nullptr};
    yarp::dev::IPreciselyTimed * iPreciselyTimed {nullptr};
    yarp::dev::ITorqueControl * iTorqueControl {nullptr};
    yarp::dev::IVelocityControl * iVelocityControl {nullptr};
 
    roboticslab::ICartesianSolver::Frame referenceFrame;
 
#if YARP_VERSION_COMPARE(>=, 4,0,0)
    std::size_t numJoints {0};
#else
    int numJoints {0};
#endif
    std::atomic<roboticslab::ICartesianControl::Mode> currentMode {roboticslab::ICartesianControl::Mode::NONE};
    roboticslab::ICartesianControl::Streaming streamingCommand {roboticslab::ICartesianControl::Streaming::POSE};
 
    std::vector<double> vmoStored;
 
    std::vector<std::unique_ptr<KDL::Trajectory>> trajectories;
 
    std::vector<double> fd;
 
    int encoderErrors {0};
    double trajectoryStartTime {0.0};
 
    std::atomic<double> maxTrajectoryDuration {0.0};
    std::atomic<bool> cmcSuccess {true};
    std::atomic<float> cmcProgress {1.0f}; // only meaningful for MOVEJ and MOVEL
 
    std::vector<double> qMin, qMax;
    std::vector<double> qdotMin, qdotMax;
    std::vector<double> qRefSpeeds;
};
 
#endif // __BASIC_CARTESIAN_CONTROL_HPP__