NJointCCDMPController.h

Go to the documentation of this file.

#pragma once
 
 
// Simox
#include <VirtualRobot/IK/IKSolver.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
// DMP
#include <dmp/representation/dmp/umitsmp.h>
 
// armarx
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
// control
#include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
 
namespace armarx
{
    class SensorValue1DoFActuatorTorque;
    class SensorValue1DoFActuatorVelocity;
    class SensorValue1DoFActuatorPosition;
    class SensorValue1DoFGravityTorque;
    class ControlTarget1DoFActuatorTorque;
    class ControlTarget1DoFActuatorVelocity;
    class SensorValueForceTorque;
} // namespace armarx
 
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
 
    TYPEDEF_PTRS_HANDLE(NJointCCDMPController);
    TYPEDEF_PTRS_HANDLE(NJointCCDMPControllerControlData);
 
    using ViaPoint = std::pair<double, DMP::DVec>;
    using ViaPointsSet = std::vector<ViaPoint>;
 
    class NJointCCDMPControllerControlData
    {
    public:
        Eigen::VectorXf targetTSVel;
        // 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 NJointCCDMPController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointCCDMPController :
        public NJointControllerWithTripleBuffer<NJointCCDMPControllerControlData>,
        public NJointCCDMPControllerInterface
    {
        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 = NJointCCDMPControllerConfigPtr;
        NJointCCDMPController(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;
 
        // NJointCCDMPControllerInterface interface
        void
        learnDMPFromFiles(int dmpId, const Ice::StringSeq& fileNames, const Ice::Current&) override;
 
        bool
        isFinished(const Ice::Current&) override
        {
            return finished;
        }
 
        void runDMP(const Ice::Current&) override;
        void setTemporalFactor(int dmpId, Ice::Double tau, const Ice::Current&) override;
        void setViaPoints(int dmpId,
                          Ice::Double u,
                          const Ice::DoubleSeq& viapoint,
                          const Ice::Current&) override;
        void setGoals(int dmpId, 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;
 
    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:
        struct DebugBufferData
        {
            StringFloatDictionary latestTargetVelocities;
            StringFloatDictionary dmpTargets;
            StringFloatDictionary realTCP;
 
            double mpcFactor;
            double error;
            double phaseStop;
            double posError;
            double oriError;
            double deltaT;
            double canVal0;
        };
 
        TripleBuffer<DebugBufferData> debugOutputData;
 
        struct NJointCCDMPControllerSensorData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPose;
        };
 
        TripleBuffer<NJointCCDMPControllerSensorData> controllerSensorData;
 
        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
        std::vector<const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
        std::vector<ControlTarget1DoFActuatorVelocity*> targets;
 
        // velocity ik controller parameters
        std::vector<pidController> torquePIDs;
        CartesianVelocityControllerPtr tcpController;
        std::string nodeSetName;
 
        // dmp parameters
        std::vector<DMP::UMITSMPPtr> dmpPtrList;
        std::vector<double> canVals;
        std::vector<double> timeDurations;
        std::vector<std::string> dmpTypes;
        std::vector<double> amplitudes;
 
        std::vector<DMP::Vec<DMP::DMPState>> currentStates;
        std::vector<DMP::DVec> targetSubStates;
 
        bool finished;
        double tau;
        ViaPointsSet viaPoints;
        bool isDisturbance;
 
 
        // phaseStop parameters
        double phaseL;
        double phaseK;
        double phaseDist0;
        double phaseDist1;
        double phaseKpPos;
        double phaseKpOri;
 
        double posToOriRatio;
 
        Eigen::VectorXf targetVels;
 
        std::vector<int> learnedDMP;
 
 
        NJointCCDMPControllerConfigPtr cfg;
        VirtualRobot::RobotNodePtr tcp;
        Eigen::Vector3f tcpPosition;
        Eigen::Quaterniond tcpOrientation;
 
        Eigen::Matrix4f oldPose;
        VirtualRobot::DifferentialIKPtr ik;
 
        DMP::DVec targetState;
        mutable MutexType controllerMutex;
        PeriodicTask<NJointCCDMPController>::pointer_type controllerTask;
    };
 
} // namespace armarx::control::deprecated_njoint_mp_controller::task_space