DeprecatedNJointPeriodicTSDMPCompliantController.h

Go to the documentation of this file.

 
#pragma once
 
#include <ArmarXCore/core/time/CycleUtil.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
 
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
#include <RobotAPI/libraries/core/Trajectory.h>
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
#include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
#include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
 
 
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
    namespace tsvmp = armarx::control::deprecated_njoint_mp_controller::tsvmp;
 
    TYPEDEF_PTRS_HANDLE(DeprecatedNJointPeriodicTSDMPCompliantController);
    TYPEDEF_PTRS_HANDLE(DeprecatedNJointPeriodicTSDMPCompliantControllerControlData);
 
    class DeprecatedNJointPeriodicTSDMPCompliantControllerControlData
    {
    public:
        float targetForce;
        Eigen::Vector6f targetTSVel;
        Eigen::Matrix4f targetTSPose;
        Eigen::VectorXf targetNullSpaceJointValues;
    };
 
    /**
     * @brief The DeprecatedNJointPeriodicTSDMPCompliantController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class DeprecatedNJointPeriodicTSDMPCompliantController :
        public NJointControllerWithTripleBuffer<DeprecatedNJointPeriodicTSDMPCompliantControllerControlData>,
        public DeprecatedNJointPeriodicTSDMPCompliantControllerInterface
    {
    public:
        using ConfigPtrT = DeprecatedNJointPeriodicTSDMPCompliantControllerConfigPtr;
        DeprecatedNJointPeriodicTSDMPCompliantController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
 
        // NJointControllerInterface interface
        std::string getClassName(const Ice::Current&) const;
 
        // NJointController interface
 
        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
 
        // DeprecatedNJointPeriodicTSDMPCompliantControllerInterface interface
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
        void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&) override;
        bool isFinished(const Ice::Current&) override;
 
        void setSpeed(Ice::Double times, const Ice::Current&) override;
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
        void setAmplitude(Ice::Double amp, const Ice::Current&) override;
        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&) override;
 
        void stopDMP(const Ice::Current&) override;
        void resumeDMP(const Ice::Current&) override;
        void pauseDMP(const Ice::Current&) override;
        void resetDMP(const Ice::Current&) override;
 
        void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&) override;
        double getCanVal(const Ice::Current&) override;
 
        void setKpImpedance(const Eigen::Vector6f& kpImpedance, const Ice::Current&) override;
        void setKdImpedance(const Eigen::Vector6f& kdImpedance, const Ice::Current&) override;
 
        Eigen::Vector6f getFilteredForceTorque(Eigen::Vector3f& forceBaseline, Eigen::Vector3f& torqueBaseline, Eigen::Vector6f& handCompensatedFT);
        Eigen::Vector6f getTCPGravityCompensation(Eigen::Matrix4f& currentPose, float tcpMass);
        std::string getKinematicChainName(const Ice::Current&);
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
        void onInitNJointController();
        void onDisconnectNJointController();
        void controllerRun();
 
    private:
        //        RTController::ControllerInfo info;
        std::string kinematic_chain;
        // ---------------------- Buffer: control parameter ----------------------
        struct CtrlParams
        {
            Eigen::Vector6f kpImpedance;
            Eigen::Vector6f kdImpedance;
 
            Eigen::VectorXf knull;
            Eigen::VectorXf dnull;
 
            Eigen::Vector4f pidForce;
 
            float tcpMass;
            Eigen::Vector3f tcpCoMInForceSensorFrame;
        };
        WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
 
        // ---------------------- Buffer: mp 2 debug data ----------------------
        struct DebugData
        {
            StringFloatDictionary latestTargetVelocities;
            StringFloatDictionary currentPose;
            double currentCanVal;
            double mpcFactor;
            double error;
            double phaseStop;
            double posError;
            double oriError;
            double deltaT;
        };
        TripleBuffer<DebugData> debugMPBuffer;
 
        // ---------------------- Buffer: rt 2 debug data ----------------------
        struct DebugRTData
        {
            Eigen::Matrix4f targetPose;
            Eigen::Vector3f filteredForce;
            Eigen::Vector3f reactForce;
            Eigen::VectorXf targetVel;
            Eigen::Matrix4f currentPose;
            Eigen::Vector3f targetVelInTool;
            bool isPhaseStop;
        };
        TripleBuffer<DebugRTData> debugRTBuffer;
 
        // ---------------------- Buffer: rt 2 MP thread data ----------------------
        struct RT2MPData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPose;
            Eigen::VectorXf currentTwist;
            bool isPhaseStop;
        };
        TripleBuffer<RT2MPData> rt2MPBuffer;
 
        // ---------------------- Buffer: rt 2 interface data ----------------------
        struct RT2InterfaceData
        {
            Eigen::Matrix4f currentTcpPose;
            float waitTimeForCalibration;
        };
        TripleBuffer<RT2InterfaceData> rt2InterfaceBuffer;
 
 
 
        // ---------------------- low-level control variables ----------------------
        // device
        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
        std::vector<ControlTarget1DoFActuatorTorque*> targets;
 
        PIDControllerPtr forcePID;
 
        VirtualRobot::DifferentialIKPtr ik;
        VirtualRobot::RobotNodePtr tcp;
 
        mutable MutexType mpMutex;
        tsvmp::TaskSpaceDMPControllerPtr dmpCtrl;
        DeprecatedNJointPeriodicTSDMPCompliantControllerConfigPtr cfg;
        PeriodicTask<DeprecatedNJointPeriodicTSDMPCompliantController>::pointer_type controllerTask;
 
        //        // phaseStop parameters
        //        double phaseL;
        //        double phaseK;
        //        double phaseDist0;
        //        double phaseDist1;
        //        double posToOriRatio;
 
 
        float torqueLimit;
        int numOfJoints;
        std::vector<std::string> jointNames;
 
        Eigen::VectorXf nullSpaceJointsVec;
        //        std::atomic_bool useNullSpaceJointDMP;
        //        bool isNullSpaceJointDMPLearned;
 
        std::atomic_bool rtFirstRun = true;
        std::atomic_bool rtReady = false;
        std::atomic_bool mpRunning = false;
 
        Eigen::Matrix4f previousTargetPose;
        Eigen::VectorXf qvel_filtered;
 
        /// force torque sensor
        const SensorValueForceTorque* forceSensor;
        Eigen::Vector3f forceOffset;
        Eigen::Vector3f torqueOffset;
        Eigen::Vector3f filteredForce;
        Eigen::Vector3f filteredTorque;
        Eigen::Vector3f filteredForceInRoot;
        Eigen::Vector3f filteredTorqueInRoot;
        std::atomic<float> timeForCalibration;
 
        std::atomic_bool enableTCPGravityCompensation;
        Eigen::Matrix4f forceFrameInTCP = Eigen::Matrix4f::Identity();
        Eigen::Vector3f tcpCoMInTCPFrame;
 
        /// Tool setup
        Eigen::Matrix4f toolFrameInRoot;
        Eigen::Matrix3f toolOriInHand;
 
 
        Eigen::Vector2f lastPosition;
        double changeTimer;
 
    };
 
} // namespace armarx