NJointCartesianNaturalPositionController.h

Go to the documentation of this file.

#pragma once
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
#include <RobotAPI/libraries/core/CartesianNaturalPositionController.h>
 
namespace armarx
{
 
    TYPEDEF_PTRS_HANDLE(NJointCartesianNaturalPositionController);
 
    /**
     * @brief The NJointCartesianNaturalPositionController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointCartesianNaturalPositionController :
        public NJointController,
        public NJointCartesianNaturalPositionControllerInterface
    {
    public:
        using ConfigPtrT = NJointCartesianNaturalPositionControllerConfigPtr;
        NJointCartesianNaturalPositionController(
            RobotUnit* robotUnit,
            const NJointCartesianNaturalPositionControllerConfigPtr& config,
            const VirtualRobot::RobotPtr&);
 
        // NJointControllerInterface interface
        std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
 
        // NJointController interface
        void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                   const IceUtil::Time& timeSinceLastIteration) override;
 
        void onPublish(const SensorAndControl&,
                       const DebugDrawerInterfacePrx&,
                       const DebugObserverInterfacePrx&) override;
 
    public:
        //bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
        //bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
 
        void setConfig(const CartesianNaturalPositionControllerConfig& cfg,
                       const Ice::Current& = Ice::emptyCurrent) override;
        void setTarget(const Eigen::Matrix4f& tcpTarget,
                       const Eigen::Vector3f& elbowTarget,
                       bool setOrientation,
                       const Ice::Current& = Ice::emptyCurrent) override;
        void setTargetFeedForward(const Eigen::Matrix4f& tcpTarget,
                                  const Eigen::Vector3f& elbowTarget,
                                  bool setOrientation,
                                  const Eigen::Vector6f& ffVel,
                                  const Ice::Current&) override;
        void setFeedForwardVelocity(const Eigen::Vector6f& vel, const Ice::Current&) override;
        void clearFeedForwardVelocity(const Ice::Current&) override;
        void setNullspaceTarget(const Ice::FloatSeq& nullspaceTarget, const Ice::Current&) override;
        void clearNullspaceTarget(const Ice::Current&) override;
        void setNullspaceControlEnabled(bool enabled, const Ice::Current&) override;
 
        FTSensorValue getCurrentFTValue(const Ice::Current&) override;
        FTSensorValue getAverageFTValue(const Ice::Current&) override;
        void setFTOffset(const FTSensorValue& offset, const Ice::Current&) override;
        void resetFTOffset(const Ice::Current&) override;
        void setFTLimit(float force, float torque, const Ice::Current&) override;
        void clearFTLimit(const Ice::Current&) override;
        void setFakeFTValue(const FTSensorValue& ftValue, const Ice::Current&) override;
        void clearFakeFTValue(const Ice::Current&) override;
        bool isAtForceLimit(const Ice::Current&) override;
 
        void stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
 
        void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
                                             const Ice::Current& = Ice::emptyCurrent) override;
        void resetVisualizationRobotGlobalPose(const Ice::Current& = Ice::emptyCurrent) override;
 
    protected:
        void rtPreActivateController() override;
        void rtPostDeactivateController() override;
 
        //structs
    private:
        struct TB_Target
        {
            Eigen::Matrix4f tcpTarget;
            Eigen::Vector3f elbowTarget;
            bool setOrientation;
            bool updated = false;
        };
 
        struct TB_FFvel
        {
            Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
            bool use = false;
            bool updated = false;
        };
 
        struct TB_Cfg
        {
            CartesianNaturalPositionControllerConfig cfg;
            bool updated = false;
        };
 
        struct TB_NT
        {
            std::vector<float> nullspaceTarget;
            bool clearRequested = false;
            bool updated = false;
        };
 
        struct TB_FT
        {
            Eigen::Vector3f force = Eigen::Vector3f::Zero();
            Eigen::Vector3f torque = Eigen::Vector3f::Zero();
            bool updated = false;
        };
 
        struct TB_FTlimit
        {
            float force = -1;
            float torque = -1;
            bool updated = false;
        };
 
        struct TB_FTfake
        {
            Eigen::Vector3f force = Eigen::Vector3f::Zero();
            Eigen::Vector3f torque = Eigen::Vector3f::Zero();
            bool use = false;
            bool updated = false;
        };
 
        struct FTval
        {
            Eigen::Vector3f force = Eigen::Vector3f::Zero();
            Eigen::Vector3f torque = Eigen::Vector3f::Zero();
        };
 
        struct RtToNonRtData
        {
            Eigen::Matrix4f rootPose = Eigen::Matrix4f::Identity();
            Eigen::Matrix4f tcp = Eigen::Matrix4f::Identity();
            Eigen::Matrix4f tcpTarg = Eigen::Matrix4f::Identity();
            Eigen::Matrix4f elb = Eigen::Matrix4f::Identity();
            Eigen::Matrix4f elbTarg = Eigen::Matrix4f::Identity();
 
            FTval currentFT;
            FTval averageFT;
 
            //Eigen::Matrix4f ftInRoot = Eigen::Matrix4f::Identity();
            //Eigen::Vector3f ftUsed = Eigen::Vector3f::Zero();
        };
 
        struct PublishData
        {
            std::atomic<float> errorPos{0};
            std::atomic<float> errorOri{0};
            std::atomic<float> errorPosMax{0};
            std::atomic<float> errorOriMax{0};
            std::atomic<float> tcpPosVel{0};
            std::atomic<float> tcpOriVel{0};
            std::atomic<float> elbPosVel{0};
            //std::atomic<bool>  targetReached;
        };
 
        //data
    private:
        void setNullVelocity();
        FTSensorValue frost(const FTval& ft);
 
        using Ctrl = CartesianNaturalPositionController;
 
        //rt data
        VirtualRobot::RobotPtr _rtRobot;
        VirtualRobot::RobotNodeSetPtr _rtRns;
        VirtualRobot::RobotNodePtr _rtTcp;
        VirtualRobot::RobotNodePtr _rtElbow;
        VirtualRobot::RobotNodePtr _rtFT;
        VirtualRobot::RobotNodePtr _rtRobotRoot;
 
        std::unique_ptr<Ctrl> _rtPosController;
        bool _rtSetOrientation = true;
        Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
        TB_FFvel _rtFFvel;
        int _rtFFvelMaxAgeMS;
        long _rtFFvelLastUpdateMS = 0;
 
        std::vector<const float*> _rtVelSensors;
        std::vector<float*> _rtVelTargets;
 
        const Eigen::Vector3f* _rtForceSensor = nullptr;
        const Eigen::Vector3f* _rtTorqueSensor = nullptr;
 
        std::vector<FTval> _rtFTHistory;
        size_t _rtFTHistoryIndex = 0;
 
        TB_FT _rtFTOffset;
        TB_FTlimit _rtFTlimit;
        TB_FTfake _rtFTfake;
 
        //Eigen::Vector3f                 _rtForceOffset;
 
        //float                           _rtForceThreshold                      = -1;
        bool _rtStopConditionReached = false;
 
        std::atomic_bool _nullspaceControlEnabled{true};
 
        //flags
        //std::atomic_bool                _publishIsAtTarget{false};
        std::atomic_bool _publishIsAtForceLimit{false};
        //std::atomic_bool                _setFTOffset{false};
        std::atomic_bool _stopNowRequested{false};
 
        //buffers
        mutable std::recursive_mutex _mutexSetTripBuf;
        //TripleBuffer<CtrlData>          _tripBufPosCtrl;
        TripleBuffer<TB_Target> _tripBufTarget;
        TripleBuffer<TB_NT> _tripBufNullspaceTarget;
        TripleBuffer<TB_FFvel> _tripBufFFvel;
        TripleBuffer<TB_Cfg> _tripBufCfg;
        TripleBuffer<TB_FT> _tripBufFToffset;
        TripleBuffer<TB_FTlimit> _tripBufFTlimit;
        TripleBuffer<TB_FTfake> _tripBufFTfake;
 
 
        mutable std::recursive_mutex _tripRt2NonRtMutex;
        TripleBuffer<RtToNonRtData> _tripRt2NonRt;
 
        mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
        TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
 
        //publish data
        PublishData _publish;
 
        //std::atomic<float>              _publishForceThreshold{0};
        //std::atomic<float>              _publishForceCurrent{0};
        //std::atomic_bool                _publishForceThresholdInRobotRootZ{0};
    };
} // namespace armarx