NJointPeriodicTSDMPCompliantController.h

Go to the documentation of this file.

 
#pragma once
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Manipulability/SingleChainManipulability.h>
#include <VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h>
 
#include <ArmarXCore/core/time/CycleUtil.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>
 
// control
#include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
#include <armarx/control/deprecated_njoint_mp_controller/TaskSpaceVMP.h>
 
 
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
    namespace tsvmp = armarx::control::deprecated_njoint_mp_controller::tsvmp;
 
    TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPCompliantController);
    TYPEDEF_PTRS_HANDLE(NJointPeriodicTSDMPCompliantControllerControlData);
 
    class NJointPeriodicTSDMPCompliantControllerControlData
    {
    public:
        Eigen::VectorXf targetTSVel;
    };
 
    /**
     * @brief The NJointPeriodicTSDMPCompliantController class
     * @ingroup Library-RobotUnit-NJointControllers
     */
    class NJointPeriodicTSDMPCompliantController :
        public NJointControllerWithTripleBuffer<NJointPeriodicTSDMPCompliantControllerControlData>,
        public NJointPeriodicTSDMPCompliantControllerInterface
    {
    public:
        using ConfigPtrT = NJointPeriodicTSDMPCompliantControllerConfigPtr;
        NJointPeriodicTSDMPCompliantController(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);
 
        // NJointPeriodicTSDMPCompliantControllerInterface 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;
        }
 
    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 reactForce;
            Eigen::Vector3f adaptK;
            Eigen::VectorXf targetVel;
            Eigen::Matrix4f currentPose;
            Eigen::VectorXf currentTwist;
            bool isPhaseStop;
            float manidist;
        };
        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;
            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;
 
        // velocity ik controller parameters
        std::string nodeSetName;
 
        bool started;
        bool firstRun;
        bool dmpRunning;
 
        VirtualRobot::DifferentialIKPtr ik;
        VirtualRobot::RobotNodePtr tcp;
 
        NJointPeriodicTSDMPCompliantControllerConfigPtr cfg;
        mutable MutexType controllerMutex;
        PeriodicTask<NJointPeriodicTSDMPCompliantController>::pointer_type controllerTask;
        Eigen::Matrix4f targetPose;
 
        Eigen::Vector3f kpos;
        Eigen::Vector3f dpos;
        Eigen::Vector3f kori;
        Eigen::Vector3f dori;
        float knull;
        float dnull;
        float kpf;
 
        Eigen::VectorXf nullSpaceJointsVec;
        const SensorValueForceTorque* forceSensor;
 
        Eigen::Vector3f filteredForce;
        Eigen::Vector3f forceOffset;
        Eigen::Vector3f filteredForceInRoot;
 
        Eigen::Matrix3f toolTransform;
        Eigen::Vector3f oriToolDir;
        Eigen::Matrix3f origHandOri;
        Eigen::VectorXf qvel_filtered;
 
        Eigen::Vector3f adaptK;
        float lastDiff;
        Eigen::Vector2f lastPosition;
        double changeTimer;
 
 
 
        bool isManipulability = false;
        VirtualRobot::SingleChainManipulabilityTrackingPtr manipulabilityTracker;
        Eigen::MatrixXd targetManipulability;
        Eigen::MatrixXd kmani;
 
    };
 
} // namespace armarx