NJointTaskSpaceImpedanceDMPController.h

Go to the documentation of this file.

#pragma once
// DMP
#include <dmp/representation/dmp/umidmp.h>
 
// armarx
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
 
// control
#include <armarx/control/deprecated_njoint_mp_controller/task_space/TaskSpaceImpedanceDMPControllerInterface.h>
 
namespace armarx
{
    class SensorValue1DoFActuatorTorque;
    class SensorValue1DoFActuatorVelocity;
    class SensorValue1DoFActuatorPosition;
    class ControlTarget1DoFActuatorTorque;
    class SensorValueForceTorque;
} // namespace armarx
 
namespace armarx
{
    class SensorValueForceTorque;
 
    class ControlTarget1DoFActuatorTorque;
 
    namespace control::deprecated_njoint_mp_controller::tsvmp
    {
        class TaskSpaceDMPController;
 
        using TaskSpaceDMPControllerPtr = std::shared_ptr<TaskSpaceDMPController>;
    } // namespace control::deprecated_njoint_mp_controller::tsvmp
} // namespace armarx
 
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
    namespace tsvmp = armarx::control::deprecated_njoint_mp_controller::tsvmp;
 
    TYPEDEF_PTRS_HANDLE(NJointTaskSpaceImpedanceDMPController);
    TYPEDEF_PTRS_HANDLE(NJointTaskSpaceImpedanceDMPControllerControlData);
 
    class NJointTaskSpaceImpedanceDMPControllerControlData
    {
    public:
        Eigen::VectorXf targetVel;
        Eigen::Matrix4f targetPose;
        Eigen::VectorXf desiredNullSpaceJointValues;
        double canVal;
        double mpcFactor;
    };
 
    /**
     * @brief The NJointTaskSpaceImpedanceDMPController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointTaskSpaceImpedanceDMPController :
        public NJointControllerWithTripleBuffer<NJointTaskSpaceImpedanceDMPControllerControlData>,
        public NJointTaskSpaceImpedanceDMPControllerInterface
    {
    public:
        using ConfigPtrT = NJointTaskSpaceImpedanceDMPControllerConfigPtr;
        NJointTaskSpaceImpedanceDMPController(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;
 
        // NJointTaskSpaceImpedanceDMPControllerInterface interface
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&) override;
 
        bool
        isFinished(const Ice::Current&) override
        {
            return finished;
        }
 
        bool
        isDMPRunning(const Ice::Current&) override
        {
            return started;
        }
 
        void
        setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
 
        void learnJointDMPFromFiles(const std::string& fileName,
                                    const Ice::FloatSeq& currentJVS,
                                    const Ice::Current&) override;
        void runDMP(const Ice::DoubleSeq& goals,
                    const Ice::Current& iceCurrent = Ice::emptyCurrent) override;
        void runDMPWithTime(const Ice::DoubleSeq& goals,
                            Ice::Double timeDuration,
                            const Ice::Current&) override;
 
        Ice::Double getVirtualTime(const Ice::Current&) override;
 
        void stopDMP(const Ice::Current&) override;
        void resumeDMP(const Ice::Current&) override;
        void resetDMP(const Ice::Current&) override;
 
        void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&) override;
        DoubleSeqSeq getMPWeights(const Ice::Current&) override;
 
        void removeAllViaPoints(const Ice::Current&) override;
 
        void setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
        void setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
        void setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
        void setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
        void setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&) override;
        void setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&) override;
        void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
        void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
                                            const Ice::Current&) 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:
        virtual void onPublish(const SensorAndControl&,
                               const DebugDrawerInterfacePrx&,
                               const DebugObserverInterfacePrx&) override;
 
        void onInitNJointController() override;
        void onDisconnectNJointController() override;
        void controllerRun();
 
    private:
        struct DebugBufferData
        {
            double currentCanVal;
            double mpcfactor;
            float targetPose_x;
            float targetPose_y;
            float targetPose_z;
            float targetPose_qw;
            float targetPose_qx;
            float targetPose_qy;
            float targetPose_qz;
 
            float currentPose_x;
            float currentPose_y;
            float currentPose_z;
            float currentPose_qw;
            float currentPose_qx;
            float currentPose_qy;
            float currentPose_qz;
 
            float currentKpos_x;
            float currentKpos_y;
            float currentKpos_z;
            float currentKori_x;
            float currentKori_y;
            float currentKori_z;
            float currentKnull_x;
            float currentKnull_y;
            float currentKnull_z;
 
            float currentDpos_x;
            float currentDpos_y;
            float currentDpos_z;
            float currentDori_x;
            float currentDori_y;
            float currentDori_z;
            float currentDnull_x;
            float currentDnull_y;
            float currentDnull_z;
 
            StringFloatDictionary desired_torques;
            StringFloatDictionary desired_nullspaceJoint;
            float forceDesired_x;
            float forceDesired_y;
            float forceDesired_z;
            float forceDesired_rx;
            float forceDesired_ry;
            float forceDesired_rz;
 
            float deltaT;
        };
 
        WriteBufferedTripleBuffer<DebugBufferData> debugOutputData;
 
        struct NJointTaskSpaceImpedanceDMPControllerSensorData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPose;
            Eigen::VectorXf currentTwist;
        };
 
        WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData>
            controllerSensorData;
 
        struct NJointTaskSpaceImpedanceDMPControllerInterfaceData
        {
            Eigen::Matrix4f currentTcpPose;
        };
 
        WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
 
        struct CtrlParams
        {
            Eigen::Vector3f kpos;
            Eigen::Vector3f dpos;
            Eigen::Vector3f kori;
            Eigen::Vector3f dori;
            Eigen::VectorXf knull;
            Eigen::VectorXf dnull;
        };
 
        WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
 
 
        DMP::Vec<DMP::DMPState> currentJointState;
        DMP::UMIDMPPtr nullSpaceJointDMPPtr;
 
        tsvmp::TaskSpaceDMPControllerPtr dmpCtrl;
 
        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
        std::vector<ControlTarget1DoFActuatorTorque*> targets;
 
        // velocity ik controller parameters
        // dmp parameters
        double timeDuration;
        bool finished;
        VirtualRobot::RobotNodeSetPtr rns;
 
        // phaseStop parameters
        double phaseL;
        double phaseK;
        double phaseDist0;
        double phaseDist1;
        double posToOriRatio;
 
 
        NJointTaskSpaceImpedanceDMPControllerConfigPtr cfg;
        VirtualRobot::DifferentialIKPtr ik;
        VirtualRobot::RobotNodePtr tcp;
 
        float torqueLimit;
 
        //        Eigen::Vector3f kpos;
        //        Eigen::Vector3f kori;
        //        Eigen::Vector3f dpos;
        //        Eigen::Vector3f dori;
        //        Eigen::VectorXf knull;
        //        Eigen::VectorXf dnull;
        int numOfJoints;
 
        std::atomic_bool useNullSpaceJointDMP;
        bool isNullSpaceJointDMPLearned;
 
 
        WriteBufferedTripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
        std::vector<std::string> jointNames;
        mutable MutexType controllerMutex;
        PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
        std::atomic_bool firstRun;
        std::atomic_bool started = false;
        std::atomic_bool stopped = false;
        std::atomic_bool prematurely_stopped = false;
 
        Eigen::Matrix4f stopPose;
 
        Eigen::Vector3f filteredForce;
        Eigen::Vector3f forceOffset;
        Eigen::Vector3f filteredForceInRoot;
        WriteBufferedTripleBuffer<Eigen::Vector3f> forceThreshold;
        std::atomic<bool> useForceStop;
        std::atomic<float> timeForCalibration;
        const SensorValueForceTorque* forceSensor;
 
        Eigen::Matrix4f oldPose;
        // NJointController interface
    protected:
        void rtPreActivateController();
    };
 
} // namespace armarx::control::deprecated_njoint_mp_controller::task_space