NJointBimanualObjLevelMultiMPController.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/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 SensorValue1DoFActuatorAcceleration;
    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(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::control::deprecated_njoint_mp_controller::bimanual