DeprecatedNJointTSDMPController.h

Go to the documentation of this file.

 
#pragma once
 
#include <VirtualRobot/IK/IKSolver.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
#include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
#include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
 
#include <dmp/representation/dmp/umitsmp.h>
 
namespace armarx
{
    class SensorValue1DoFActuatorTorque;
    class SensorValue1DoFActuatorVelocity;
    class SensorValue1DoFActuatorPosition;
    class ControlTarget1DoFActuatorTorque;
    class ControlTarget1DoFActuatorVelocity;
    class SensorValue1DoFGravityTorque;
    class SensorValueForceTorque;
} // namespace armarx
 
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::control::deprecated_njoint_mp_controller::task_space