NJointCartesianWaypointController.h

Go to the documentation of this file.

#pragma once
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
#include <RobotAPI/libraries/core/CartesianWaypointController.h>
 
namespace armarx
{
 
    TYPEDEF_PTRS_HANDLE(NJointCartesianWaypointController);
 
    /**
     * @brief The NJointCartesianWaypointController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointCartesianWaypointController :
        public NJointController,
        public NJointCartesianWaypointControllerInterface
    {
    public:
        using ConfigPtrT = NJointCartesianWaypointControllerConfigPtr;
        NJointCartesianWaypointController(RobotUnit* robotUnit,
                                          const NJointCartesianWaypointControllerConfigPtr& config,
                                          const VirtualRobot::RobotPtr&);
 
        // NJointControllerInterface interface
        std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
        //        WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
        //        void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current& = Ice::emptyCurrent) override;
 
        // NJointController interface
        void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                   const IceUtil::Time& timeSinceLastIteration) override;
        //        static WidgetDescription::WidgetPtr GenerateConfigDescription(
        //            const VirtualRobot::RobotPtr&,
        //            const std::map<std::string, ConstControlDevicePtr>&,
        //            const std::map<std::string, ConstSensorDevicePtr>&);
 
        //        static NJointCartesianWaypointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
        //        NJointCartesianWaypointController(
        //            RobotUnitPtr prov,
        //            NJointCartesianWaypointControllerConfigPtr config,
        //            const VirtualRobot::RobotPtr& r);
 
        void onPublish(const SensorAndControl&,
                       const DebugDrawerInterfacePrx&,
                       const DebugObserverInterfacePrx&) override;
 
    public:
        bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
        bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
        int getCurrentWaypointIndex(const Ice::Current& = Ice::emptyCurrent) override;
 
        void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
                       const Ice::Current& = Ice::emptyCurrent) override;
        void setWaypoints(const std::vector<Eigen::Matrix4f>& wps,
                          const Ice::Current& = Ice::emptyCurrent) override;
        void setWaypointAx(const Ice::FloatSeq& data,
                           const Ice::Current& = Ice::emptyCurrent) override;
        void setWaypoint(const Eigen::Matrix4f& wp,
                         const Ice::Current& = Ice::emptyCurrent) override;
        void setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
                                   const std::vector<Eigen::Matrix4f>& wps,
                                   const Ice::Current& = Ice::emptyCurrent) override;
        void setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
                                  const Eigen::Matrix4f& wp,
                                  const Ice::Current& = Ice::emptyCurrent) override;
        void stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
 
        FTSensorValue getFTSensorValue(const Ice::Current& = Ice::emptyCurrent) override;
        void setCurrentFTAsOffset(const Ice::Current& = Ice::emptyCurrent) override;
 
        void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
                                             const Ice::Current& = Ice::emptyCurrent) override;
        void resetVisualizationRobotGlobalPose(const Ice::Current& = Ice::emptyCurrent) override;
 
    protected:
        void rtPreActivateController() override;
        void rtPostDeactivateController() override;
 
        //structs
    private:
        struct CtrlData
        {
            std::vector<Eigen::Matrix4f> wps;
            NJointCartesianWaypointControllerRuntimeConfig cfg;
            float skipRad2mmFactor = 500;
            bool wpsUpdated = false;
            bool cfgUpdated = false;
            bool skipToClosestWaypoint = true;
        };
 
        struct RtToNonRtData
        {
            FTSensorValue ft;
            Eigen::Matrix4f rootPose = Eigen::Matrix4f::Identity();
            Eigen::Matrix4f tcp = Eigen::Matrix4f::Identity();
            Eigen::Matrix4f tcpTarg = Eigen::Matrix4f::Identity();
            Eigen::Matrix4f ftInRoot = Eigen::Matrix4f::Identity();
            Eigen::Vector3f ftUsed = Eigen::Vector3f::Zero();
        };
 
        //data
    private:
        void setNullVelocity();
 
        using Ctrl = CartesianWaypointController;
 
        //rt data
        VirtualRobot::RobotPtr _rtRobot;
        VirtualRobot::RobotNodeSetPtr _rtRns;
        VirtualRobot::RobotNodePtr _rtTcp;
        VirtualRobot::RobotNodePtr _rtFT;
        VirtualRobot::RobotNodePtr _rtRobotRoot;
 
        std::unique_ptr<Ctrl> _rtWpController;
        Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
 
        std::vector<const float*> _rtVelSensors;
        std::vector<float*> _rtVelTargets;
 
        const Eigen::Vector3f* _rtForceSensor = nullptr;
        const Eigen::Vector3f* _rtTorqueSensor = nullptr;
 
        Eigen::Vector3f _rtForceOffset;
 
        float _rtForceThreshold = -1;
        bool _rtOptimizeNullspaceIfTargetWasReached = false;
        bool _rtForceThresholdInRobotRootZ = true;
        bool _rtHasWps = false;
        bool _rtStopConditionReached = false;
 
        //flags
        std::atomic_bool _publishIsAtTarget{false};
        std::atomic_bool _publishIsAtForceLimit{false};
        std::atomic_bool _setFTOffset{false};
 
        //buffers
        mutable std::recursive_mutex _tripBufWpCtrlMut;
        WriteBufferedTripleBuffer<CtrlData> _tripBufWpCtrl;
 
        mutable std::recursive_mutex _tripRt2NonRtMutex;
        TripleBuffer<RtToNonRtData> _tripRt2NonRt;
 
        TripleBuffer<Eigen::Matrix4f> _tripRt2NonRtRobotGP;
 
        //publish data
        std::atomic_size_t _publishWpsNum{0};
        std::atomic_size_t _publishWpsCur{0};
        std::atomic<float> _publishErrorPos{0};
        std::atomic<float> _publishErrorOri{0};
        std::atomic<float> _publishErrorPosMax{0};
        std::atomic<float> _publishErrorOriMax{0};
        std::atomic<float> _publishForceThreshold{0};
        std::atomic<float> _publishForceCurrent{0};
        std::atomic_bool _publishForceThresholdInRobotRootZ{0};
    };
} // namespace armarx