NJointBimanualObjLevelMultiMPController.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(NJointBimanualObjLevelMultiMPController);
    TYPEDEF_PTRS_HANDLE(NJointBimanualObjLevelMultiMPControlData);
 
    class NJointBimanualObjLevelMultiMPControlData
    {
    public:
        // control target from Movement Primitives
        Eigen::Matrix4f objTargetPose;
        Eigen::VectorXf objTargetTwist;
 
        Eigen::Matrix4f leftTargetPoseInObj;
        Eigen::VectorXf leftTargetTwistInObj;
 
        Eigen::Matrix4f rightTargetPoseInObj;
        Eigen::VectorXf rightTargetTwistInObj;
    };
 
 
    class NJointBimanualObjLevelMultiMPController :
        public NJointControllerWithTripleBuffer<NJointBimanualObjLevelMultiMPControlData>,
        public NJointBimanualObjLevelMultiMPControllerInterface
    {
    public:
        //        using ConfigPtrT = BimanualForceControllerConfigPtr;
        NJointBimanualObjLevelMultiMPController(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&);
        void learnMultiDMPFromFiles(const Ice::StringSeq& objFileNames, const Ice::StringSeq& leftFileNames, const Ice::StringSeq& rightFileNames, 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& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, 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 setMultiMPGoals(const Ice::DoubleSeq& goalObj, const Ice::DoubleSeq& goalLeft, const Ice::DoubleSeq& goalRight, const Ice::Current& ice);
 
        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 getObjStatus(Eigen::Matrix4f& pose, Eigen::VectorXf& twist);
        std::vector<double> eigenPose2Vec(const Eigen::Matrix4f& pose);
        Eigen::VectorXf eigenPose2EigenVec(const Eigen::Matrix4f& pose);
        Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate, const Eigen::Matrix4f& globalTargetPose);
        Eigen::Matrix4f getLocalPose(const std::vector<double>& newCoordinateVec, const std::vector<double>& globalTargetPoseVec);
        void integrateVel2Pose(const double deltaT, Eigen::VectorXf& vel, Eigen::Matrix4f& pose);
        void publishVec(const Eigen::VectorXf& bufferVec, const std::string name, StringVariantBaseMap& datafields);
        void setAmplitude(Ice::Double amp, const Ice::Current&);
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
        void onInitNJointController();
        void onDisconnectNJointController();
        void controllerRun();
 
    private:
        Eigen::VectorXf targetWrench;
        struct RT2DebugData
        {
            StringFloatDictionary desired_torques;
 
            // dmp targets
            Eigen::VectorXf objTargetPoseVec;
            Eigen::VectorXf leftPoseVecInObj;
            Eigen::VectorXf rightPoseVecInObj;
            Eigen::VectorXf objTargetTwist;
 
            // hand poses
            Eigen::VectorXf targetHandPoseInRootLeft;
            Eigen::VectorXf targetHandPoseInRootRight;
            Eigen::VectorXf currentHandPoseInRootLeft;
            Eigen::VectorXf currentHandPoseInRootRight;
 
            // object pose, vel and force torque
            Eigen::VectorXf objForceTorque;
            Eigen::VectorXf objPoseVec;
            Eigen::VectorXf objCurrentTwist;
 
            // virtual pose, vel, acc
            Eigen::VectorXf virtualPoseVec;
            Eigen::VectorXf virtualVel;
            Eigen::VectorXf virtualAcc;
 
            // integrated pose
            Eigen::VectorXf integratedPoseObjVec;
            Eigen::VectorXf integratedPoseLeftVec;
            Eigen::VectorXf integratedPoseRightVec;
 
            Eigen::VectorXf poseError;
 
            // force
            Eigen::VectorXf forceImpedance;
            Eigen::VectorXf forcePID;
            Eigen::VectorXf forcePIDControlValue;
            Eigen::VectorXf forceTorqueMeasurementInRoot;
 
            // parameters
            Eigen::VectorXf KpImpedance;
            Eigen::VectorXf KdImpedance;
            Eigen::VectorXf KpAdmittance;
            Eigen::VectorXf KdAdmittance;
            Eigen::VectorXf KmAdmittance;
            Eigen::VectorXf KmPID;
        };
        TripleBuffer<RT2DebugData> rt2DebugBuffer;
 
        struct RT2ControlData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPoseObj;
            Eigen::VectorXf currentTwistObj;
 
            Eigen::Matrix4f currentPoseLeftInObj;
            Eigen::VectorXf currentTwistLeftInObj;
 
            Eigen::Matrix4f currentPoseRightInObj;
            Eigen::VectorXf currentTwistRightInObj;
        };
        TripleBuffer<RT2ControlData> rt2ControlBuffer;
 
        struct RT2InterfaceData
        {
            Eigen::Matrix4f currentLeftPoseInObjFrame;
            Eigen::Matrix4f currentRightPoseInObjFrame;
            Eigen::Matrix4f currentObjPose;
            Eigen::Vector3f currentObjVel;
            Eigen::Vector3f currentObjForce;
        };
        TripleBuffer<RT2InterfaceData> rt2InterfaceBuffer;
 
        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;
 
        NJointBimanualObjLevelMultiMPControllerConfigPtr cfg;
        VirtualRobot::DifferentialIKPtr leftIK;
        VirtualRobot::DifferentialIKPtr rightIK;
 
        tsvmp::TaskSpaceDMPControllerPtr objectDMP;
        tsvmp::TaskSpaceDMPControllerPtr leftTCPInObjDMP;
        tsvmp::TaskSpaceDMPControllerPtr rightTCPInObjDMP;
 
        Eigen::Matrix4f objInitialPose;   // in root frame
        Eigen::Matrix4f leftInitialPose;  // in obj frame
        Eigen::Matrix4f rightInitialPose; // in obj frame
 
        // add integrated pose by accumulating dmp target velocity to initial pose
        Eigen::Matrix4f integratedPoseObj;
        Eigen::Matrix4f integratedPoseLeft;
        Eigen::Matrix4f integratedPoseRight;
 
        Eigen::Matrix4f objTransMatrixInAnchor;
 
        double virtualtimer;
 
        mutable MutexType controllerMutex;
        mutable MutexType interfaceDataMutex;
        Eigen::VectorXf leftDesiredJointValues;
        Eigen::VectorXf rightDesiredJointValues;
 
        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