NJointBimanualCCDMPVelocityController.h

Go to the documentation of this file.

#pragma once
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/CycleUtil.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/ControllerInterface.h>
 
namespace armarx
{
    class SensorValue1DoFActuatorTorque;
    class SensorValue1DoFActuatorVelocity;
    class SensorValue1DoFActuatorPosition;
    class SensorValue1DoFActuatorAcceleration;
    class ControlTarget1DoFActuatorVelocity;
    class ControlTarget1DoFActuatorTorque;
    class SensorValueForceTorque;
} // 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::bimanual
{
    namespace tsvmp = armarx::control::deprecated_njoint_mp_controller::tsvmp;
 
    TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPVelocityController);
    TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPVelocityControllerControlData);
 
    using ViaPoint = std::pair<double, DMP::DVec>;
    using ViaPointsSet = std::vector<ViaPoint>;
 
    class NJointBimanualCCDMPVelocityControllerControlData
    {
    public:
        Eigen::VectorXf leftTargetVel;
        Eigen::VectorXf rightTargetVel;
 
        Eigen::Matrix4f leftTargetPose;
        Eigen::Matrix4f rightTargetPose;
 
        double virtualTime;
 
        Eigen::VectorXf leftDesiredJoint;
        Eigen::VectorXf rightDesiredJoint;
    };
 
    /**
     * @brief The NJointBimanualCCDMPVelocityController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointBimanualCCDMPVelocityController :
        public NJointControllerWithTripleBuffer<NJointBimanualCCDMPVelocityControllerControlData>,
        public NJointBimanualCCDMPVelocityControllerInterface
    {
    public:
        using ConfigPtrT = NJointBimanualCCDMPVelocityControllerConfigPtr;
        NJointBimanualCCDMPVelocityController(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;
 
        // NJointBimanualCCDMPVelocityControllerInterface interface
        void
        learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&) override;
        void learnDMPFromBothFiles(const Ice::StringSeq&,
                                   const Ice::StringSeq&,
                                   const Ice::Current&) override;
 
        bool isFinished(const Ice::Current&) override;
 
        void setRatios(const Ice::DoubleSeq& ratios, const Ice::Current&) override;
 
        void runDMP(const Ice::DoubleSeq& leftGoals,
                    const Ice::DoubleSeq& rightGoals,
                    const Ice::Current&) override;
        void
        setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
 
        void changeLeader(const Ice::Current&) override;
 
        double getVirtualTime(const Ice::Current&) override;
 
        std::string getLeaderName(const Ice::Current&) override;
 
    protected:
        virtual void onPublish(const SensorAndControl&,
                               const DebugDrawerInterfacePrx&,
                               const DebugObserverInterfacePrx&) override;
 
        void onInitNJointController() override;
        void onDisconnectNJointController() override;
        void controllerRun();
 
    private:
        Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist,
                                         const Eigen::Matrix4f& currentPose,
                                         const Eigen::Matrix4f& targetPose);
 
        Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate,
                                     const Eigen::Matrix4f& globalTargetPose);
        Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec,
                                     const std::vector<double>& globalTargetPoseVec);
 
        struct DebugBufferData
        {
            StringFloatDictionary desired_velocities;
            StringFloatDictionary constrained_force;
            float leftTargetPose_x;
            float leftTargetPose_y;
            float leftTargetPose_z;
            float rightTargetPose_x;
            float rightTargetPose_y;
            float rightTargetPose_z;
 
            float leftCurrentPose_x;
            float leftCurrentPose_y;
            float leftCurrentPose_z;
            float rightCurrentPose_x;
            float rightCurrentPose_y;
            float rightCurrentPose_z;
 
            float leftControlSignal_x;
            float leftControlSignal_y;
            float leftControlSignal_z;
            float leftControlSignal_ro;
            float leftControlSignal_pi;
            float leftControlSignal_ya;
 
            float rightControlSignal_x;
            float rightControlSignal_y;
            float rightControlSignal_z;
            float rightControlSignal_ro;
            float rightControlSignal_pi;
            float rightControlSignal_ya;
 
            double virtualTime;
        };
 
        bool finished;
        TripleBuffer<DebugBufferData> debugDataInfo;
 
        struct NJointBimanualCCDMPVelocityControllerSensorData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentLeftPose;
            Eigen::Matrix4f currentRightPose;
            Eigen::VectorXf currentLeftTwist;
            Eigen::VectorXf currentRightTwist;
        };
 
        TripleBuffer<NJointBimanualCCDMPVelocityControllerSensorData> controllerSensorData;
 
        struct NJointBimanualCCDMPVelocityControllerInterfaceData
        {
            Eigen::Matrix4f currentLeftPose;
            Eigen::Matrix4f currentRightPose;
 
            Eigen::VectorXf currentLeftJointVals;
            Eigen::VectorXf currentRightJointVals;
        };
 
        TripleBuffer<NJointBimanualCCDMPVelocityControllerInterfaceData> interfaceData;
 
 
        std::vector<ControlTarget1DoFActuatorVelocity*> leftTargets;
        std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
 
        std::vector<ControlTarget1DoFActuatorVelocity*> rightTargets;
        std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
 
        const SensorValueForceTorque* rightForceTorque;
        const SensorValueForceTorque* leftForceTorque;
 
        NJointBimanualCCDMPVelocityControllerConfigPtr cfg;
        VirtualRobot::DifferentialIKPtr leftIK;
        VirtualRobot::DifferentialIKPtr rightIK;
 
        std::vector<tsvmp::TaskSpaceDMPControllerPtr> leftGroup;
        std::vector<tsvmp::TaskSpaceDMPControllerPtr> rightGroup;
        std::vector<tsvmp::TaskSpaceDMPControllerPtr> bothLeaderGroup;
        DMP::UMIDMPPtr leftJointDMP;
        DMP::UMIDMPPtr rightJointDMP;
        bool isLeftJointLearned;
        bool isRightJointLearned;
 
 
        std::string leaderName;
 
        VirtualRobot::RobotNodePtr tcpLeft;
        VirtualRobot::RobotNodePtr tcpRight;
 
        double virtualtimer;
        double timeDuration;
 
        mutable MutexType controllerMutex;
 
        Eigen::VectorXf leftDesiredJointValues;
        Eigen::VectorXf rightDesiredJointValues;
 
        Eigen::Vector3f leftKpos;
        Eigen::Vector3f leftKori;
        Eigen::Vector3f leftDpos;
        Eigen::Vector3f leftDori;
 
        Eigen::Vector3f rightKpos;
        Eigen::Vector3f rightKori;
        Eigen::Vector3f rightDpos;
        Eigen::Vector3f rightDori;
 
 
        float knull;
        float dnull;
 
        std::vector<std::string> leftJointNames;
        std::vector<std::string> rightJointNames;
 
        float torqueLimit;
        VirtualRobot::RobotNodeSetPtr leftRNS;
        VirtualRobot::RobotNodeSetPtr rightRNS;
 
        Eigen::VectorXf leftNullSpaceCoefs;
        Eigen::VectorXf rightNullSpaceCoefs;
 
 
        float maxLinearVel;
        float maxAngularVel;
 
 
        CartesianVelocityControllerPtr leftCtrl;
        CartesianVelocityControllerPtr rightCtrl;
 
        bool started;
        bool isDMPRun;
        DMP::Vec<DMP::DMPState> currentLeftJointState;
        DMP::Vec<DMP::DMPState> currentRightJointState;
 
        // NJointBimanualCCDMPVelocityControllerInterface interface
 
        // NJointController interface
    protected:
        void rtPreActivateController() override;
    };
 
} // namespace armarx::control::deprecated_njoint_mp_controller::bimanual