ControlBoardRemapperHelpers.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms of the
 * BSD-3-Clause license. See the accompanying LICENSE file for details.
 */
 
#ifndef YARP_DEV_CONTROLBOARDREMAPPER_CONTROLBOARDREMAPPERHELPERS_H
#define YARP_DEV_CONTROLBOARDREMAPPER_CONTROLBOARDREMAPPERHELPERS_H
 
 
#include <yarp/os/PortablePair.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/Time.h>
#include <yarp/os/Network.h>
#include <yarp/os/Stamp.h>
#include <yarp/os/Vocab.h>
 
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/IInteractionMode.h>
#include <yarp/dev/IControlLimits2.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/IPreciselyTimed.h>
 
 
#include <yarp/sig/Vector.h>
 
#include <mutex>
#include <string>
#include <vector>
 
 
#ifdef MSVC
    #pragma warning(disable:4355)
#endif
 
/*
 * Helper class for the ControlBoardRemapper.
 * It contains all the data structure related
 * to a given controlboard used by the remapper.
 */
class RemappedSubControlBoard
{
public:
    std::string id;
 
    yarp::dev::PolyDriver            *subdevice;
    yarp::dev::IPidControl           *pid;
    yarp::dev::IPositionControl      *pos;
    yarp::dev::IVelocityControl      *vel;
    yarp::dev::IEncodersTimed        *iJntEnc;
    yarp::dev::IMotorEncoders        *iMotEnc;
    yarp::dev::IAmplifierControl     *amp;
    yarp::dev::IControlLimits        *lim;
    yarp::dev::IControlCalibration   *calib;
    yarp::dev::IRemoteCalibrator     *remcalib;
    yarp::dev::IPreciselyTimed       *iTimed;
    yarp::dev::ITorqueControl        *iTorque;
    yarp::dev::IImpedanceControl     *iImpedance;
    yarp::dev::IControlMode          *iMode;
    yarp::dev::IAxisInfo             *info;
    yarp::dev::IPositionDirect       *posDir;
    yarp::dev::IInteractionMode      *iInteract;
    yarp::dev::IMotor                *imotor;
    yarp::dev::IRemoteVariables      *iVar;
    yarp::dev::IPWMControl           *iPwm;
    yarp::dev::ICurrentControl       *iCurr;
 
    RemappedSubControlBoard();
 
    bool attach(yarp::dev::PolyDriver *d, const std::string &id);
    void detach();
 
    inline void setVerbose(bool _verbose) {_subDevVerbose = _verbose; }
 
    bool isAttached()
    { return attachedF; }
 
private:
    bool _subDevVerbose;
    bool attachedF;
};
 
class RemappedAxis
{
public:
    size_t subControlBoardIndex;
 
    size_t axisIndexInSubControlBoard;
};
 
class RemappedControlBoards
{
public:
    std::vector<RemappedSubControlBoard> subdevices;
 
    std::vector<RemappedAxis> lut;
 
    inline RemappedSubControlBoard* getSubControlBoard(size_t i)
    {
        return &(subdevices[i]);
    }
 
    size_t getNrOfSubControlBoards() const
    {
        return subdevices.size();
    }
 
    size_t getNrOfRemappedAxes() const
    {
        return lut.size();
    }
};
 
class ControlBoardRemapperBuffers
{
public:
    std::mutex mutex;
    std::vector<int> controlBoardModes;
    std::vector<double> dummyBuffer;
    yarp::os::Stamp stamp;
 
};
 
class ControlBoardSubControlBoardAxesDecomposition
{
public:
    bool configure(const RemappedControlBoards & remappedControlBoards);
 
    void fillSubControlBoardBuffersFromCompleteJointVector(const double * full, const RemappedControlBoards & remappedControlBoards);
 
 
    void fillSubControlBoardBuffersFromCompleteJointVector(const int * full,
                                                            const RemappedControlBoards & remappedControlBoards);
 
    void fillSubControlBoardBuffersFromCompleteJointVector(const yarp::dev::InteractionModeEnum * full,
                                                            const RemappedControlBoards & remappedControlBoards);
 
    void fillCompleteJointVectorFromSubControlBoardBuffers(double * full, const RemappedControlBoards & remappedControlBoards);
 
    void fillCompleteJointVectorFromSubControlBoardBuffers(int * full,
                                                            const RemappedControlBoards & remappedControlBoards);
 
    void fillCompleteJointVectorFromSubControlBoardBuffers(yarp::dev::InteractionModeEnum * full,
                                                           const RemappedControlBoards & remappedControlBoards);
 
 
    std::mutex mutex;
 
    // Buffer to be used in MultiJoint version of the
    int m_nrOfControlledAxesInRemappedCtrlBrd;
    std::vector<int> m_nJointsInSubControlBoard;
    std::vector< std::vector<int> > m_jointsInSubControlBoard;
 
 
    std::vector< std::vector<double> > m_bufferForSubControlBoard;
    std::vector< std::vector<int>    > m_bufferForSubControlBoardControlModes;
    std::vector< std::vector<yarp::dev::InteractionModeEnum>  > m_bufferForSubControlBoardInteractionModes;
 
    std::vector<int> m_counterForControlBoard;
};
 
class ControlBoardArbitraryAxesDecomposition
{
    void createListOfJointsDecomposition(const int n_joints, const int *joints, const RemappedControlBoards & remappedControlBoards);
 
public:
    bool configure(const RemappedControlBoards & remappedControlBoards);
 
 
    void fillSubControlBoardBuffersFromArbitraryJointVector(const double * arbitraryVec,
                                                            const int n_joints,
                                                            const int *joints,
                                                            const RemappedControlBoards & remappedControlBoards);
 
    void fillSubControlBoardBuffersFromArbitraryJointVector(const int * arbitraryVec,
                                                            const int n_joints,
                                                            const int *joints,
                                                            const RemappedControlBoards & remappedControlBoards);
 
    void fillSubControlBoardBuffersFromArbitraryJointVector(const yarp::dev::InteractionModeEnum * arbitraryVec,
                                                            const int n_joints,
                                                            const int *joints,
                                                            const RemappedControlBoards & remappedControlBoards);
 
    void resizeSubControlBoardBuffers(const int n_joints,
                                      const int *joints,
                                      const RemappedControlBoards & remappedControlBoards);
 
    void fillArbitraryJointVectorFromSubControlBoardBuffers(double * arbitraryVec,
                                                            const int n_joints, const int *joints,
                                                            const RemappedControlBoards & remappedControlBoards);
 
    void fillArbitraryJointVectorFromSubControlBoardBuffers(int * arbitraryVec,
                                                            const int n_joints, const int *joints,
                                                            const RemappedControlBoards & remappedControlBoards);
 
    void fillArbitraryJointVectorFromSubControlBoardBuffers(yarp::dev::InteractionModeEnum * arbitraryVec,
                                                            const int n_joints, const int *joints,
                                                            const RemappedControlBoards & remappedControlBoards);
 
    std::mutex mutex;
 
    // Total number of axes in the remapped controlboard
    int m_nrOfControlledAxesInRemappedCtrlBrd;
 
    // Vector of size getNrOfSubControlBoards
    std::vector<int> m_nJointsInSubControlBoard;
    std::vector< std::vector<int> > m_jointsInSubControlBoard;
 
    // Buffers for the control board (the size of each one should
    // match the size of m_nJointsInSubControlBoard[ctrlBoard] and
    // the size of m_jointsInSubControlBoard[ctrlBoard].size()
    std::vector< std::vector<double> > m_bufferForSubControlBoard;
    std::vector< std::vector<int>    > m_bufferForSubControlBoardControlModes;
    std::vector< std::vector<yarp::dev::InteractionModeEnum>  > m_bufferForSubControlBoardInteractionModes;
 
 
    // Counter used when converting a full vector to
    // the subcontrolboard buffers
    std::vector<int> m_counterForControlBoard;
};
 
#endif  // YARP_DEV_CONTROLBOARDREMAPPER_CONTROLBOARDREMAPPERHELPERS_H