NJointTaskSpaceAdaptiveDMPController.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>
 
// control
#include <armarx/control/deprecated_njoint_mp_controller/adaptive/ControllerInterface.h>
#include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
 
 
namespace armarx::control::deprecated_njoint_mp_controller::adaptive
{
    namespace tsvmp = armarx::control::deprecated_njoint_mp_controller::tsvmp;
 
    TYPEDEF_PTRS_HANDLE(NJointTaskSpaceAdaptiveDMPController);
    TYPEDEF_PTRS_HANDLE(NJointTaskSpaceAdaptiveDMPControllerControlData);
 
    class NJointTaskSpaceAdaptiveDMPControllerControlData
    {
    public:
        Eigen::VectorXf targetVel;
        Eigen::Matrix4f targetPose;
        Eigen::VectorXf desiredNullSpaceJointValues;
        double canVal;
        double mpcFactor;
    };
 
 
 
    /**
     * @brief The NJointTaskSpaceAdaptiveDMPController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointTaskSpaceAdaptiveDMPController :
        public NJointControllerWithTripleBuffer<NJointTaskSpaceAdaptiveDMPControllerControlData>,
        public NJointTaskSpaceAdaptiveDMPControllerInterface
    {
    public:
        using ConfigPtrT = NJointTaskSpaceAdaptiveDMPControllerConfigPtr;
        NJointTaskSpaceAdaptiveDMPController(const RobotUnitPtr& robotUnit, 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);
 
        // NJointTaskSpaceAdaptiveDMPControllerInterface interface
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
        bool isFinished(const Ice::Current&)
        {
            return finished;
        }
 
        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
 
        void learnJointDMPFromFiles(const std::string& fileName, const Ice::FloatSeq& currentJVS, const Ice::Current&);
        void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
        void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
 
        Ice::Double getVirtualTime(const Ice::Current&)
        {
            return dmpCtrl->canVal;
        }
 
        void stopDMP(const Ice::Current&);
        void resumeDMP(const Ice::Current&);
        void resetDMP(const Ice::Current&);
 
        void setKdImpedance(const Ice::FloatSeq& dampings, const Ice::Current&);
        void setKpImpedance(const Ice::FloatSeq& stiffness, const Ice::Current&);
 
        void setKdNull(const Ice::FloatSeq& dnull, const Ice::Current&);
        void setKpNull(const Ice::FloatSeq& knull, const Ice::Current&);
        Ice::FloatSeq getForce(const Ice::Current&);
        Ice::FloatSeq getVelocityInMM(const Ice::Current&);
        void setCanVal(double canVal, const Ice::Current&);
 
        void removeAllViaPoints(const Ice::Current&);
        void setUseNullSpaceJointDMP(bool useJointDMP, const Ice::Current&);
        void setDefaultJointValues(const Ice::FloatSeq& desiredJointVals, const Ice::Current&);
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
        void onInitNJointController();
        void onDisconnectNJointController();
        void controllerRun();
 
    private:
        struct DebugBufferData
        {
            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 currentPose_x;
            float currentPose_y;
            float currentPose_z;
            float currentPose_qw;
            float currentPose_qx;
            float currentPose_qy;
            float currentPose_qz;
 
            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 impedanceKp_x;
            float impedanceKp_y;
            float impedanceKp_z;
            float impedanceKp_rx;
            float impedanceKp_ry;
            float impedanceKp_rz;
 
            float forceInRoot_x;
            float forceInRoot_y;
            float forceInRoot_z;
            //            float torqueInRoot_x;
            //            float torqueInRoot_y;
            //            float torqueInRoot_z;
 
            float vel_x;
            float vel_y;
            float vel_z;
 
            float deltaT;
 
        };
 
        TripleBuffer<DebugBufferData> debugOutputData;
 
        struct NJointTaskSpaceAdaptiveDMPControllerSensorData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPose;
            Eigen::VectorXf currentTwist;
        };
        TripleBuffer<NJointTaskSpaceAdaptiveDMPControllerSensorData> controllerSensorData;
 
        struct NJointTaskSpaceAdaptiveDMPControllerInterfaceData
        {
            Eigen::Matrix4f currentTcpPose;
            Eigen::VectorXf currentVel;
            Eigen::Vector3f currentForce;
 
        };
 
        TripleBuffer<NJointTaskSpaceAdaptiveDMPControllerInterfaceData> interfaceData;
 
 
        struct Inferface2rtData
        {
            Eigen::VectorXf KpImpedance;
            Eigen::VectorXf KdImpedance;
            Eigen::VectorXf Knull;
            Eigen::VectorXf Dnull;
        };
        TripleBuffer<Inferface2rtData> interface2rtBuffer;
 
        struct Interface2CtrlData
        {
            double canVal;
        };
        TripleBuffer<Interface2CtrlData> interface2CtrlBuffer;
 
 
        DMP::Vec<DMP::DMPState> currentJointState;
        DMP::UMIDMPPtr nullSpaceJointDMPPtr;
 
        tsvmp::TaskSpaceDMPControllerPtr dmpCtrl;
 
        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
        std::vector<ControlTarget1DoFActuatorTorque*> targets;
 
        // velocity ik controller parameters
        // dmp parameters
        double timeDuration;
        bool finished;
        VirtualRobot::RobotNodeSetPtr rns;
 
        // phaseStop parameters
        double phaseL;
        double phaseK;
        double phaseDist0;
        double phaseDist1;
        double posToOriRatio;
 
 
        NJointTaskSpaceAdaptiveDMPControllerConfigPtr cfg;
        VirtualRobot::DifferentialIKPtr ik;
        VirtualRobot::RobotNodePtr tcp;
 
        float torqueLimit;
 
        Eigen::Vector3f kpos;
        Eigen::Vector3f kori;
        Eigen::Vector3f dpos;
        Eigen::Vector3f dori;
        Eigen::VectorXf knull;
        Eigen::VectorXf dnull;
        int numOfJoints;
 
        bool useNullSpaceJointDMP;
        bool isNullSpaceJointDMPLearned;
 
 
        Eigen::VectorXf defaultNullSpaceJointValues;
        std::vector<std::string> jointNames;
        mutable MutexType controllerMutex;
        PeriodicTask<NJointTaskSpaceAdaptiveDMPController>::pointer_type controllerTask;
        bool firstRun;
        bool started = false;
        bool stopped = false;
        Eigen::Vector3f forceOffset;
 
        Eigen::Matrix4f oldPose;
        const SensorValueForceTorque* forceSensor;
        Eigen::Vector3f filteredForce;
 
        mutable MutexType interfaceDataMutex;
        mutable MutexType int2ctrlMutex;
 
        // NJointController interface
    protected:
        void rtPreActivateController();
    };
 
} // namespace armarx