DeprecatedNJointTaskSpaceAdmittanceDMPController.h

Go to the documentation of this file.

 
#pragma once
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <ArmarXCore/core/time/CycleUtil.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 <dmp/representation/dmp/umidmp.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(DeprecatedNJointTaskSpaceAdmittanceDMPController);
    TYPEDEF_PTRS_HANDLE(DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData);
 
    class DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData
    {
    public:
        Eigen::Vector6f targetTSVel;
        Eigen::Matrix4f targetTSPose;
        Eigen::VectorXf targetNullSpaceJointValues;
        double canVal;
        double mpcFactor;
    };
 
 
 
    /**
     * @brief The DeprecatedNJointTaskSpaceAdmittanceDMPController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class DeprecatedNJointTaskSpaceAdmittanceDMPController :
        public NJointControllerWithTripleBuffer<DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData>,
        public DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface
    {
    public:
        using ConfigPtrT = DeprecatedNJointTaskSpaceAdmittanceDMPControllerConfigPtr;
        DeprecatedNJointTaskSpaceAdmittanceDMPController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
 
        // NJointControllerInterface interface
        std::string getClassName(const Ice::Current&) const override;
 
        // NJointController interface
        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
 
        // DeprecatedNJointTaskSpaceAdmittanceDMPControllerInterface interface
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
        bool isFinished(const Ice::Current&) override;
 
        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
 
        void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&) override;
        void runDMP(const Ice::DoubleSeq& goals, const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
        void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
 
        Ice::Double getVirtualTime(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 setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
        DoubleSeqSeq getMPWeights(const Ice::Current&) override;
 
        void removeAllViaPoints(const Ice::Current&) override;
 
        void setKpImpedance(const Eigen::Vector6f& kpImpedance, const Ice::Current&) override;
        void setKdImpedance(const Eigen::Vector6f& kdImpedance, const Ice::Current&) override;
 
        void setKpAdmittance(const Eigen::Vector6f& kpAdmittance, const Ice::Current&) override;
        void setKdAdmittance(const Eigen::Vector6f& kdAdmittance, const Ice::Current&) override;
        void setKmAdmittance(const Eigen::Vector6f& kmAdmittance, const Ice::Current&) override;
 
        void setTCPMass(Ice::Float mass, const Ice::Current&) override;
        void setTCPCoMInFTFrame(const Eigen::Vector3f& com, const Ice::Current&) override;
 
        void setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&) override;
        void setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&) override;
        void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
        void setTargetNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
        void setPotentialForceBaseline(const Eigen::Vector3f&, const Eigen::Vector3f&, const Ice::Current&) override;
 
        std::string getDMPAsString(const Ice::Current&) override;
        Ice::DoubleSeq createDMPFromString(const std::string&, const Ice::Current&) override;
        Ice::Double getCanVal(const Ice::Current&) override;
        void setStartAndGoals(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, const Ice::Current& ice) override;
 
        Eigen::Vector6f getFilteredForceTorque(Eigen::Vector3f& forceBaseline, Eigen::Vector3f& torqueBaseline, Eigen::Vector6f& handCompensatedFT);
        Eigen::Vector6f getTCPGravityCompensation(Eigen::Matrix4f& currentPose, float tcpMass);
        //        RTController::ControllerInfo getControllerInfo(const Ice::Current& ice) override;
        std::string getKinematicChainName(const Ice::Current&) override;
 
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
 
        void onInitNJointController() override;
        void onDisconnectNJointController() override;
        void controllerRun();
 
    private:
//        RTController::ControllerInfo info;
        std::string kinematic_chain;
        // ---------------------- Buffer: rt 2 debug data ----------------------
        struct DebugRTData
        {
            double currentCanVal;
            double mpcfactor;
            float targetPose_x;
            float targetPose_y;
            float targetPose_z;
            float targetPose_qw;
            float targetPose_qx;
            float targetPose_qy;
            float targetPose_qz;
 
            float virtualPose_x;
            float virtualPose_y;
            float virtualPose_z;
            float virtualPose_qw;
            float virtualPose_qx;
            float virtualPose_qy;
            float virtualPose_qz;
 
            float currentPose_x;
            float currentPose_y;
            float currentPose_z;
            float currentPose_qw;
            float currentPose_qx;
            float currentPose_qy;
            float currentPose_qz;
 
            float currentKpos_x;
            float currentKpos_y;
            float currentKpos_z;
            float currentKori_x;
            float currentKori_y;
            float currentKori_z;
            float currentKnull_x;
            float currentKnull_y;
            float currentKnull_z;
 
            float currentDpos_x;
            float currentDpos_y;
            float currentDpos_z;
            float currentDori_x;
            float currentDori_y;
            float currentDori_z;
            float currentDnull_x;
            float currentDnull_y;
            float currentDnull_z;
 
            StringFloatDictionary desired_torques;
            StringFloatDictionary desired_nullspaceJoint;
            float forceDesired_x;
            float forceDesired_y;
            float forceDesired_z;
            float forceDesired_rx;
            float forceDesired_ry;
            float forceDesired_rz;
 
            float ft_InRoot_x;
            float ft_InRoot_y;
            float ft_InRoot_z;
            float ft_InRoot_rx;
            float ft_InRoot_ry;
            float ft_InRoot_rz;
 
            float ft_FilteredInRoot_x;
            float ft_FilteredInRoot_y;
            float ft_FilteredInRoot_z;
            float ft_FilteredInRoot_rx;
            float ft_FilteredInRoot_ry;
            float ft_FilteredInRoot_rz;
 
            float deltaT;
 
            Eigen::Vector6f kmAdmittance;
            Eigen::Vector6f gravityCompensation;
            Eigen::Vector3f forceBaseline;
            Eigen::Vector3f torqueBaseline;
            Eigen::Vector3f forceOffset;
            Eigen::Vector3f torqueOffset;
        };
        WriteBufferedTripleBuffer<DebugRTData> debugRTBuffer;
 
        // ---------------------- Buffer: rt 2 MP thread data ----------------------
        struct RT2MPData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPose;
            Eigen::VectorXf currentTwist;
        };
        WriteBufferedTripleBuffer<RT2MPData> rt2MPBuffer;
 
        // ---------------------- Buffer: rt 2 interface data ----------------------
        struct RT2InterfaceData
        {
            Eigen::Matrix4f currentTcpPose;
        };
        WriteBufferedTripleBuffer<RT2InterfaceData> rt2InterfaceBuffer;
 
        // ---------------------- Buffer: control parameter ----------------------
        struct CtrlParams
        {
            Eigen::Vector6f kpImpedance;
            Eigen::Vector6f kdImpedance;
 
            Eigen::Vector6f kpAdmittance;
            Eigen::Vector6f kdAdmittance;
            Eigen::Vector6f kmAdmittance;
 
            Eigen::VectorXf knull;
            Eigen::VectorXf dnull;
 
            Eigen::Vector3f forceBaseline;
            Eigen::Vector3f torqueBaseline;
 
            float tcpMass;
            Eigen::Vector3f tcpCoMInForceSensorFrame;
        };
        WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
 
 
        // ---------------------- Admittance Control Variables ----------------------
 
        Eigen::Vector6f virtualAcc;
        Eigen::Vector6f virtualVel;
        Eigen::Matrix4f virtualPose;
 
        // ---------------------- low-level control variables ----------------------
        // device
        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
        std::vector<ControlTarget1DoFActuatorTorque*> targets;
 
        VirtualRobot::DifferentialIKPtr ik;
        VirtualRobot::RobotNodePtr tcp;
 
        // ---------------------- MP variables ----------------------
        DMP::Vec<DMP::DMPState> currentJointState;
        DMP::UMIDMPPtr nullSpaceJointDMPPtr;
 
        mutable MutexType mpMutex;
        tsvmp::TaskSpaceDMPControllerPtr dmpCtrl;
        DeprecatedNJointTaskSpaceAdmittanceDMPControllerConfigPtr cfg;
        PeriodicTask<DeprecatedNJointTaskSpaceAdmittanceDMPController>::pointer_type controllerTask;
 
        // phaseStop parameters
        double phaseL;
        double phaseK;
        double phaseDist0;
        double phaseDist1;
        double posToOriRatio;
 
        float torqueLimit;
        int numOfJoints;
        std::vector<std::string> jointNames;
 
        std::atomic_bool useNullSpaceJointDMP;
        bool isNullSpaceJointDMPLearned;
 
        std::atomic_bool rtFirstRun = true;
        std::atomic_bool rtReady = false;
        std::atomic_bool mpRunning = false;
        std::atomic_bool mpFinished = 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;
 
    protected:
        void rtPreActivateController() override;
    };
 
} // namespace armarx