NJointBimanualForceController.h

Go to the documentation of this file.

#pragma once
 
 
// Simox
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
 
// RobotAPI
#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/ForceMPControllerInterface.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(NJointBimanualForceController);
    TYPEDEF_PTRS_HANDLE(NJointBimanualForceControlData);
 
    class NJointBimanualForceControlData
    {
    public:
        // from dmp
        Eigen::Matrix4f boxPose;
        Eigen::VectorXf boxTwist;
        //        Eigen::VectorXf leftTargetTwist;
        //        Eigen::VectorXf rightTargetTwist;
 
        //        Eigen::Matrix4f leftTargetPose;
        //        Eigen::Matrix4f rightTargetPose;
 
        //        std::vector<float> nullspaceJointVelocities;
        //        double virtualTime;
    };
 
 
    class NJointBimanualForceController :
        public NJointControllerWithTripleBuffer<NJointBimanualForceControlData>,
        public NJointBimanualForceControllerInterface
    {
    public:
        //        using ConfigPtrT = BimanualForceControllerConfigPtr;
        NJointBimanualForceController(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&);
        bool isFinished(const Ice::Current&)
        {
            return finished;
        }
 
        //        void runDMP(const Ice::DoubleSeq& goals, Ice::Double tau, const Ice::Current&);
        void runDMP(const Ice::DoubleSeq& goals, const Ice::Current&);
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
        double getVirtualTime(const Ice::Current&)
        {
            return virtualtimer;
        }
 
    protected:
        virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
 
        void onInitNJointController();
        void onDisconnectNJointController();
        void controllerRun();
 
    private:
        Eigen::VectorXf targetWrench;
        struct DebugBufferData
        {
            StringFloatDictionary desired_torques;
 
            float modifiedPoseRight_x;
            float modifiedPoseRight_y;
            float modifiedPoseRight_z;
            float currentPoseLeft_x;
            float currentPoseLeft_y;
            float currentPoseLeft_z;
 
            float modifiedPoseLeft_x;
            float modifiedPoseLeft_y;
            float modifiedPoseLeft_z;
            float currentPoseRight_x;
            float currentPoseRight_y;
            float currentPoseRight_z;
 
            float dmpBoxPose_x;
            float dmpBoxPose_y;
            float dmpBoxPose_z;
 
            float dmpTwist_x;
            float dmpTwist_y;
            float dmpTwist_z;
 
            float modifiedTwist_lx;
            float modifiedTwist_ly;
            float modifiedTwist_lz;
            float modifiedTwist_rx;
            float modifiedTwist_ry;
            float modifiedTwist_rz;
 
            float rx;
            float ry;
            float rz;
 
            Eigen::VectorXf wrenchDMP;
            Eigen::VectorXf computedBoxWrench;
 
            Eigen::VectorXf forceImpedance;
            Eigen::VectorXf forcePID;
            Eigen::VectorXf forcePIDControlValue;
            Eigen::VectorXf poseError;
            Eigen::VectorXf wrenchesConstrained;
            Eigen::VectorXf wrenchesMeasuredInRoot;
        };
        TripleBuffer<DebugBufferData> debugOutputData;
 
        struct NJointBimanualForceControllerSensorData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentPose;
            Eigen::VectorXf currentTwist;
        };
        TripleBuffer<NJointBimanualForceControllerSensorData> controllerSensorData;
 
        struct NJointBimanualForceControllerInterfaceData
        {
            Eigen::Matrix4f currentLeftPose;
            Eigen::Matrix4f currentRightPose;
        };
 
        TripleBuffer<NJointBimanualForceControllerInterfaceData> interfaceData;
 
        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;
 
        NJointBimanualForceControllerConfigPtr cfg;
        VirtualRobot::DifferentialIKPtr leftIK;
        VirtualRobot::DifferentialIKPtr rightIK;
 
        tsvmp::TaskSpaceDMPControllerPtr objectDMP;
 
 
 
        double virtualtimer;
 
        mutable MutexType controllerMutex;
        Eigen::VectorXf leftDesiredJointValues;
        Eigen::VectorXf rightDesiredJointValues;
 
        Eigen::Matrix4f leftInitialPose;
        Eigen::Matrix4f rightInitialPose;
        Eigen::Matrix4f boxInitialPose;
 
        Eigen::VectorXf KpImpedance;
        Eigen::VectorXf KdImpedance;
        Eigen::VectorXf KpAdmittance;
        Eigen::VectorXf KdAdmittance;
        Eigen::VectorXf KmAdmittance;
        Eigen::VectorXf KmPID;
 
        Eigen::VectorXf modifiedAcc;
        Eigen::VectorXf modifiedTwist;
        Eigen::Matrix4f modifiedLeftPose;
        Eigen::Matrix4f modifiedRightPose;
 
        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;
    protected:
        //        void rtPreActivateController();
        bool firstLoop;
    };
 
} // namespace armarx