NJointBimanualObjLevelController.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/ObjLevelControllerInterface.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(NJointBimanualObjLevelController);
    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelControlData);
 
    class NJointBimanualObjLevelControlData
    {
    public:
        // control target from Movement Primitives
        Eigen::Matrix4f boxPose;
        Eigen::VectorXf boxTwist;
    };
 
 
    class NJointBimanualObjLevelController :
        public NJointControllerWithTripleBuffer<NJointBimanualObjLevelControlData>,
        public NJointBimanualObjLevelControllerInterface
    {
    public:
        //        using ConfigPtrT = BimanualForceControllerConfigPtr;
        NJointBimanualObjLevelController(const RobotUnitPtr&, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
 
        // NJointControllerInterface interface
        std::string getClassName(const Ice::Current&) const;
 
        // NJointController interface
 
        void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
 
        // NJointCCDMPControllerInterface interface
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
        bool isFinished(const Ice::Current&)
        {
            return finished;
        }
        Eigen::Matrix3f skew(Eigen::Vector3f vec)
        {
            Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
            mat(1, 2) = -vec(0);
            mat(0, 2) = vec(1);
            mat(0, 1) = -vec(2);
            mat(2, 1) = vec(0);
            mat(2, 0) = -vec(1);
            mat(1, 0) = vec(2);
            return mat;
        }
 
        //        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
        void runDMP(const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
        void runDMPWithVirtualStart(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals, Ice::Double timeDuration, const Ice::Current&);
 
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
        void removeAllViaPoints(const Ice::Current&);
 
        double getVirtualTime(const Ice::Current&)
        {
            return virtualtimer;
        }
 
        void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
        void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
        void setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
        void setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
        void setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
 
        std::vector<float> getCurrentObjVel(const Ice::Current&);
        std::vector<float> getCurrentObjForce(const Ice::Current&);
 
        void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
        DoubleSeqSeq getMPWeights(const Ice::Current&);
 
        void setMPRotWeights(const DoubleSeqSeq& weights, const Ice::Current&);
        DoubleSeqSeq getMPRotWeights(const Ice::Current&);
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
        void onInitNJointController();
        void onDisconnectNJointController();
        void controllerRun();
 
    private:
        Eigen::VectorXf targetWrench;
        struct DebugBufferData
        {
            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 leftQuat_w;
            float leftQuat_x;
            float leftQuat_y;
            float leftQuat_z;
            float rightQuat_w;
            float rightQuat_x;
            float rightQuat_y;
            float rightQuat_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;
        };
        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;
            Eigen::Matrix4f currentObjPose;
            Eigen::Vector3f currentObjVel;
            Eigen::Vector3f currentObjForce;
        };
 
        TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
 
        struct Inferface2rtData
        {
            Eigen::VectorXf KpImpedance;
            Eigen::VectorXf KdImpedance;
            Eigen::VectorXf KmAdmittance;
            Eigen::VectorXf KpAdmittance;
            Eigen::VectorXf KdAdmittance;
        };
        TripleBuffer<Inferface2rtData> interface2rtBuffer;
 
 
 
        std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
        std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> leftPositionSensors;
 
        std::vector<ControlTarget1DoFActuatorTorque*> rightTargets;
        std::vector<const SensorValue1DoFActuatorAcceleration*> rightAccelerationSensors;
        std::vector<const SensorValue1DoFActuatorVelocity*> rightVelocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> rightPositionSensors;
 
        const SensorValueForceTorque* rightForceTorque;
        const SensorValueForceTorque* leftForceTorque;
 
        NJointBimanualObjLevelControllerConfigPtr cfg;
        VirtualRobot::DifferentialIKPtr leftIK;
        VirtualRobot::DifferentialIKPtr rightIK;
 
        tsvmp::TaskSpaceDMPControllerPtr objectDMP;
 
        double virtualtimer;
 
        mutable MutexType controllerMutex;
        mutable MutexType interfaceDataMutex;
        Eigen::VectorXf leftDesiredJointValues;
        Eigen::VectorXf rightDesiredJointValues;
 
        Eigen::Matrix4f leftInitialPose;
        Eigen::Matrix4f rightInitialPose;
        Eigen::Matrix4f boxInitialPose;
 
        Eigen::VectorXf KpImpedance;
        Eigen::VectorXf KdImpedance;
        Eigen::VectorXf KpAdmittance;
        Eigen::VectorXf KdAdmittance;
        Eigen::VectorXf KmAdmittance;
        Eigen::VectorXf KmPID;
 
        Eigen::VectorXf virtualAcc;
        Eigen::VectorXf virtualVel;
        Eigen::Matrix4f virtualPose;
 
        Eigen::Matrix4f sensorFrame2TcpFrameLeft;
        Eigen::Matrix4f sensorFrame2TcpFrameRight;
 
        //static compensation
        float massLeft;
        Eigen::Vector3f CoMVecLeft;
        Eigen::Vector3f forceOffsetLeft;
        Eigen::Vector3f torqueOffsetLeft;
 
        float massRight;
        Eigen::Vector3f CoMVecRight;
        Eigen::Vector3f forceOffsetRight;
        Eigen::Vector3f torqueOffsetRight;
 
        //        float knull;
        //        float dnull;
 
        std::vector<std::string> leftJointNames;
        std::vector<std::string> rightJointNames;
 
        //        float torqueLimit;
        VirtualRobot::RobotNodeSetPtr leftRNS;
        VirtualRobot::RobotNodeSetPtr rightRNS;
        VirtualRobot::RobotNodePtr tcpLeft;
        VirtualRobot::RobotNodePtr tcpRight;
 
        std::vector<PIDControllerPtr> forcePIDControllers;
 
        // filter parameters
        float filterCoeff;
        Eigen::VectorXf filteredOldValue;
        bool finished;
        bool dmpStarted;
        double ftcalibrationTimer;
        Eigen::VectorXf ftOffset;
 
        Eigen::Matrix3f fixedLeftRightRotOffset;
        Eigen::Vector3f objCom2TCPLeftInObjFrame, objCom2TCPRightInObjFrame;
 
    protected:
        void rtPreActivateController();
        bool firstLoop;
    };
 
} // namespace armarx