NJointBimanualCartesianAdmittanceController.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/PIDController.h>
 
#include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
#include <armarx/control/deprecated_njoint_mp_controller/adaptive/ControllerInterface.h>
#include <armarx/control/deprecated_njoint_mp_controller/bimanual/CartesianAdmittanceControllerInterface.h>
 
namespace armarx
{
    class SensorValue1DoFActuatorTorque;
    class SensorValue1DoFActuatorVelocity;
    class SensorValue1DoFActuatorPosition;
    class ControlTarget1DoFActuatorTorque;
    class SensorValueForceTorque;
} // namespace armarx
 
namespace DMP
{
    using UMIDMPPtr = boost::shared_ptr<class UMIDMP>;
}
 
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::control::deprecated_njoint_mp_controller::bimanual