NJointBimanualCartesianAdmittanceController.h

Go to the documentation of this file.

#pragma once
 
 
// Simox
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.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/math/MathUtils.h>
#include <RobotAPI/libraries/core/PIDController.h>
 
// control
#include <armarx/control/deprecated_njoint_mp_controller/bimanual/CartesianAdmittanceControllerInterface.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(NJointBimanualCartesianAdmittanceController);
    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelControlData);
 
 
    class NJointBimanualCartesianAdmittanceController :
        public NJointController,
        public NJointBimanualCartesianAdmittanceControllerInterface
    {
    public:
        NJointBimanualCartesianAdmittanceController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
 
        // NJointController interface
        std::string getClassName(const Ice::Current&) const override;
        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
 
        //set config
        void setConfig(const NJointBimanualCartesianAdmittanceControllerConfigPtr& ptr, const Ice::Current& = Ice::emptyCurrent) override;
        void setDesiredJointValuesLeft(const Ice::FloatSeq& vals, const Ice::Current& = Ice::emptyCurrent) override;
        void setDesiredJointValuesRight(const Ice::FloatSeq& vals, const Ice::Current& = Ice::emptyCurrent) override;
        void setNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace, const Ice::Current& = Ice::emptyCurrent) override;
        void setAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance& admittanceObject, const Ice::Current& = Ice::emptyCurrent) override;
        void setForceConfig(const detail::NJBmanCartAdmCtrl::Force& left, const detail::NJBmanCartAdmCtrl::Force& right, const Ice::Current& = Ice::emptyCurrent) override;
        void setImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& left, const detail::NJBmanCartAdmCtrl::Impedance& right, const Ice::Current& = Ice::emptyCurrent) override;
        //control
        Eigen::Matrix4f getBoxPose(const Ice::Current& = Ice::emptyCurrent) const override;
        void setBoxPose(const Eigen::Matrix4f& pose, const Ice::Current& = Ice::emptyCurrent) override;
        void setBoxWidth(float w, const Ice::Current& = Ice::emptyCurrent) override;
        void setBoxVelocity(
            const Eigen::Vector3f& velXYZ,
            const Eigen::Vector3f& velRPY,
            const Ice::Current& = Ice::emptyCurrent) override;
        void setBoxPoseAndVelocity(const Eigen::Matrix4f& pose,
                                   const Eigen::Vector3f& velXYZ,
                                   const Eigen::Vector3f& velRPY,
                                   const Ice::Current& = Ice::emptyCurrent) override;
        void moveBoxPose(const Eigen::Matrix4f& pose, const Ice::Current& = Ice::emptyCurrent) override;
        void moveBoxPosition(const Eigen::Vector3f& pos, const Ice::Current& = Ice::emptyCurrent) override;
    protected:
        void updateConfig(const NJointBimanualCartesianAdmittanceControllerConfig& cfg);
        void updateDesiredJointValuesLeft(const Ice::FloatSeq& cfg);
        void updateDesiredJointValuesRight(const Ice::FloatSeq& cfg);
        void updateNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace);
        void updateAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance admittanceObject);
        void updateForceConfig(const detail::NJBmanCartAdmCtrl::Force& left, const detail::NJBmanCartAdmCtrl::Force& right);
        void updateImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& left, const detail::NJBmanCartAdmCtrl::Impedance& right);
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
        //        void onInitNJointController();
        //        void onDisconnectNJointController();
        //        void controllerRun(); //runs dmp controller
 
    private:
        struct Target
        {
            Eigen::Matrix4f pose;
            Eigen::Vector6f vel;
        };
        mutable std::recursive_mutex      targBufWriteMutex;
        WriteBufferedTripleBuffer<Target> targBuf;
 
 
        struct PreprocessedCfg
        {
            float boxWidth;
 
            Eigen::Vector6f KmAdmittance;
            Eigen::Vector6f KpAdmittance;
            Eigen::Vector6f KdAdmittance;
 
            float ftCalibrationTime;
 
            Eigen::Vector12f KpImpedance;
            Eigen::Vector12f KdImpedance;
 
            float massLeft;
            Eigen::Vector3f CoMVecLeft;
            Eigen::Vector3f forceOffsetLeft;
            Eigen::Vector3f torqueOffsetLeft;
 
            float massRight;
            Eigen::Vector3f CoMVecRight;
            Eigen::Vector3f forceOffsetRight;
            Eigen::Vector3f torqueOffsetRight;
 
            Eigen::VectorXf desiredJointValuesLeft;
            Eigen::VectorXf desiredJointValuesRight;
 
            float knull;
            float dnull;
 
            float torqueLimit;
 
            Eigen::Vector12f targetWrench;
 
            float filterCoeff;
 
            Eigen::Vector6f forceThreshold;
        };
        mutable std::recursive_mutex               cfgBufWriteMutex;
        WriteBufferedTripleBuffer<PreprocessedCfg> cfgBuf;
 
        struct RTData
        {
            struct Arm
            {
                std::vector<ControlTarget1DoFActuatorTorque*> targets;
                std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
                std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
                const SensorValueForceTorque* forceTorque;
                VirtualRobot::DifferentialIKPtr IK;
 
                std::vector<std::string> jointNames;
                VirtualRobot::RobotNodeSetPtr rns;
                VirtualRobot::RobotNodePtr tcp;
                VirtualRobot::RobotNodePtr frameFTSensor;
 
                Eigen::Matrix4f sensorFrame2TcpFrame{Eigen::Matrix4f::Identity()};
            };
            Arm left;
            Arm right;
 
            double ftcalibrationTimer = 0;
            //            Eigen::Vector12f ftOffset = Eigen::Vector12f::Zero();
            bool firstLoop = true;
 
            Eigen::Vector6f virtualAcc = Eigen::Vector6f::Zero();
            Eigen::Vector6f virtualVel = Eigen::Vector6f::Zero();
            Eigen::Matrix4f virtualPose = Eigen::Matrix4f::Identity();
 
            Eigen::Vector12f filteredOldValue = Eigen::Vector12f::Zero();
        };
        RTData rt;
 
 
        struct DebugBufferData
        {
            Eigen::Matrix4f currentBoxPose;
 
            StringFloatDictionary desired_torques;
 
            float virtualPose_x;
            float virtualPose_y;
            float virtualPose_z;
 
            float objPose_x;
            float objPose_y;
            float objPose_z;
 
            float objForce_x;
            float objForce_y;
            float objForce_z;
            float objTorque_x;
            float objTorque_y;
            float objTorque_z;
 
            float deltaPose_x;
            float deltaPose_y;
            float deltaPose_z;
            float deltaPose_rx;
            float deltaPose_ry;
            float deltaPose_rz;
 
            float objVel_x;
            float objVel_y;
            float objVel_z;
            float objVel_rx;
            float objVel_ry;
            float objVel_rz;
 
            float modifiedPoseRight_x;
            float modifiedPoseRight_y;
            float modifiedPoseRight_z;
            float currentPoseLeft_x;
            float currentPoseLeft_y;
            float currentPoseLeft_z;
 
            float modifiedPoseLeft_x;
            float modifiedPoseLeft_y;
            float modifiedPoseLeft_z;
            float currentPoseRight_x;
            float currentPoseRight_y;
            float currentPoseRight_z;
 
            float dmpBoxPose_x;
            float dmpBoxPose_y;
            float dmpBoxPose_z;
 
            float dmpTwist_x;
            float dmpTwist_y;
            float dmpTwist_z;
 
            float modifiedTwist_lx;
            float modifiedTwist_ly;
            float modifiedTwist_lz;
            float modifiedTwist_rx;
            float modifiedTwist_ry;
            float modifiedTwist_rz;
 
            float rx;
            float ry;
            float rz;
 
            //            Eigen::VectorXf wrenchDMP;
            //            Eigen::VectorXf computedBoxWrench;
 
            Eigen::VectorXf forceImpedance;
            Eigen::VectorXf forcePID;
            Eigen::VectorXf forcePIDControlValue;
            Eigen::VectorXf poseError;
            Eigen::VectorXf wrenchesConstrained;
            Eigen::VectorXf wrenchesMeasuredInRoot;
        };
 
        mutable std::recursive_mutex  debugOutputDataReadMutex;
        TripleBuffer<DebugBufferData> debugOutputData;
 
        //        struct rt2ControlData
        //        {
        //            double currentTime;
        //            double deltaT;
        //            Eigen::Matrix4f currentPose;
        //            Eigen::VectorXf currentTwist;
        //        };
        //        TripleBuffer<rt2ControlData> rt2ControlBuffer;
 
        //        struct ControlInterfaceData
        //        {
        //            Eigen::Matrix4f currentLeftPose;
        //            Eigen::Matrix4f currentRightPose;
        //        };
 
        //        TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
 
        //        float torqueLimit;
 
 
 
        //        TaskSpaceDMPControllerPtr objectDMP;
 
 
        //        Eigen::Matrix4f leftInitialPose;
        //        Eigen::Matrix4f rightInitialPose;
        //        Eigen::Matrix4f boxInitialPose;
 
 
        //        std::vector<PIDControllerPtr> forcePIDControllers;
 
        // filter parameters
        //        bool finished;
        //        bool dmpStarted;
    protected:
        void rtPreActivateController();
    };
 
} // namespace armarx