NJointJointSpaceDMPController.h

Go to the documentation of this file.

 
#pragma once
 
#include <VirtualRobot/Robot.h>
#include <dmp/representation/dmp/umidmp.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>
 
// control
#include <armarx/control/deprecated_njoint_mp_controller/joint_space/ControllerInterface.h>
 
 
namespace armarx::control::deprecated_njoint_mp_controller::joint_space
{
 
    TYPEDEF_PTRS_HANDLE(NJointJointSpaceDMPController);
 
    TYPEDEF_PTRS_HANDLE(NJointJointSpaceDMPControllerControlData);
    class NJointJointSpaceDMPControllerControlData
    {
    public:
        double tau;
        bool isStart;
    };
 
 
    //    class SimplePID
    //    {
    //    public:
    //        float Kp = 0, Kd = 0;
    //        float lastError = 0;
    //        float update(float dt, float error);
    //    };
 
    /**
     * @brief The NJointJointSpaceDMPController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointJointSpaceDMPController :
        public NJointControllerWithTripleBuffer<NJointJointSpaceDMPControllerControlData>,
        public NJointJointSpaceDMPControllerInterface
    {
    public:
        using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr;
        NJointJointSpaceDMPController(RobotUnitPtr prov, 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;
 
        //
        bool isFinished(const Ice::Current&) override
        {
            return finished;
        }
 
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
        void setTemporalFactor(double tau, const Ice::Current&) override;
 
        void runDMP(const Ice::DoubleSeq&  goals, double tau, const Ice::Current&) override;
 
        void showMessages(const Ice::Current&) override;
 
    protected:
        void rtPreActivateController() override;
        void rtPostDeactivateController() override;
 
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
    private:
 
        struct DebugBufferData
        {
            StringFloatDictionary latestTargetVelocities;
            StringFloatDictionary dmpTargetState;
            double currentCanVal;
            double mpcFactor;
        };
 
        std::map<std::string, const SensorValue1DoFActuatorTorque*> torqueSensors;
        std::map<std::string, const SensorValue1DoFGravityTorque*> gravityTorqueSensors;
        std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors;
        std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
 
 
        TripleBuffer<DebugBufferData> debugOutputData;
 
 
        std::vector<double> goals;
        DMP::UMIDMPPtr dmpPtr;
        bool DMPAsForwardControl;
        double timeDuration;
 
        double canVal;
 
        double tau;
        double finished;
 
        // phaseStop parameters
        double phaseL;
        double phaseK;
        double phaseDist0;
        double phaseDist1;
        double phaseKp;
 
        double mpcFactor;
 
        bool isDisturbance;
        std::vector<std::string> dimNames;
        DMP::Vec<DMP::DMPState> currentState;
        DMP::DVec targetState;
 
    };
 
} // namespace armarx