NJointBimanualObjLevelVelController.h

Go to the documentation of this file.

#pragma once
 
#include <VirtualRobot/VirtualRobot.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/ObjLevelControllerInterface.h>
 
namespace armarx
{
    class SensorValue1DoFActuatorTorque;
    class SensorValue1DoFActuatorVelocity;
    class SensorValue1DoFActuatorPosition;
    class ControlTarget1DoFActuatorVelocity;
    class ControlTarget1DoFActuatorTorque;
    class SensorValueForceTorque;
} // namespace armarx
 
namespace armarx::control::deprecated_njoint_mp_controller::bimanual
{
    namespace tsvmp = armarx::control::deprecated_njoint_mp_controller::tsvmp;
 
    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelVelController);
    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelVelControlData);
 
    class NJointBimanualObjLevelVelControlData
    {
    public:
        // control target from Movement Primitives
        Eigen::Matrix4f boxPose = Eigen::Matrix4f::Identity();
    };
 
    class NJointBimanualObjLevelVelController :
        public NJointControllerWithTripleBuffer<NJointBimanualObjLevelVelControlData>,
        public NJointBimanualObjLevelVelControllerInterface
    {
    public:
        //        using ConfigPtrT = BimanualForceControllerConfigPtr;
        NJointBimanualObjLevelVelController(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&);
 
        std::vector<float> getCurrentObjVel(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&);
        Eigen::VectorXf calcIK(VirtualRobot::DifferentialIKPtr ik,
                               const Eigen::MatrixXf& jacobi,
                               const Eigen::VectorXf& cartesianVel,
                               const Eigen::VectorXf& nullspaceVel);
 
    protected:
        virtual void onPublish(const SensorAndControl&,
                               const DebugDrawerInterfacePrx&,
                               const DebugObserverInterfacePrx&);
 
        void onInitNJointController();
        void onDisconnectNJointController();
        void controllerRun();
 
    private:
        Eigen::VectorXf targetWrench;
 
        struct DebugBufferData
        {
            StringFloatDictionary desired_vels;
 
            float virtualPose_x;
            float virtualPose_y;
            float virtualPose_z;
 
            float currentPoseLeft_x;
            float currentPoseLeft_y;
            float currentPoseLeft_z;
            float leftQuat_w;
            float leftQuat_x;
            float leftQuat_y;
            float leftQuat_z;
 
            float currentPoseRight_x;
            float currentPoseRight_y;
            float currentPoseRight_z;
            float rightQuat_w;
            float rightQuat_x;
            float rightQuat_y;
            float rightQuat_z;
 
 
            float dmpBoxPose_x;
            float dmpBoxPose_y;
            float dmpBoxPose_z;
 
            float dmpBoxPose_qx;
            float dmpBoxPose_qy;
            float dmpBoxPose_qz;
            float dmpBoxPose_qw;
        };
 
        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;
        };
 
        TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
 
        struct Inferface2rtData
        {
            Eigen::VectorXf KpImpedance;
            Eigen::VectorXf KdImpedance;
        };
 
        TripleBuffer<Inferface2rtData> interface2rtBuffer;
 
        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;
 
        NJointBimanualObjLevelVelControllerConfigPtr 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;
 
        CartesianVelocityControllerPtr leftTCPController;
        CartesianVelocityControllerPtr rightTCPController;
 
        std::vector<PIDControllerPtr> forcePIDControllers;
 
        // filter parameters
        float filterCoeff;
        Eigen::VectorXf filteredOldValue;
        bool finished;
        bool dmpStarted;
        Eigen::VectorXf ftOffset;
        Eigen::Matrix4f dmpGoal;
 
        Eigen::Matrix3f fixedLeftRightRotOffset;
        Eigen::Vector3f objCom2TCPLeftInObjFrame, objCom2TCPRightInObjFrame;
 
    protected:
        void rtPreActivateController();
        bool firstLoop;
    };
 
} // namespace armarx::control::deprecated_njoint_mp_controller::bimanual