NJointBimanualForceMPController.h

Go to the documentation of this file.

#pragma once
 
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
#include <RobotAPI/libraries/core/PIDController.h>
 
#include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
#include <armarx/control/deprecated_njoint_mp_controller/bimanual/ForceMPControllerInterface.h>
 
namespace armarx
{
    class SensorValue1DoFActuatorTorque;
    class SensorValue1DoFActuatorVelocity;
    class SensorValue1DoFActuatorPosition;
    class SensorValue1DoFActuatorAcceleration;
    class ControlTarget1DoFActuatorVelocity;
    class SensorValueForceTorque;
} // namespace armarx
 
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::Identity();
        Eigen::Matrix4f rightTargetPose = Eigen::Matrix4f::Identity();
    };
 
    /**
     * @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::control::deprecated_njoint_mp_controller::bimanual