NJointBimanualDMPForceController.h

Go to the documentation of this file.

 
#pragma once
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/CycleUtil.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/adaptive/ControllerInterface.h>
#include <armarx/control/deprecated_njoint_mp_controller/bimanual/ControllerInterface.h>
 
namespace armarx
{
    class SensorValue1DoFActuatorTorque;
    class SensorValue1DoFActuatorVelocity;
    class SensorValue1DoFActuatorPosition;
    class SensorValue1DoFActuatorAcceleration;
    class ControlTarget1DoFActuatorTorque;
    class SensorValueForceTorque;
} // namespace armarx
 
namespace armarx
{
    TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPController);
    TYPEDEF_PTRS_HANDLE(NJointBimanualCCDMPControllerControlData);
 
    using ViaPoint = std::pair<double, DMP::DVec>;
    using ViaPointsSet = std::vector<ViaPoint>;
 
    class NJointBimanualCCDMPControllerControlData
    {
    public:
        Eigen::VectorXf leftTargetVel;
        Eigen::VectorXf rightTargetVel;
 
        Eigen::Matrix4f leftTargetPose;
        Eigen::Matrix4f rightTargetPose;
    };
 
    class NJointBimanualCCDMPController :
        public NJointControllerWithTripleBuffer<NJointBimanualCCDMPControllerControlData>,
        public NJointBimanualCCDMPControllerInterface
    {
    public:
        using ConfigPtrT = NJointBimanualCCDMPControllerConfigPtr;
        NJointBimanualCCDMPController(NJointControllerDescriptionProviderInterfacePtr prov,
                                      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);
 
        // NJointBimanualCCDMPControllerInterface interface
        void learnDMPFromFiles(const std::string&, const Ice::StringSeq&, const Ice::Current&);
 
        bool
        isFinished(const Ice::Current&)
        {
            return false;
        }
 
        void runDMP(const Ice::DoubleSeq& leftGoals,
                    const Ice::DoubleSeq& rightGoals,
                    const Ice::Current&);
        void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
        void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
 
        void changeLeader(const Ice::Current&);
 
        double
        getVirtualTime(const Ice::Current&)
        {
            return virtualtimer;
        }
 
        std::string
        getLeaderName(const Ice::Current&)
        {
            return leaderName;
        }
 
    protected:
        virtual void onPublish(const SensorAndControl&,
                               const DebugDrawerInterfacePrx&,
                               const DebugObserverInterfacePrx&);
 
        void onInitComponent();
        void onDisconnectComponent();
        void controllerRun();
 
    private:
        Eigen::VectorXf getControlWrench(const Eigen::VectorXf& twist,
                                         const Eigen::Matrix4f& currentPose,
                                         const Eigen::Matrix4f& targetPose);
 
        Eigen::Matrix4f getLocalPose(const Eigen::Matrix4f& newCoordinate,
                                     const Eigen::Matrix4f& globalTargetPose);
 
        Eigen::Matrix4f
        getLocalPose(const std::vector<double>& newCoordinateVec,
                     const std::vector<double>& globalTargetPoseVec)
        {
            Eigen::Matrix4f newCoordinate =
                VirtualRobot::MathTools::quat2eigen4f(newCoordinateVec.at(4),
                                                      newCoordinateVec.at(5),
                                                      newCoordinateVec.at(6),
                                                      newCoordinateVec.at(3));
            newCoordinate(0, 3) = newCoordinateVec.at(0);
            newCoordinate(1, 3) = newCoordinateVec.at(1);
            newCoordinate(2, 3) = newCoordinateVec.at(2);
 
            Eigen::Matrix4f globalTargetPose =
                VirtualRobot::MathTools::quat2eigen4f(globalTargetPoseVec.at(4),
                                                      globalTargetPoseVec.at(5),
                                                      globalTargetPoseVec.at(6),
                                                      globalTargetPoseVec.at(3));
            globalTargetPose(0, 3) = globalTargetPoseVec.at(0);
            globalTargetPose(1, 3) = globalTargetPoseVec.at(1);
            globalTargetPose(2, 3) = globalTargetPoseVec.at(2);
 
            return getLocalPose(newCoordinate, globalTargetPose);
        }
 
        struct DebugBufferData
        {
            StringFloatDictionary desired_torques;
            StringFloatDictionary constrained_force;
            float leftTargetPose_x;
            float leftTargetPose_y;
            float leftTargetPose_z;
            float rightTargetPose_x;
            float rightTargetPose_y;
            float rightTargetPose_z;
 
            float leftCurrentPose_x;
            float leftCurrentPose_y;
            float leftCurrentPose_z;
            float rightCurrentPose_x;
            float rightCurrentPose_y;
            float rightCurrentPose_z;
 
            //            StringFloatDictionary latestTargetVelocities;
            //            StringFloatDictionary dmpTargets;
            //            StringFloatDictionary currentPose;
 
            //            double leadermpcFactor;
            //            double leadererror;
            //            double leaderposError;
            //            double leaderoriError;
            //            double leaderCanVal;
 
            //            double followermpcFactor;
            //            double followererror;
            //            double followerposError;
            //            double followeroriError;
            //            double followerCanVal;
        };
 
        TripleBuffer<DebugBufferData> debugDataInfo;
 
        struct NJointBimanualCCDMPControllerSensorData
        {
            double currentTime;
            double deltaT;
            Eigen::Matrix4f currentLeftPose;
            Eigen::Matrix4f currentRightPose;
            Eigen::VectorXf currentLeftTwist;
            Eigen::VectorXf currentRightTwist;
        };
 
        TripleBuffer<NJointBimanualCCDMPControllerSensorData> controllerSensorData;
 
 
        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;
 
        NJointBimanualCCDMPControllerConfigPtr cfg;
        VirtualRobot::DifferentialIKPtr leftIK;
        VirtualRobot::DifferentialIKPtr rightIK;
 
        std::vector<TaskSpaceDMPControllerPtr> leftGroup;
        std::vector<TaskSpaceDMPControllerPtr> rightGroup;
        std::vector<TaskSpaceDMPControllerPtr> bothLeaderGroup;
 
 
        std::string leaderName;
 
        VirtualRobot::RobotNodePtr tcpLeft;
        VirtualRobot::RobotNodePtr tcpRight;
 
        double virtualtimer;
 
        mutable MutexType controllerMutex;
        PeriodicTask<NJointBimanualCCDMPController>::pointer_type controllerTask;
 
        Eigen::VectorXf leftDesiredJointValues;
        Eigen::VectorXf rightDesiredJointValues;
 
        float KoriFollower;
        float KposFollower;
        float DposFollower;
        float DoriFollower;
 
        Eigen::VectorXf forceC_des;
        float boxWidth;
 
        Eigen::Vector3f kpos;
        Eigen::Vector3f kori;
        Eigen::Vector3f dpos;
        Eigen::Vector3f dori;
        Eigen::VectorXf kpf;
        Eigen::VectorXf kif;
 
        float knull;
        float dnull;
 
        std::vector<std::string> leftJointNames;
        std::vector<std::string> rightJointNames;
 
        float torqueLimit;
        VirtualRobot::RobotNodeSetPtr leftRNS;
        VirtualRobot::RobotNodeSetPtr rightRNS;
        VirtualRobot::RobotNodeSetPtr rnsLeftBody;
        VirtualRobot::RobotNodeSetPtr rnsRightBody;
 
        Eigen::Vector3f filtered_leftForce;
        Eigen::Vector3f filtered_leftTorque;
        Eigen::Vector3f filtered_rightForce;
        Eigen::Vector3f filtered_rightTorque;
        float filterTimeConstant;
 
        std::vector<PIDControllerPtr> ftPIDController;
 
        Eigen::Vector3f offset_leftForce;
        Eigen::Vector3f offset_leftTorque;
        Eigen::Vector3f offset_rightForce;
        Eigen::Vector3f offset_rightTorque;
 
        bool isForceCompensateDone;
 
        // NJointBimanualCCDMPControllerInterface interface
    };
 
} // namespace armarx