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 <mutex>
#include <utility>
#include <vector>
 
#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"
 
namespace roboticslab
{
 
class BasicCartesianControl : public yarp::dev::DeviceDriver,
                              public yarp::os::PeriodicThread,
                              public ICartesianControl
{
public:
    BasicCartesianControl() : yarp::os::PeriodicThread(1.0, yarp::os::PeriodicThreadClock::Absolute)
    {}
 
    // -- ICartesianControl declarations. Implementation in ICartesianControlImpl.cpp--
    bool stat(std::vector<double> & x, int * state = nullptr, double * timestamp = nullptr) override;
    bool inv(const std::vector<double> & xd, std::vector<double> & q) override;
    bool movj(const std::vector<double> & xd) override;
    bool relj(const std::vector<double> & xd) override;
    bool movl(const std::vector<double> & xd) override;
    bool movv(const std::vector<double> & xdotd) override;
    bool gcmp() override;
    bool forc(const std::vector<double> & fd) override;
    bool stopControl() override;
    bool wait(double timeout) override;
    bool tool(const std::vector<double> & x) override;
    bool act(int command) override;
    void pose(const std::vector<double> & x) override;
    void twist(const std::vector<double> & xdot) override;
    void wrench(const std::vector<double> & w) override;
    bool setParameter(int vocab, double value) override;
    bool getParameter(int vocab, double * value) override;
    bool setParameters(const std::map<int, double> & params) override;
    bool getParameters(std::map<int, 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;
    };
 
    int getCurrentState() const;
    void setCurrentState(int value);
 
    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);
    bool checkControlModes(int mode);
    bool setControlModes(int mode);
    bool presetStreamingCommand(int 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 handleMovlVel(const std::vector<double> & q, const StateWatcher & watcher);
    void handleMovlPosd(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;
    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};
 
    ICartesianSolver::reference_frame referenceFrame;
 
    double gain;
    double duration; // [s]
 
    int cmcPeriodMs;
    int waitPeriodMs;
    int numJoints;
    int currentState;
    int streamingCommand;
 
    bool usePosdMovl;
    bool enableFailFast;
 
    mutable std::mutex stateMutex;
 
    std::vector<double> vmoStored;
 
    double movementStartTime;
 
    std::vector<std::unique_ptr<KDL::Trajectory>> trajectories;
 
    std::vector<double> fd;
 
    int encoderErrors {0};
    std::atomic_bool cmcSuccess;
 
    std::vector<double> qMin, qMax;
    std::vector<double> qdotMin, qdotMax;
    std::vector<double> qRefSpeeds;
};
 
} // namespace roboticslab
 
#endif // __BASIC_CARTESIAN_CONTROL_HPP__