NJointBimanualForceMPController.h

Go to the documentation of this file.

#pragma once
 
 
// Simox
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
 
// DMP
#include <dmp/representation/dmp/umitsmp.h>
 
// armarx
#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>
#include <RobotAPI/libraries/core/PIDController.h>
 
// control
#include <armarx/control/deprecated_njoint_mp_controller/bimanual/ForceMPControllerInterface.h>
#include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
 
 
namespace armarx::control::deprecated_njoint_mp_controller::bimanual
{
    namespace tsvmp = armarx::control::deprecated_njoint_mp_controller::tsvmp;
 
    TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPController);
    TYPEDEF_PTRS_HANDLE(NJointBimanualForceMPControllerControlData);
 
    using ViaPoint = std::pair<double, DMP::DVec >;
    using ViaPointsSet = std::vector<ViaPoint >;
    class NJointBimanualForceMPControllerControlData
    {
    public:
        Eigen::Matrix4f leftTargetPose;
        Eigen::Matrix4f rightTargetPose;
    };
 
    /**
     * @brief The NJointBimanualForceMPController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointBimanualForceMPController :
        public NJointControllerWithTripleBuffer<NJointBimanualForceMPControllerControlData>,
        public NJointBimanualForceMPControllerInterface
    {
    public:
        using ConfigPtrT = NJointBimanualForceMPControllerConfigPtr;
        NJointBimanualForceMPController(const RobotUnitPtr&, 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;
 
        // NJointBimanualForceMPControllerInterface interface
        void learnDMPFromFiles(const std::string& whichMP, const Ice::StringSeq& fileNames, const Ice::Current&) override;
        bool isFinished(const Ice::Current&) override
        {
            return finished;
        }
 
        void runDMP(const Ice::DoubleSeq& leftGoals, const Ice::DoubleSeq& rightGoals, const Ice::Current&) override;
 
        double getCanVal(const Ice::Current&) override
        {
            return canVal;
        }
 
        void setViaPoints(const std::string& whichDMP, double canVal, const Ice::DoubleSeq& viaPoint, const Ice::Current&) override;
 
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
 
        void onInitNJointController() override;
        void onDisconnectNJointController() override;
 
        void controllerRun();
    private:
        struct DebugBufferData
        {
            StringFloatDictionary dmpTargets;
            StringFloatDictionary currentPose;
            double canVal;
            float forceOnHands;
        };
        TripleBuffer<DebugBufferData> debugOutputData;
 
        struct NJointBimanualForceMPControllerSensorData
        {
            Eigen::Matrix4f leftPoseInRootFrame;
            Eigen::Vector6f leftTwistInRootFrame;
 
            Eigen::Matrix4f rightPoseInRootFrame;
            Eigen::Vector6f rightTwistInRootFrame;
 
            Eigen::Vector3f leftForceInRootFrame;
            Eigen::Vector3f rightForceInRootFrame;
            double deltaT;
        };
        TripleBuffer<NJointBimanualForceMPControllerSensorData> controllerSensorData;
 
        struct NJointBimanualForceMPControllerInterfaceData
        {
            Eigen::Matrix4f leftTcpPoseInRootFrame;
            Eigen::Matrix4f rightTcpPoseInRootFrame;
        };
        TripleBuffer<NJointBimanualForceMPControllerInterfaceData> interfaceData;
 
        std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
 
        std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
 
        const SensorValueForceTorque* rightForceTorque;
        const SensorValueForceTorque* leftForceTorque;
 
        VirtualRobot::DifferentialIKPtr leftIK;
        VirtualRobot::DifferentialIKPtr rightIK;
 
 
        tsvmp::TaskSpaceDMPControllerPtr leftDMPController;
        tsvmp::TaskSpaceDMPControllerPtr rightDMPController;
 
        // velocity ik controller parameters
        CartesianVelocityControllerPtr leftTCPController;
        CartesianVelocityControllerPtr rightTCPController;
 
        // dmp parameters
        bool finished;
        VirtualRobot::RobotNodePtr leftTCP;
        VirtualRobot::RobotNodePtr rightTCP;
 
 
        Eigen::VectorXf targetVels;
        Eigen::Matrix4f targetPose;
 
        NJointBimanualForceMPControllerConfigPtr cfg;
        mutable MutexType controllerMutex;
        PeriodicTask<NJointBimanualForceMPController>::pointer_type controllerTask;
 
        // velocity control
        float Kp_LinearVel;
        float Kd_LinearVel;
        float Kp_AngularVel;
        float Kd_AngularVel;
 
        // force control
        float Kp_f;
        float Kd_f;
        float Ki_f;
 
        double canVal;
        double xvel;
 
        float forceIterm;
        bool leftLearned;
        bool rightLearned;
 
        Eigen::Vector3f leftFilteredValue;
        Eigen::Vector3f rightFilteredValue;
 
        Eigen::Vector3f leftForceOffset;
        Eigen::Vector3f rightForceOffset;
        float targetSupportForce;
 
        double I_decay;
 
        float forceSign;
 
    };
 
} // namespace armarx