NJointAnomalyDetectionAdaptiveWipingController.h

Go to the documentation of this file.

#pragma once
 
#include <ArmarXCore/core/time/CycleUtil.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
 
#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/CartesianVelocityController.h>
#include <RobotAPI/libraries/core/Trajectory.h>
#include <RobotAPI/libraries/core/PIDController.h>
 
// control
#include <armarx/control/deprecated_njoint_mp_controller/adaptive/ControllerInterface.h>
#include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
 
 
namespace armarx::control::deprecated_njoint_mp_controller::adaptive
{
    namespace tsvmp = armarx::control::deprecated_njoint_mp_controller::tsvmp;
 
    TYPEDEF_PTRS_HANDLE(NJointAnomalyDetectionAdaptiveWipingController);
    TYPEDEF_PTRS_HANDLE(NJointAnomalyDetectionAdaptiveWipingControllerControlData);
 
    class NJointAnomalyDetectionAdaptiveWipingControllerControlData
    {
    public:
        Eigen::VectorXf targetTSVel;
        Eigen::Matrix4f targetTSPose;
        double canVal;
    };
 
    /**
     * @brief The NJointAnomalyDetectionAdaptiveWipingController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointAnomalyDetectionAdaptiveWipingController :
        public NJointControllerWithTripleBuffer<NJointAnomalyDetectionAdaptiveWipingControllerControlData>,
        public NJointAnomalyDetectionAdaptiveWipingControllerInterface
    {
    public:
        using ConfigPtrT = NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr;
        NJointAnomalyDetectionAdaptiveWipingController(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);
 
        // NJointAnomalyDetectionAdaptiveWipingControllerInterface interface
        void learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&);
        void learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&);
        bool isFinished(const Ice::Current&)
        {
            return false;
        }
 
        void setSpeed(Ice::Double times, const Ice::Current&);
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
        void setAmplitude(Ice::Double amp, const Ice::Current&);
        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
        void setTargetForceInRootFrame(Ice::Float force, const Ice::Current&);
        double getCanVal(const Ice::Current&)
        {
            return dmpCtrl->canVal;
        }
        void setTrigerAbnormalEvent(bool abnormal, const Ice::Current&);
 
        std::vector<float> getAnomalyInput(const Ice::Current&);
        std::vector<float> getAnomalyOutput(const Ice::Current&);
 
        void pauseDMP(const Ice::Current&);
        void resumeDMP(const Ice::Current&);
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
        void onInitNJointController();
        void onDisconnectNJointController();
        void controllerRun();
 
    private:
        struct DebugBufferData
        {
            StringFloatDictionary latestTargetVelocities;
            StringFloatDictionary currentPose;
            double currentCanVal;
            double mpcFactor;
            double error;
            double phaseStop;
            double posError;
            double oriError;
            double deltaT;
        };
 
        TripleBuffer<DebugBufferData> debugOutputData;
 
 
        struct DebugRTData
        {
            Eigen::Matrix4f targetPose;
            Eigen::Vector3f filteredForce;
            Eigen::Vector3f filteredForceInRoot;
            Eigen::Vector3f filteredTorque;
 
            Eigen::Vector3f rotationAxis;
 
            Eigen::Vector3f reactForce;
            Eigen::Vector3f adaptK;
            Eigen::Vector3f adaptD;
            Eigen::VectorXf targetVel;
            Eigen::Matrix4f currentPose;
            bool isPhaseStop;
 
            Eigen::Matrix4f globalPose;
            Eigen::Vector3f globalFilteredForce;
            Eigen::Vector3f currentToolDir;
            Eigen::VectorXf currentTwist;
 
            float rotAngle;
 
            int wipingCounter;
            float mu;
            Eigen::Vector2f estimatedFriction;
            Eigen::Vector3f velPInTool;
            Eigen::Vector2f frictionInToolXY;
 
            float kpForcePID;
            float kpRotPID;
            float loseContactForceIntegral;
        };
        TripleBuffer<DebugRTData> debugRT;
 
 
        struct RTToControllerData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPose;
            Eigen::VectorXf currentTwist;
            bool isPhaseStop;
        };
        TripleBuffer<RTToControllerData> rt2CtrlData;
 
        struct RTToUserData
        {
            Eigen::Matrix4f currentTcpPose;
            Eigen::Vector3f tcpTranslVel;
            Eigen::Vector3f forceOutput;
            float waitTimeForCalibration;
        };
        TripleBuffer<RTToUserData> rt2UserData;
 
        struct UserToRTData
        {
            float targetForce;
        };
        TripleBuffer<UserToRTData> user2rtData;
 
 
        tsvmp::TaskSpaceDMPControllerPtr dmpCtrl;
 
        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors;
        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors;
        std::vector<ControlTarget1DoFActuatorTorque*> targets;
 
        // anomaly detection
        std::deque<Eigen::VectorXf> velocityHorizonList;
        size_t velocityHorizon;
        bool lastAbnormalFlag = false;
        bool abnormalFlag;
        bool startLoseContactDetection = false;
        float forceIntegral;
        int loseContactCounter = 0;
        bool isLoseContact = false;
 
        // velocity ik controller parameters
        std::string nodeSetName;
 
        bool started;
        bool firstRun;
        bool dmpRunning;
 
        // friction estimation
        float mu = 1.5f; // init friction coefficient
        Eigen::Vector2f lastForceInToolXY;
        double lastCanVal = 0.0;
        int wipingCounter = 0;
        std::deque<float> recordFrictionNorm;
        std::deque<float> recordForceNormToSurface;
        size_t frictionHorizon;
        Eigen::Vector2f estimatedFriction;
        float safeFrictionConeLowerLimit = 0.2;
 
        // lose contact detection
        float loseContactRatio = 0.2f;
        int makingContactCounter = 0;
        bool isMakingContact = false;
        bool isLCREnabled = false;
        bool isContactedOnce = false;
        float forceControlGate = 1.0;
        int lcrCounter = 500;
 
        VirtualRobot::DifferentialIKPtr ik;
        VirtualRobot::RobotNodePtr tcp;
 
        NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr cfg;
        mutable MutexType controllerMutex;
        PeriodicTask<NJointAnomalyDetectionAdaptiveWipingController>::pointer_type controllerTask;
        Eigen::Matrix4f targetPose;
        Eigen::Matrix4f initHandPose;
 
        Eigen::Vector3f kpos;
        Eigen::Vector3f dpos;
        Eigen::Vector3f kori;
        Eigen::Vector3f dori;
        Eigen::VectorXf knull;
        Eigen::VectorXf dnull;
 
        Eigen::Vector3f adaptK;
        Eigen::Vector3f adaptD;
        Eigen::Vector3f adaptKOri;
        Eigen::Vector3f adaptDOri;
        Eigen::VectorXf adaptKNull;
        Eigen::VectorXf adaptDNull;
 
        Eigen::VectorXf nullSpaceJointsVec;
        const SensorValueForceTorque* forceSensor;
 
        // pid controllers
        bool isForceCtrlInForceDir;
        bool isForceControlEnabled;
        bool isRotControlEnabled;
        bool isTorqueControlEnabled;
        bool isLCRControlEnabled;
        PIDControllerPtr forcePID;
        PIDControllerPtr rotPID;
        PIDControllerPtr torquePID;
        PIDControllerPtr lcrPID; // lose contact recover pid controller
        float adaptKpForce;
        float adaptKpRot;
 
        // force torque related
        Eigen::Vector6f targetFTInToolFrame;
        Eigen::Vector3f filteredForce;
        Eigen::Vector3f filteredTorque;
        Eigen::Vector6f filteredFTCommand;
        Eigen::Vector3f forceOffset;
        Eigen::Vector3f currentForceOffset;
 
        Eigen::Vector3f torqueOffset;
        Eigen::Vector3f currentTorqueOffset;
        float handMass;
        Eigen::Vector3f handCOM;
        Eigen::Vector3f gravityInRoot;
 
        Eigen::Vector3f filteredForceInRoot;
        Eigen::Vector3f filteredTorqueInRoot;
 
        Eigen::Matrix3f toolTransform;
        Eigen::Vector3f oriToolDir;
        Eigen::Matrix3f origHandOri;
        Eigen::VectorXf qvel_filtered;
 
        bool useDMPInGlobalFrame;
 
        float lastDiff;
        Eigen::Vector2f lastPosition;
        double changeTimer;
 
        Eigen::Vector3f toolToFTSensorLink;
        Eigen::Vector3f positionOffset;
 
    };
} // namespace armarx