Class CartesianAdmittance

Inheritance Relationships

Base Type

Class Documentation

class CartesianAdmittance : public OpenSoT::tasks::velocity::Cartesian

The CartesianAdmittance class implements a simple admittance controller in velocity at the end-effectors.

The implemented scheme is the following:

\( \boldsymbol{\Delta}\mathbf{x}_r = \mathbf{C}\boldsymbol{\Delta}\mathbf{F} \)

where \( \mathbf{C} \in \mathbb{R}^{6 \times 6}\) is the Compliance matrix. The torque error is computed as:

\( \boldsymbol{\Delta}\mathbf{F} = \mathbf{F}_d - \mathbf{F}_m \)

with \( \mathbf{F}_m \) transformed in the base frame of the Cartesian task. The computed Cartesian velocity reference is plugged in the Cartesian task as a feed-forward desired Cartesian velocity:

\( \boldsymbol{\Delta}\mathbf{x}_d = \boldsymbol{\Delta}\mathbf{x}_r + \lambda \left( \mathbf{x}_d - \mathbf{x}\right) \\ \)

NOTE: the task distal_link is the same link of the force/torque sensor

Public Types

typedef std::shared_ptr<CartesianAdmittance> Ptr

Public Functions

CartesianAdmittance(std::string task_id, XBot::ModelInterface &robot, std::string base_link, XBot::ForceTorqueSensor::ConstPtr ft_sensor)

CartesianAdmittance constructor.

Parameters

  • task_id – name of the task

  • x – the robot configuration

  • robot – model

  • base_link – base link of the task

  • ft_sensor – used to get measured contact forces and retrieve the distal_link

const Eigen::Matrix6d &getCartesianCompliance()

getCartesianCompliance

Returns

the actual Compliance matrix

void getCartesianCompliance(Eigen::Matrix6d &C)

getCartesianCompliance

Parameters

C – the actual Compliance matrix

void setWrenchReference(const Eigen::Vector6d &wrench)

setWrenchReference set the desired wrench at the controlled distal_link

Parameters

wrench –

const Eigen::Vector6d &getWrenchReference()

getWrenchReference

Returns

the actual reference wrench

void getWrenchReference(Eigen::Vector6d &wrench_reference)

getWrenchReference

Parameters

wrench_reference – the actual reference wrench

virtual bool reset()

reset is used to reset the internal references of the Task (and Cartesian Task) The reference wrench is reset to the one measured by the Force/Torque sensors

Returns

true

virtual void _log(XBot::MatLogger2::Ptr logger)

_log can be used to log internal Task variables

Parameters

logger – a shared pointer to a MatLogger

const Eigen::Matrix6d getStiffness()

getStiffness

Returns

actual Cartesian Stiffness

const Eigen::Matrix6d getInertia()

getStiffness

Returns

actual Cartesian Stiffness

const Eigen::Matrix6d getDamping()

getDamping

Returns

actual Cartesian Damping

double getFilterTimeStep()
void setFilterDamping(const double damping)
void setImpedanceParams(const Eigen::Vector6d &K, const Eigen::Vector6d &D, const double lambda, const double dt)

Set impedance parameters to be emulated via admittance control.

Stiffness and lambda MUST BE POSITIVE! Higher values of lambda will make the robot feel lighter, but will also destabilize the system.

Parameters

  • K – Stiffness

  • D – Damping

  • lambda – Lambda

  • dt – Control period

bool setRawParams(const Eigen::Vector6d &C, const Eigen::Vector6d &omega, const double lambda, const double dt)

Set internal controller parameters implemeting the law:

dx = lambda*(xd-x)+C*F_filt*dt F_filt = 1 / (1 + s/omega) * F

Parameters

  • C – compliance

  • omega – filter cutoff

  • lambda – position feedback

  • dt – control period

bool setDeadZone(const Eigen::Vector6d &dead_zone_amplitude)
bool computeParameters(const Eigen::Vector6d &K, const Eigen::Vector6d &D, const double lambda, const double dt, Eigen::Vector6d &C, Eigen::Vector6d &M, Eigen::Vector6d &w)

computeParameters given user’s K, D and lambda, computes M and w

Parameters

  • K – user desired Cartesian Stiffness

  • D – user desired Cartesian Damping

  • lambda – user desired lambda

  • dt – filter time step

  • C – Cartesian Compliance \( \mathbf{C} = \lambda\mathbf{K}^{-1} \)

  • M – Cartesian Mass \( \mathbf{M} = \frac{dt}{\lambda}\left( \mathbf{D} - \mathbf{D}dt \right) \)

  • w – filter params \( \mathbf{w} = dt\left( \mathbf{CM} \right)^{-1} \)

Returns

true

inline virtual void setLambda(double lambda)

setLambda do nothing in CartesianAdmittance, use instead the setRawParams() or the setImpedanceParams()

Parameters

lambda –

Public Static Functions

static bool isCartesianAdmittance(OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>::TaskPtr task)

isCartesianAdmittance

Parameters

task – a generic task pointer

Returns

true if task is a JointAdmittance task

static OpenSoT::tasks::velocity::CartesianAdmittance::Ptr asCartesianAdmittance(OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>::TaskPtr task)

asCartesianAdmittance

Parameters

task – a generic task pointer

Returns

a pointer to a JointAdmittance task