NJointPeriodicTSDMPForwardVelController.h

Go to the documentation of this file.

#pragma once
 
 
// Simox
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
 
// armarx
#include <ArmarXCore/core/time/CycleUtil.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/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
 
// control
#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(NJointPeriodicTSDMPForwardVelController);
    TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPForwardVelControllerControlData);
 
    class NJointPeriodicTSDMPForwardVelControllerControlData
    {
    public:
        Eigen::VectorXf targetTSVel;
        Eigen::Matrix4f targetPose;
    };
 
    /**
     * @brief The NJointPeriodicTSDMPForwardVelController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointPeriodicTSDMPForwardVelController :
        public NJointControllerWithTripleBuffer<NJointPeriodicTSDMPForwardVelControllerControlData>,
        public NJointPeriodicTSDMPControllerInterface
    {
    public:
        using ConfigPtrT = NJointPeriodicTSDMPControllerConfigPtr;
        NJointPeriodicTSDMPForwardVelController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
 
        // NJointControllerInterface interface
        std::string getClassName(const Ice::Current&) const;
 
        // NJointController interface
 
        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
 
        // NJointPeriodicTSDMPForwardVelControllerInterface interface
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
        bool isFinished(const Ice::Current&)
        {
            return finished;
        }
 
        void runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&);
        void setSpeed(Ice::Double times, const Ice::Current&);
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
        void setAmplitude(Ice::Double amp, const Ice::Current&);
 
 
        double getCanVal(const Ice::Current&)
        {
            return dmpCtrl->canVal;
        }
 
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
        void onInitNJointController();
        void onDisconnectNJointController();
        void controllerRun();
 
    private:
        struct DebugBufferData
        {
            StringFloatDictionary latestTargetVelocities;
            StringFloatDictionary currentPose;
            double currentCanVal;
            double mpcFactor;
            double error;
            double phaseStop;
            double posError;
            double oriError;
            double deltaT;
        };
 
        TripleBuffer<DebugBufferData> debugOutputData;
 
        struct RTToControllerData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPose;
            Eigen::VectorXf currentTwist;
        };
        TripleBuffer<RTToControllerData> rt2CtrlData;
 
        struct RTToUserData
        {
            Eigen::Matrix4f currentTcpPose;
            float waitTimeForCalibration;
        };
        TripleBuffer<RTToUserData> rt2UserData;
 
 
        tsvmp::TaskSpaceDMPControllerPtr dmpCtrl;
 
        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
        std::vector<ControlTarget1DoFActuatorVelocity*> targets;
 
        // velocity ik controller parameters
        CartesianVelocityControllerPtr tcpController;
        std::string nodeSetName;
 
        // dmp parameters
        bool finished;
        bool started;
        bool dmpRunning;
        bool firstRun;
 
        VirtualRobot::DifferentialIKPtr ik;
        VirtualRobot::RobotNodePtr tcp;
 
        NJointPeriodicTSDMPControllerConfigPtr cfg;
        mutable MutexType controllerMutex;
        PeriodicTask<NJointPeriodicTSDMPForwardVelController>::pointer_type controllerTask;
 
        Eigen::Matrix4f targetPose;
 
 
        const SensorValueForceTorque* forceSensor;
        Eigen::Vector3f filteredForce;
        Eigen::Vector3f forceOffset;
        Eigen::Matrix3f toolTransform;
        Eigen::Vector3f oriToolDir;
        struct UserToRTData
        {
            float targetForce;
        };
        TripleBuffer<UserToRTData> user2rtData;
        float kpf;
 
        // NJointPeriodicTSDMPControllerInterface interface
 
    };
 
} // namespace armarx