NJointJSDMPController.h

Go to the documentation of this file.

 
#pragma once
 
#include <boost/shared_ptr.hpp>
 
#include <Eigen/Dense>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/services/tasks/RunningTask.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
 
#include <armarx/control/deprecated_njoint_mp_controller/joint_space/ControllerInterface.h>
 
#include <dmp/general/vec.h>
#include <dmp/representation/systemstatus.h>
 
namespace armarx
{
    class SensorValue1DoFActuatorTorque;
    class SensorValue1DoFActuatorVelocity;
    class SensorValue1DoFActuatorPosition;
    class ControlTarget1DoFActuatorTorque;
    class ControlTarget1DoFActuatorVelocity;
} // namespace armarx
 
namespace DMP
{
    // using UMITSMPPtr = boost::shared_ptr<class UMITSMP>;
    using UMIDMPPtr = ::boost::shared_ptr<class UMIDMP>;
} // namespace DMP
 
namespace armarx::control::deprecated_njoint_mp_controller::joint_space
{
    TYPEDEF_PTRS_HANDLE(NJointJSDMPController);
    TYPEDEF_PTRS_HANDLE(NJointJSDMPControllerControlData);
 
    class NJointJSDMPControllerControlData
    {
    public:
        Eigen::VectorXf targetJointVels;
    };
 
    /**
     * @brief The NJointJSDMPController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointJSDMPController :
        public NJointControllerWithTripleBuffer<NJointJSDMPControllerControlData>,
        public NJointJointSpaceDMPControllerInterface
    {
    public:
        using ConfigPtrT = NJointJointSpaceDMPControllerConfigPtr;
        NJointJSDMPController(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;
 
        bool
        isFinished(const Ice::Current&) override
        {
            return finished;
        }
 
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
        void setSpeed(double times, const Ice::Current&) override;
 
        void runDMP(const Ice::DoubleSeq& goals, double times, const Ice::Current&) override;
 
        void showMessages(const Ice::Current&) override;
        //        std::string getDMPAsString(const Ice::Current&) override;
        std::string getDMPAsString(const Ice::Current&) override;
        std::vector<double> createDMPFromString(const std::string& dmpString,
                                                const Ice::Current&) override;
        void setViaPoints(Ice::Double u, double viapoint, const Ice::Current&) override;
 
        void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
        DoubleSeqSeq getMPWeights(const Ice::Current&) override;
 
 
    protected:
        void rtPreActivateController() override;
        void rtPostDeactivateController() override;
 
        virtual void onPublish(const SensorAndControl&,
                               const DebugDrawerInterfacePrx&,
                               const DebugObserverInterfacePrx&) override;
 
    private:
        NJointJointSpaceDMPControllerConfigPtr cfg;
 
        struct DebugBufferData
        {
            StringFloatDictionary latestTargetVelocities;
            StringFloatDictionary latestTargets;
 
            double currentCanVal;
            double mpcFactor;
        };
 
        TripleBuffer<DebugBufferData> debugOutputData;
 
        struct NJointJSDMPControllerSensorData
        {
            double currentTime;
            double deltaT;
            DMP::Vec<DMP::DMPState> currentState;
        };
 
        TripleBuffer<NJointJSDMPControllerSensorData> controllerSensorData;
 
        struct RTToUserData
        {
            Eigen::VectorXf qpos;
            Eigen::VectorXf qvel;
        };
 
        TripleBuffer<RTToUserData> rt2UserData;
 
 
        std::map<std::string, const SensorValue1DoFActuatorPosition*> positionSensors;
        std::map<std::string, const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
 
        IceUtil::Time last;
 
        DMP::UMIDMPPtr dmpPtr;
        double timeDuration;
        DMP::Vec<DMP::DMPState> currentState;
        DMP::Vec<DMP::DMPState> currentDMPState;
 
        double canVal;
        double deltaT;
        double tau;
 
        double finished;
 
        // phaseStop parameters
        double phaseL;
        double phaseK;
        double phaseDist0;
        double phaseDist1;
        double phaseKp;
 
        bool isDisturbance;
        bool started;
        std::vector<std::string> dimNames;
        DMP::DVec targetState;
        Eigen::VectorXf targetVels;
 
        mutable MutexType controllerMutex;
 
        Eigen::VectorXf qpos;
        Eigen::VectorXf qvel;
        // ManagedIceObject interface
    protected:
        void controllerRun();
        void onInitNJointController() override;
        void onDisconnectNJointController() override;
    };
 
} // namespace armarx::control::deprecated_njoint_mp_controller::joint_space