Class Cartesian

Defined in File Cartesian.h

Inheritance Relationships

Base Type

public OpenSoT::Task< Eigen::MatrixXd, Eigen::VectorXd > (Template Class Task)

Derived Type

public OpenSoT::tasks::velocity::CartesianAdmittance (Class CartesianAdmittance)

Class Documentation

class Cartesian : public OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>

The Cartesian class implements a task that tries to impose a pose (position and orientation) of a distal link w.r.t.

a base link. The reference for the cartesian task is set in base link coordinate frame, or in world if the base link name is set to “world”. When relative Cartesian task is required,for example from link A to link B, the relative velocity considered is the the one of B respect to A expressed in A. The Cartesian Task is implemented so that \(A={}^\text{base}J_\text{distal}\) and \(b=K_p*e+\frac{1}{\lambda}\xi_d\)

You can see an example in example_cartesian.cpp

Subclassed by OpenSoT::tasks::velocity::CartesianAdmittance

Public Types

typedef std::shared_ptr<Cartesian> Ptr

Public Functions

Cartesian(std::string task_id, XBot::ModelInterface &robot, const std::string &distal_link, const std::string &base_link)

Cartesian creates a new Cartesian task.

Parameters

task_id – an identifier for the task.
x – the robot configuration. The Cartesian task will be created so that the task error is zero in position x.
robot – the robot model. Cartesian expects the robot model to be updated externally.
distal_link – the name of the distal link as expressed in the robot urdf
base_link – the name of the base link as expressed in the robot urdf. Can be set to “world”

~Cartesian()

virtual void _update(): Updates the A, b, Aeq, beq, Aineq, b*Bound matrices.

void setReference(const Eigen::Affine3d &desiredPose)

setReference sets a new reference for the Cartesian task.

It causes the task error to be recomputed immediately, without the need to call the _update(x) function. It also assumes a null desired velocity at the desired pose, meaning we are trying to achieve a regulation task.

Parameters: desiredPose – the \(R^{4x4}\) homogeneous transform matrix describing the desired pose for the distal_link in the base_link frame of reference.

void setReference(const Eigen::Matrix4d &desiredPose)

void setReference(const KDL::Frame &desiredPose)

void setReference(const Eigen::Affine3d &desiredPose, const Eigen::Vector6d &desiredTwist)

setReference sets a new reference for the Cartesian task.

It causes the task error to be recomputed immediately, without the need to call the _update(x) function Notice how the setReference(desiredPose, desiredTwist) needs to be called before each _update(x) of the Cartesian task, since THE _update() RESETS THE FEED-FORWARD VELOCITY TERM for safety reasons.

Parameters

desiredPose – the \(R^{4x4}\) homogeneous transform matrix describing the desired pose for the distal_link in the base_link frame of reference.
desireTwist – is a \(R^{6}\) twist describing the desired trajectory velocity, and it represents a feed-forward term in the cartesian task computation. NOTICE how the velocities are in units/sample, instead of units/s. This means that if you have a twist expressed in SI units, you have to call the function as setReference(desiredPose, desiredTwist*dt)

void setReference(const Eigen::Matrix4d &desiredPose, const Eigen::Vector6d &desiredTwist)

void setReference(const KDL::Frame &desiredPose, const KDL::Twist &desiredTwist)

void setVelocityLocalReference(const Eigen::Vector6d &desiredTwist)

setVelocityLocalReference permits to set velocity expressed in local (ee) distal frame

Parameters: desireTwist – is a \(R^{6}\) twist describing the desired trajectory velocity, and it represents a feed-forward term in the cartesian task computation. NOTICE how the velocities are in units/sample, instead of units/s. This means that if you have a twist expressed in SI units, you have to call the function as setVelocityLocalReference(desiredTwist*dt)

void getReference(Eigen::Affine3d &desiredPose) const

getReference returns the Cartesian task reference

Returns: the Cartesian task reference \(R^{4x4}\) homogeneous transform matrix describing the desired pose for the distal_link in the base_link frame of reference.

const Eigen::Matrix4d &getReference() const

void getReference(KDL::Frame &desiredPose) const

void getReference(Eigen::Affine3d &desiredPose, Eigen::Vector6d &desiredTwist) const

getReference gets the current reference and feed-forward velocity for the Cartesian task.

Parameters

desiredPose – the \(R^{4x4}\) homogeneous transform matrix describing the desired pose for the distal_link in the base_link frame of reference.
desireVelocity – is a \(R^{6}\) twist describing the desired trajectory velocity, and it represents a feed-forward term in the cartesian task computation

void getReference(Eigen::Matrix4d &desiredPose, Eigen::Vector6d &desiredTwist) const

void getReference(KDL::Frame &desiredPose, KDL::Vector &desiredTwist) const

void getActualPose(Eigen::Affine3d &actual_pose) const

getActualPose returns the distal_link actual pose.

You need to call _update(x) for the actual pose to change

Returns: the \(R^{4x4}\) homogeneous transform matrix describing the actual pose for the distal_link in the base_link frame of reference.

const Eigen::Matrix4d &getActualPose() const

void getActualPose(KDL::Frame &actual_pose) const

const Eigen::Vector6d &getCachedVelocityReference() const

getCachedVelocityReference can be used to get Velocity reference after update(), it will reset next update()

Returns: internal velcity reference

void setOrientationErrorGain(const double &orientationErrorGain)

const double getOrientationErrorGain() const

const std::string &getDistalLink() const

const std::string &getBaseLink() const

const bool baseLinkIsWorld() const

virtual void setLambda(double lambda)

const Eigen::Vector6d &getError() const

getError returns the 6d cartesian error (position and orientation) between actual and reference pose

Returns: a \(R^{6}\) vector describing cartesian error between actual and reference pose

bool setBaseLink(const std::string &base_link)

setBaseLink change the base link of the task

Parameters: base_link – the new base link
Returns: false if the base link does not exists

bool setDistalLink(const std::string &distal_link)

Changes the distal link of the task.

It also resets the reference according to the current robot pose.

Parameters: distal_link – the new distal link
Returns: false if the distal link does not exists

virtual bool reset()

reset set as actual Cartesian reference the actual pose

Returns

void rotateToLocal(const bool rotate_to_local)

rotateToLocal rotates both Jacobian and references to local (ee) distal frame, this is mostly used for local subtasks

Parameters: rotate_to_local – default is false

Public Static Functions

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

static OpenSoT::tasks::velocity::Cartesian::Ptr asCartesian(OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>::TaskPtr task)

Protected Functions

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

_log can be used to log internal Task variables

Parameters: logger – a shared pointer to a MatLogger

void update_b()

Protected Attributes

XBot::ModelInterface &_robot

std::string _distal_link

std::string _base_link

int _distal_link_index

int _base_link_index

Eigen::Affine3d _actualPose

Eigen::Affine3d _desiredPose

Eigen::Vector6d _desiredTwist

Eigen::Vector6d _desiredTwistRef

bool _base_link_is_world

double _orientationErrorGain

bool _is_initialized

Eigen::Vector6d _error

Eigen::Affine3d _tmpMatrix

Eigen::Affine3d _tmpMatrix2

Eigen::Affine3d _base_T_distal: _base_T_distal is used to change new distal link!

Eigen::Vector3d positionError

Eigen::Vector3d orientationError

bool _rotate_to_local

bool _velocity_refs_are_local

Eigen::MatrixXd _tmp_A

Eigen::VectorXd _tmp_b

Eigen::Vector6d _tmp_twist