DeprecatedNJointTaskSpaceImpedanceDMPController.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(DeprecatedNJointTaskSpaceImpedanceDMPController);
    TYPEDEF_PTRS_HANDLE(DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData);
 
    class DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData
    {
    public:
        Eigen::VectorXf targetVel;
        Eigen::Matrix4f targetPose;
        Eigen::VectorXf desiredNullSpaceJointValues;
        double canVal;
        double mpcFactor;
    };
 
 
 
    /**
     * @brief The DeprecatedNJointTaskSpaceImpedanceDMPController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class DeprecatedNJointTaskSpaceImpedanceDMPController :
        public NJointControllerWithTripleBuffer<DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData>,
        public DeprecatedNJointTaskSpaceImpedanceDMPControllerInterface
    {
    public:
        using ConfigPtrT = DeprecatedNJointTaskSpaceImpedanceDMPControllerConfigPtr;
        DeprecatedNJointTaskSpaceImpedanceDMPController(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;
 
        // DeprecatedNJointTaskSpaceImpedanceDMPControllerInterface interface
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
        bool isFinished(const Ice::Current&) override
        {
            return finished;
        }
 
        bool isDMPRunning(const Ice::Current&) override
        {
            return started;
        }
 
        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
        {
            return dmpCtrl->canVal;
        }
 
        void stopDMP(const Ice::Current&) override;
        void resumeDMP(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 setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
        void setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
        void setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
        void setAngularVelocityKp(const Eigen::Vector3f& kp, 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 setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
 
        void enableForceStop(const Ice::Current&) override
        {
            useForceStop = true;
        }
        void disableForceStop(const Ice::Current&) override
        {
            useForceStop = false;
        }
 
        void setForceThreshold(const Eigen::Vector3f& f, const Ice::Current& current) override
        {
            forceThreshold.getWriteBuffer() = f;
            forceThreshold.commitWrite();
        }
        std::string getDMPAsString(const Ice::Current&) override;
        Ice::DoubleSeq createDMPFromString(const std::string&, const Ice::Current&) override;
        Ice::Double getCanVal(const Ice::Current&);
 
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
 
        void onInitNJointController() override;
        void onDisconnectNJointController() override;
        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;
 
            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;
 
            Eigen::Vector3f filteredForceInRoot;
 
            float deltaT;
 
 
 
        };
 
        WriteBufferedTripleBuffer<DebugBufferData> debugOutputData;
 
        struct DeprecatedNJointTaskSpaceImpedanceDMPControllerSensorData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPose;
            Eigen::VectorXf currentTwist;
        };
        WriteBufferedTripleBuffer<DeprecatedNJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
 
        struct DeprecatedNJointTaskSpaceImpedanceDMPControllerInterfaceData
        {
            Eigen::Matrix4f currentTcpPose;
        };
 
        WriteBufferedTripleBuffer<DeprecatedNJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
 
        struct CtrlParams
        {
            Eigen::Vector3f kpos;
            Eigen::Vector3f dpos;
            Eigen::Vector3f kori;
            Eigen::Vector3f dori;
            Eigen::VectorXf knull;
            Eigen::VectorXf dnull;
        };
 
        WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
 
 
 
        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;
 
 
        DeprecatedNJointTaskSpaceImpedanceDMPControllerConfigPtr 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;
 
        std::atomic_bool useNullSpaceJointDMP;
        bool isNullSpaceJointDMPLearned;
 
 
        WriteBufferedTripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
        std::vector<std::string> jointNames;
        mutable MutexType controllerMutex;
        PeriodicTask<DeprecatedNJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
        bool firstRun;
        bool started = false;
        bool stopped = false;
 
        Eigen::Matrix4f stopPose;
 
        Eigen::Vector3f filteredForce;
        Eigen::Vector3f forceOffset;
        Eigen::Vector3f filteredForceInRoot;
        WriteBufferedTripleBuffer<Eigen::Vector3f> forceThreshold;
        std::atomic<bool> useForceStop;
        std::atomic<float> timeForCalibration;
        const SensorValueForceTorque* forceSensor;
 
        //        Eigen::Matrix4f oldPose;
        // NJointController interface
    protected:
        void rtPreActivateController();
    };
 
} // namespace armarx