DeprecatedNJointTSDMPController.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/SensorValues/SensorValueForceTorque.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/libraries/core/CartesianVelocityController.h>
 
#include <dmp/representation/dmp/umitsmp.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(DeprecatedNJointTSDMPController);
    TYPEDEF_PTRS_HANDLE(DeprecatedNJointTSDMPControllerControlData);
 
    using ViaPoint = std::pair<double, DMP::DVec >;
    using ViaPointsSet = std::vector<ViaPoint >;
    class DeprecatedNJointTSDMPControllerControlData
    {
    public:
        Eigen::Vector6f targetTSVel;
        Eigen::Matrix4f targetPose;
        // cartesian velocity control data
        std::vector<float> nullspaceJointVelocities;
        float avoidJointLimitsKp = 0;
        std::vector<float> torqueKp;
        std::vector<float> torqueKd;
        VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
    };
 
    /**
     * @brief The DeprecatedNJointTSDMPController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class DeprecatedNJointTSDMPController :
        public NJointControllerWithTripleBuffer<DeprecatedNJointTSDMPControllerControlData>,
        public DeprecatedNJointTaskSpaceDMPControllerInterface
    {
        class pidController
        {
        public:
            float Kp = 0, Kd = 0;
            float lastError = 0;
            float update(float dt, float error)
            {
                float derivative = (error - lastError) / dt;
                float retVal = Kp * error + Kd * derivative;
                lastError = error;
                return retVal;
            }
        };
    public:
        using ConfigPtrT = DeprecatedNJointTaskSpaceDMPControllerConfigPtr;
        DeprecatedNJointTSDMPController(const RobotUnitPtr&, 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;
 
        // DeprecatedNJointTSDMPControllerInterface interface
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
        bool isFinished(const Ice::Current&) override
        {
            return finished;
        }
 
        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&) override;
        void runDMPWithTime(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&) override;
 
        void setSpeed(Ice::Double times, const Ice::Current&) override;
        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
        void removeAllViaPoints(const Ice::Current&) override;
 
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
 
        void setControllerTarget(Ice::Float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode, const Ice::Current&) override;
        void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
        void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&) override;
 
 
        void pauseDMP(const Ice::Current&) override;
        void resumeDMP(const Ice::Current&) override;
 
        void resetDMP(const Ice::Current&) override;
        void stopDMP(const Ice::Current&) override;
        bool isDMPRunning(const Ice::Current&) override
        {
            return started;
        }
 
        double getCanVal(const Ice::Current&) override
        {
            return dmpCtrl->canVal;
        }
        std::string getDMPAsString(const Ice::Current&) override;
        std::vector<double> createDMPFromString(const std::string& dmpString, const Ice::Current&) override;
 
        // VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const DeprecatedNJointTaskSpaceDMPControllerMode::CartesianSelection mode);
        Eigen::VectorXf calcIK(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspace, VirtualRobot::IKSolver::CartesianSelection mode);
 
 
        void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
        DoubleSeqSeq getMPWeights(const Ice::Current&) override;
 
        void setLinearVelocityKd(Ice::Float kd, const Ice::Current& = Ice::emptyCurrent) override;
        void setLinearVelocityKp(Ice::Float kp, const Ice::Current& = Ice::emptyCurrent) override;
        void setAngularVelocityKd(Ice::Float kd, const Ice::Current& = Ice::emptyCurrent) override;
        void setAngularVelocityKp(Ice::Float kp, const Ice::Current& = Ice::emptyCurrent) 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();
        }
    protected:
        void rtPreActivateController() override;
        void rtPostDeactivateController() override;
        VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
 
        void onInitNJointController() override;
        void onDisconnectNJointController() override;
        void controllerRun();
 
    private:
        std::vector<std::string> jointNames;
        struct DebugBufferData
        {
            StringFloatDictionary latestTargetVelocities;
            StringFloatDictionary dmpTargets;
            StringFloatDictionary currentPose;
            double currentCanVal;
            double mpcFactor;
            double error;
            double phaseStop;
            double posError;
            double oriError;
            double deltaT;
        };
 
        TripleBuffer<DebugBufferData> debugOutputData;
 
 
        struct RTDebugData
        {
            Eigen::VectorXf targetJointVels;
        };
 
        TripleBuffer<RTDebugData> rtDebugData;
 
 
        struct RTToControllerData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPose;
            Eigen::VectorXf currentTwist;
        };
        TripleBuffer<RTToControllerData> rt2CtrlData;
 
        struct RTToUserData
        {
            Eigen::Matrix4f currentTcpPose;
 
        };
        TripleBuffer<RTToUserData> rt2UserData;
 
 
        tsvmp::TaskSpaceDMPControllerPtr dmpCtrl;
 
        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
        std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
        std::vector<ControlTarget1DoFActuatorVelocity*> targets;
        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
 
        // velocity ik controller parameters
        std::vector<pidController> torquePIDs;
        CartesianVelocityControllerPtr tcpController;
        std::string nodeSetName;
 
        // dmp parameters
        bool finished;
        bool started;
 
        VirtualRobot::DifferentialIKPtr ik;
        VirtualRobot::RobotNodePtr tcp;
        VirtualRobot::RobotNodePtr refFrame;
        Eigen::Vector6f targetVels;
        Eigen::Matrix4f targetPose;
 
        DeprecatedNJointTaskSpaceDMPControllerConfigPtr cfg;
        mutable MutexType controllerMutex;
        PeriodicTask<DeprecatedNJointTSDMPController>::pointer_type controllerTask;
 
 
        std::string debugName;
 
        Eigen::VectorXf filtered_qvel;
        Eigen::Vector3f filtered_position;
        float vel_filter_factor;
        float pos_filter_factor;
        bool firstRun;
 
        float KpF;
        float DpF;
        float KoF;
        float DoF;
 
        Eigen::VectorXf jointHighLimits;
        Eigen::VectorXf jointLowLimits;
 
        Eigen::Vector3f filteredForce;
        Eigen::Vector3f forceOffset;
        Eigen::Vector3f filteredForceInRoot;
        WriteBufferedTripleBuffer<Eigen::Vector3f> forceThreshold;
        std::atomic<bool> useForceStop;
        const SensorValueForceTorque* forceSensor;
 
        std::atomic<float> timeForCalibration;
        std::atomic_bool stopped = false;
    };
 
} // namespace armarx