mc_tasks::force::AdmittanceTask Struct Reference

Hybrid position-force control on a contacting end-effector. More...

#include <mc_tasks/AdmittanceTask.h>

Inheritance diagram for mc_tasks::force::AdmittanceTask:
Collaboration diagram for mc_tasks::force::AdmittanceTask:

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW AdmittanceTask (const mc_rbdyn::RobotFrame &frame, double stiffness=5.0, double weight=1000.0)
 Initialize a new admittance task. More...
 
 AdmittanceTask (const std::string &robotSurface, const mc_rbdyn::Robots &robots, unsigned robotIndex, double stiffness=5.0, double weight=1000.0)
 Initialize a new admittance task. More...
 
void reset () override
 Reset the task. More...
 
const sva::ForceVecd & admittance () const
 Get the admittance coefficients of the task. More...
 
void admittance (const sva::ForceVecd &admittance)
 Set the admittance coefficients of the task. More...
 
sva::PTransformd surfacePose () const
 Get the current pose of the control frame in the inertial frame. More...
 
sva::PTransformd targetPose ()
 Get the target pose of the control frame in the world frame. More...
 
void targetPose (const sva::PTransformd &X_0_target)
 Set target pose (position and orientation) of the frame in the world frame. More...
 
sva::PTransformd poseError ()
 Transform from current frame pose to target. More...
 
const sva::ForceVecd & targetWrench () const
 Get the target wrench in the control frame. More...
 
void targetWrenchW (const sva::ForceVecd &wrenchW)
 Set the target wrench in world frame. More...
 
void targetWrench (const sva::ForceVecd &wrench)
 Set the target wrench in the control frame. More...
 
sva::ForceVecd measuredWrench () const
 Get the measured wrench in the control frame. More...
 
void maxLinearVel (const Eigen::Vector3d &maxLinearVel)
 Set the maximum translation velocity of the task. More...
 
const Eigen::Vector3d & maxLinearVel () const noexcept
 
void maxAngularVel (const Eigen::Vector3d &maxAngularVel)
 Set the maximum angular velocity of the task. More...
 
const Eigen::Vector3d & maxAngularVel () const noexcept
 
void velFilterGain (double gain)
 Set the gain of the low-pass filter on the reference velocity. More...
 
double velFilterGain () const noexcept
 Return the gain of the low-pass filter on the reference velocity. More...
 
void refVelB (const sva::MotionVecd &velB)
 Set the reference body velocity. More...
 
void load (mc_solver::QPSolver &solver, const mc_rtc::Configuration &config) override
 Load parameters from a Configuration object. More...
 
- Public Member Functions inherited from mc_tasks::TransformTask
 TransformTask (const mc_rbdyn::RobotFrame &frame, double stiffness=2.0, double weight=500.0)
 Constructor. More...
 
 TransformTask (const std::string &surfaceName, const mc_rbdyn::Robots &robots, unsigned int robotIndex, double stiffness=2.0, double weight=500)
 Constructor. More...
 
void reset () override
 Reset the task. More...
 
virtual sva::PTransformd target () const
 Get the task's target. More...
 
virtual void target (const sva::PTransformd &worldPos)
 Set the task's target. More...
 
virtual void targetVel (const sva::MotionVecd &worldVel)
 Get the task's target velocity. More...
 
void targetSurface (unsigned int robotIndex, const std::string &surfaceName, const sva::PTransformd &offset=sva::PTransformd::Identity())
 Targets a robot surface with an optional offset. The offset is expressed in the target contact frame. More...
 
void targetFrame (const mc_rbdyn::Frame &targetFrame, const sva::PTransformd &offset=sva::PTransformd::Identity())
 Targets a given frame with an optional offset. More...
 
void targetFrameVelocity (const mc_rbdyn::Frame &targetFrame, const sva::PTransformd &offset=sva::PTransformd::Identity())
 Targets a given frame velocity with an optional offset. More...
 
virtual void target (const mc_rbdyn::Frame &frame, const sva::PTransformd &offset)
 Targets a given frame with an optional offset. More...
 
const std::string & surface () const noexcept
 Retrieve the controlled frame name. More...
 
const mc_rbdyn::RobotFrameframe () const noexcept
 Return the controlled frame (const) More...
 
sva::PTransformd surfacePose () const noexcept
 
std::function< bool(const mc_tasks::MetaTask &task, std::string &)> buildCompletionCriteria (double dt, const mc_rtc::Configuration &config) const override
 
void addToLogger (mc_rtc::Logger &logger) override
 
void setGains (const sva::MotionVecd &stiffness, const sva::MotionVecd &damping)
 Set dimensional stiffness and damping. More...
 
void stiffness (const sva::MotionVecd &stiffness)
 Set dimensional stiffness. More...
 
sva::MotionVecd mvStiffness ()
 Get dimensional stiffness as a motion vector. More...
 
void damping (const sva::MotionVecd &damping)
 Set dimensional damping. More...
 
sva::MotionVecd mvDamping ()
 Get dimensional damping as a motion vector. More...
 
void refVelB (const sva::MotionVecd &velB)
 Set trajectory task's reference velocity from motion vector in frame coordinates. More...
 
sva::MotionVecd refVelB () const
 Get reference velocity in frame coordinates as a motion vector. More...
 
void refAccel (const sva::MotionVecd &accel)
 Set trajectory task's reference acceleration from motion vector. More...
 
void load (mc_solver::QPSolver &solver, const mc_rtc::Configuration &config) override
 Load parameters from a Configuration object. More...
 
void damping (double damping)
 Set the task damping, leaving its stiffness unchanged. More...
 
void damping (const Eigen::VectorXd &damping)
 Set dimensional damping. More...
 
double damping () const
 Get the current task damping. More...
 
void setGains (double stiffness, double damping)
 Set both stiffness and damping. More...
 
void setGains (const Eigen::VectorXd &stiffness, const Eigen::VectorXd &damping)
 Set dimensional stiffness and damping. More...
 
void stiffness (double stiffness)
 Set the task stiffness/damping. More...
 
void stiffness (const Eigen::VectorXd &stiffness)
 Set dimensional stiffness. More...
 
double stiffness () const
 Get the current task stiffness. More...
 
- Public Member Functions inherited from mc_tasks::TrajectoryTaskGeneric
 TrajectoryTaskGeneric (const mc_rbdyn::Robots &robots, unsigned int robotIndex, double stiffness, double weight)
 Constructor (auto damping) More...
 
 TrajectoryTaskGeneric (const mc_rbdyn::RobotFrame &frame, double stiffness, double weight)
 Constructor (auto damping) More...
 
virtual ~TrajectoryTaskGeneric ()=default
 
void refVel (const Eigen::VectorXd &vel)
 Set the trajectory reference velocity. More...
 
const Eigen::VectorXd & refVel () const
 Get the trajectory reference velocity. More...
 
void refAccel (const Eigen::VectorXd &accel)
 Set the trajectory reference acceleration. More...
 
const Eigen::VectorXd & refAccel () const
 Get the trajectory reference acceleration. More...
 
void stiffness (double stiffness)
 Set the task stiffness/damping. More...
 
void stiffness (const Eigen::VectorXd &stiffness)
 Set dimensional stiffness. More...
 
void damping (double damping)
 Set the task damping, leaving its stiffness unchanged. More...
 
void damping (const Eigen::VectorXd &damping)
 Set dimensional damping. More...
 
void setGains (double stiffness, double damping)
 Set both stiffness and damping. More...
 
void setGains (const Eigen::VectorXd &stiffness, const Eigen::VectorXd &damping)
 Set dimensional stiffness and damping. More...
 
double stiffness () const
 Get the current task stiffness. More...
 
double damping () const
 Get the current task damping. More...
 
const Eigen::VectorXd & dimStiffness () const
 Get the current task dimensional stiffness. More...
 
const Eigen::VectorXd & dimDamping () const
 Get the current task dimensional damping. More...
 
void weight (double w)
 Set the task weight. More...
 
double weight () const
 Returns the task weight. More...
 
void dimWeight (const Eigen::VectorXd &dimW) override
 Set the task's dimension weight vector. More...
 
Eigen::VectorXd dimWeight () const override
 Get the current task's dim weight vector. More...
 
void selectActiveJoints (const std::vector< std::string > &activeJointsName, const std::map< std::string, std::vector< std::array< int, 2 >>> &activeDofs={}, bool checkJoints=true)
 Create an active joints selector. More...
 
void selectActiveJoints (mc_solver::QPSolver &solver, const std::vector< std::string > &activeJointsName, const std::map< std::string, std::vector< std::array< int, 2 >>> &activeDofs={}) override
 Create an active joints selector. More...
 
void selectUnactiveJoints (const std::vector< std::string > &unactiveJointsName, const std::map< std::string, std::vector< std::array< int, 2 >>> &unactiveDofs={}, bool checkJoints=true)
 Create an unactive joints selector. More...
 
void selectUnactiveJoints (mc_solver::QPSolver &solver, const std::vector< std::string > &unactiveJointsName, const std::map< std::string, std::vector< std::array< int, 2 >>> &unactiveDofs={}) override
 Create an unactive joints selector. More...
 
virtual void resetJointsSelector ()
 
void resetJointsSelector (mc_solver::QPSolver &solver) override
 Reset active joints selection. More...
 
Eigen::VectorXd eval () const override
 Returns the task error. More...
 
Eigen::VectorXd speed () const override
 Returns the task velocity. More...
 
const Eigen::VectorXd & normalAcc () const
 
- Public Member Functions inherited from mc_tasks::MetaTask
 MetaTask ()
 
virtual ~MetaTask ()
 
const std::string & type () const
 
virtual void name (const std::string &name)
 
const std::string & name () const
 
size_t iterInSolver () const noexcept
 Get the number of iterations since the task was added to the solver. More...
 
void resetIterInSolver () noexcept
 Set the number of iterations since the task was added to the solver to zero. More...
 
void incrementIterInSolver () noexcept
 Increment the number of iterations since the task was added to the solver. More...
 
Backend backend () const noexcept
 

Protected Member Functions

void update (mc_solver::QPSolver &) override
 Update the task. More...
 
void addToGUI (mc_rtc::gui::StateBuilder &gui) override
 
void addToLogger (mc_rtc::Logger &logger) override
 
void addToSolver (mc_solver::QPSolver &solver) override
 
void refVelB (const sva::MotionVecd &velB)
 
sva::MotionVecd refVelB () const
 
virtual sva::PTransformd target () const
 
virtual void target (const sva::PTransformd &worldPos)
 
virtual void target (const mc_rbdyn::Frame &frame, const sva::PTransformd &offset)
 
- Protected Member Functions inherited from mc_tasks::TransformTask
void addToGUI (mc_rtc::gui::StateBuilder &gui) override
 
void refVel (const Eigen::VectorXd &vel)
 Set the trajectory reference velocity. More...
 
const Eigen::VectorXd & refVel () const
 Get the trajectory reference velocity. More...
 
- Protected Member Functions inherited from mc_tasks::TrajectoryTaskGeneric
template<Backend backend, typename ErrorT , typename... Args>
void finalize (Args &&... args)
 
void addToSolver (mc_solver::QPSolver &solver) override
 Add the task to a solver. More...
 
void removeFromSolver (mc_solver::QPSolver &solver) override
 Remove the task from a solver. More...
 
void update (mc_solver::QPSolver &) override
 Update the task. More...
 
- Protected Member Functions inherited from mc_tasks::MetaTask
virtual void removeFromLogger (mc_rtc::Logger &)
 
virtual void removeFromGUI (mc_rtc::gui::StateBuilder &)
 

Protected Attributes

Eigen::Vector3d maxAngularVel_ = {0.1, 0.1, 0.1}
 
Eigen::Vector3d maxLinearVel_ = {0.1, 0.1, 0.1}
 
double timestep_
 
double velFilterGain_ = 0.8
 
sva::ForceVecd admittance_ = sva::ForceVecd(Eigen::Vector6d::Zero())
 
sva::ForceVecd targetWrench_ = sva::ForceVecd(Eigen::Vector6d::Zero())
 
sva::ForceVecd wrenchError_ = sva::ForceVecd(Eigen::Vector6d::Zero())
 
sva::MotionVecd feedforwardVelB_ = sva::MotionVecd(Eigen::Vector6d::Zero())
 
sva::MotionVecd refVelB_ = sva::MotionVecd(Eigen::Vector6d::Zero())
 
- Protected Attributes inherited from mc_tasks::TransformTask
mc_rbdyn::ConstRobotFramePtr frame_
 
- Protected Attributes inherited from mc_tasks::TrajectoryTaskGeneric
const mc_rbdyn::Robotsrobots
 
unsigned int rIndex
 
mc_rtc::void_ptr errorT {nullptr, nullptr}
 
Eigen::VectorXd refVel_
 
Eigen::VectorXd refAccel_
 
bool inSolver_ = false
 
mc_rtc::void_ptr trajectoryT_ {nullptr, nullptr}
 
Eigen::VectorXd stiffness_
 
Eigen::VectorXd damping_
 
double weight_
 
mc_rtc::void_ptr selectorT_ {nullptr, nullptr}
 
- Protected Attributes inherited from mc_tasks::MetaTask
Backend backend_
 
std::string type_
 
std::string name_
 
size_t iterInSolver_ = 0
 

Additional Inherited Members

- Public Types inherited from mc_tasks::TrajectoryTaskGeneric
using TrajectoryBase = TrajectoryTaskGeneric
 
- Public Types inherited from mc_tasks::MetaTask
using Backend = mc_solver::QPSolver::Backend
 
- Static Protected Member Functions inherited from mc_tasks::MetaTask
static void addToSolver (MetaTask &t, mc_solver::QPSolver &solver)
 
static void removeFromSolver (MetaTask &t, mc_solver::QPSolver &solver)
 
static void update (MetaTask &t, mc_solver::QPSolver &solver)
 
static void addToLogger (MetaTask &t, mc_rtc::Logger &logger)
 
static void removeFromLogger (MetaTask &t, mc_rtc::Logger &logger)
 
static void addToGUI (MetaTask &t, mc_rtc::gui::StateBuilder &gui)
 
static void removeFromGUI (MetaTask &t, mc_rtc::gui::StateBuilder &gui)
 
static void ensureHasJoints (const mc_rbdyn::Robot &robot, const std::vector< std::string > &joints, const std::string &prefix)
 

Detailed Description

Hybrid position-force control on a contacting end-effector.

The AdmittanceTask is by default a TransformTask, i.e. pure position control of a frame. Admittance coefficients that map force errors to displacements (see [1] and [2]) are initially set to zero.

When the admittance along one axis (Fx, Fy, Fz, Tx, Ty or Tz) is set to a non-zero positive value, this axis switches from position to force control. The goal is then to realize the prescribed target wrench at the control frame (bis repetita placent: wrenches are expressed in the control frame of the task, not in the sensor frame of the corresponding body). The force control law applied is damping control [3].

See the discussion in [4] for a comparison with the ComplianceTask.

[1] https://en.wikipedia.org/wiki/Mechanical_impedance [2] https://en.wikipedia.org/wiki/Impedance_analogy [3] https://doi.org/10.1109/IROS.2010.5651082 [4] https://gite.lirmm.fr/multi-contact/mc_rtc/issues/34

Constructor & Destructor Documentation

◆ AdmittanceTask() [1/2]

EIGEN_MAKE_ALIGNED_OPERATOR_NEW mc_tasks::force::AdmittanceTask::AdmittanceTask ( const mc_rbdyn::RobotFrame frame,
double  stiffness = 5.0,
double  weight = 1000.0 
)

Initialize a new admittance task.

Parameters
frameControl frame
stiffnessStiffness of the underlying Transform task
weightWeight of the underlying Transform task
Exceptions
Ifthe frame does not have a force sensor attached

◆ AdmittanceTask() [2/2]

mc_tasks::force::AdmittanceTask::AdmittanceTask ( const std::string &  robotSurface,
const mc_rbdyn::Robots robots,
unsigned  robotIndex,
double  stiffness = 5.0,
double  weight = 1000.0 
)

Initialize a new admittance task.

Parameters
robotSurfaceName of the surface frame to control, in which the desired wrench will also be expressed
robotsRobots where the task will be applied
robotIndexWhich robot among the robots
stiffnessStiffness of the underlying Transform task
weightWeight of the underlying Transform task
Exceptions
Ifthe body the task is attempting to control does not have a sensor attached to it

Member Function Documentation

◆ addToGUI()

void mc_tasks::force::AdmittanceTask::addToGUI ( mc_rtc::gui::StateBuilder )
overrideprotectedvirtual

Add elements to the GUI through the helper

This will be called by the solver when the task is added.

The default implementation adds the type of the task under the {"Tasks", name_} category.

Reimplemented from mc_tasks::MetaTask.

Reimplemented in mc_tasks::force::CoPTask.

◆ addToLogger()

void mc_tasks::force::AdmittanceTask::addToLogger ( mc_rtc::Logger )
overrideprotectedvirtual

Add entries to the logger

This will be called by the solver if it holds a valid logger instance when the task is added.

The default implementation adds nothing to the log.

Reimplemented from mc_tasks::MetaTask.

Reimplemented in mc_tasks::force::CoPTask.

◆ addToSolver()

void mc_tasks::force::AdmittanceTask::addToSolver ( mc_solver::QPSolver solver)
overrideprotectedvirtual

Override addToSolver in order to get the timestep's solver automatically

Implements mc_tasks::MetaTask.

◆ admittance() [1/2]

const sva::ForceVecd& mc_tasks::force::AdmittanceTask::admittance ( ) const
inline

Get the admittance coefficients of the task.

◆ admittance() [2/2]

void mc_tasks::force::AdmittanceTask::admittance ( const sva::ForceVecd &  admittance)
inline

Set the admittance coefficients of the task.

Parameters
admittanceVector of positive admittance coefficients

◆ load()

void mc_tasks::force::AdmittanceTask::load ( mc_solver::QPSolver solver,
const mc_rtc::Configuration config 
)
overridevirtual

Load parameters from a Configuration object.

Reimplemented from mc_tasks::MetaTask.

Reimplemented in mc_tasks::force::CoPTask.

◆ maxAngularVel() [1/2]

const Eigen::Vector3d& mc_tasks::force::AdmittanceTask::maxAngularVel ( ) const
inlinenoexcept

Get the maximum angular velocity of the task

◆ maxAngularVel() [2/2]

void mc_tasks::force::AdmittanceTask::maxAngularVel ( const Eigen::Vector3d &  maxAngularVel)
inline

Set the maximum angular velocity of the task.

◆ maxLinearVel() [1/2]

const Eigen::Vector3d& mc_tasks::force::AdmittanceTask::maxLinearVel ( ) const
inlinenoexcept

Get the maximum linear velocity of the task

◆ maxLinearVel() [2/2]

void mc_tasks::force::AdmittanceTask::maxLinearVel ( const Eigen::Vector3d &  maxLinearVel)
inline

Set the maximum translation velocity of the task.

◆ measuredWrench()

sva::ForceVecd mc_tasks::force::AdmittanceTask::measuredWrench ( ) const
inline

Get the measured wrench in the control frame.

◆ poseError()

sva::PTransformd mc_tasks::force::AdmittanceTask::poseError ( )
inline

Transform from current frame pose to target.

◆ refVelB() [1/3]

sva::MotionVecd mc_tasks::TransformTask::refVelB
inlineprotected

Transform's refVelB() becomes internal to the task.

◆ refVelB() [2/3]

void mc_tasks::force::AdmittanceTask::refVelB ( const sva::MotionVecd &  velB)
inline

Set the reference body velocity.

Parameters
velBFeedforward body velocity

Body velocity means that velB is the velocity of the control frame expressed in the control frame itself. See e.g. (Murray et al., 1994, CRC Press).

Note
The refVelB body velocity ultimately set as task target to the QP is the sum of this feedforward velocity and a feedback term for force control.

◆ refVelB() [3/3]

void mc_tasks::TransformTask::refVelB
inlineprotected

Transform's refVelB() becomes internal to the task.

◆ reset()

void mc_tasks::force::AdmittanceTask::reset ( )
overridevirtual

Reset the task.

Set the end effector objective to the current position of the end-effector, disable force control and reset admittance and target wrench to zero.

Implements mc_tasks::MetaTask.

Reimplemented in mc_tasks::force::CoPTask.

◆ surfacePose()

sva::PTransformd mc_tasks::force::AdmittanceTask::surfacePose ( ) const
inline

Get the current pose of the control frame in the inertial frame.

◆ target() [1/3]

virtual sva::PTransformd mc_tasks::TransformTask::target
protected

Surface transform's target becomes internal to the task. Its setter is now targetPose().

◆ target() [2/3]

virtual void mc_tasks::TransformTask::target
protected

Surface transform's target becomes internal to the task. Its setter is now targetPose().

◆ target() [3/3]

virtual void mc_tasks::TransformTask::target
protected

Surface transform's target becomes internal to the task. Its setter is now targetPose().

◆ targetPose() [1/2]

sva::PTransformd mc_tasks::force::AdmittanceTask::targetPose ( )
inline

Get the target pose of the control frame in the world frame.

◆ targetPose() [2/2]

void mc_tasks::force::AdmittanceTask::targetPose ( const sva::PTransformd &  X_0_target)
inline

Set target pose (position and orientation) of the frame in the world frame.

Parameters
X_0_targetPlucker transform to the target frame.

◆ targetWrench() [1/2]

const sva::ForceVecd& mc_tasks::force::AdmittanceTask::targetWrench ( ) const
inline

Get the target wrench in the control frame.

◆ targetWrench() [2/2]

void mc_tasks::force::AdmittanceTask::targetWrench ( const sva::ForceVecd &  wrench)
inline

Set the target wrench in the control frame.

Parameters
wrenchTarget wrench in the control frame

◆ targetWrenchW()

void mc_tasks::force::AdmittanceTask::targetWrenchW ( const sva::ForceVecd &  wrenchW)
inline

Set the target wrench in world frame.

Parameters
wrenchTarget wrench in world frame

◆ update()

void mc_tasks::force::AdmittanceTask::update ( mc_solver::QPSolver solver)
overrideprotectedvirtual

Update the task.

This function (usually) has to be called at every iteration of the solver once it has been added. It should update the state of the task.

Parameters
solverSolver in which the task is inserted

Implements mc_tasks::MetaTask.

Reimplemented in mc_tasks::force::DampingTask.

◆ velFilterGain() [1/2]

double mc_tasks::force::AdmittanceTask::velFilterGain ( ) const
inlinenoexcept

Return the gain of the low-pass filter on the reference velocity.

◆ velFilterGain() [2/2]

void mc_tasks::force::AdmittanceTask::velFilterGain ( double  gain)
inline

Set the gain of the low-pass filter on the reference velocity.

Parameters
gainGain between 0 and 1.

Member Data Documentation

◆ admittance_

sva::ForceVecd mc_tasks::force::AdmittanceTask::admittance_ = sva::ForceVecd(Eigen::Vector6d::Zero())
protected

◆ feedforwardVelB_

sva::MotionVecd mc_tasks::force::AdmittanceTask::feedforwardVelB_ = sva::MotionVecd(Eigen::Vector6d::Zero())
protected

◆ maxAngularVel_

Eigen::Vector3d mc_tasks::force::AdmittanceTask::maxAngularVel_ = {0.1, 0.1, 0.1}
protected

◆ maxLinearVel_

Eigen::Vector3d mc_tasks::force::AdmittanceTask::maxLinearVel_ = {0.1, 0.1, 0.1}
protected

◆ refVelB_

sva::MotionVecd mc_tasks::force::AdmittanceTask::refVelB_ = sva::MotionVecd(Eigen::Vector6d::Zero())
protected

◆ targetWrench_

sva::ForceVecd mc_tasks::force::AdmittanceTask::targetWrench_ = sva::ForceVecd(Eigen::Vector6d::Zero())
protected

◆ timestep_

double mc_tasks::force::AdmittanceTask::timestep_
protected

◆ velFilterGain_

double mc_tasks::force::AdmittanceTask::velFilterGain_ = 0.8
protected

◆ wrenchError_

sva::ForceVecd mc_tasks::force::AdmittanceTask::wrenchError_ = sva::ForceVecd(Eigen::Vector6d::Zero())
protected

The documentation for this struct was generated from the following file: