CartesianImpedanceController.h

Go to the documentation of this file.

#pragma once
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
 
namespace armarx
{
    class CartesianImpedanceController
    {
    public:
        struct Config
        {
            Eigen::Vector3f desiredPosition;
            Eigen::Quaternionf desiredOrientation;
            Eigen::Vector3f Kpos;
            Eigen::Vector3f Dpos;
            Eigen::Vector3f Kori;
            Eigen::Vector3f Dori;
            Eigen::VectorXf Knull;
            Eigen::VectorXf Dnull;
            Eigen::VectorXf desiredJointPosition;
            float torqueLimit;
        };
 
        //state for debugging
        struct Debug
        {
            float errorAngle;
            float posiError;
            Eigen::Vector3f tcpDesiredForce;
            Eigen::Vector3f tcpDesiredTorque;
        };
 
    private:
        Eigen::VectorXf qpos;
        Eigen::VectorXf qvel;
        Eigen::VectorXf tcptwist;
        Eigen::VectorXf nullqerror;
        Eigen::VectorXf nullspaceTorque;
        Eigen::VectorXf targetJointTorques;
 
        Eigen::MatrixXf I;
 
        VirtualRobot::DifferentialIKPtr ik;
        VirtualRobot::RobotNodePtr tcp;
 
        const float lambda = 2;
 
    public:
        Debug dbg;
 
        int bufferSize() const;
        void initialize(const VirtualRobot::RobotNodeSetPtr& rns);
        const Eigen::VectorXf&
        run(const Config& cfg,
            std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors,
            std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors,
            std::vector<const SensorValue1DoFActuatorPosition*> positionSensors);
    };
} // namespace armarx