mc_tasks::force::ImpedanceTask Struct Reference

Impedance control of the end-effector. More...

#include <mc_tasks/ImpedanceTask.h>

Inheritance diagram for mc_tasks::force::ImpedanceTask:
Collaboration diagram for mc_tasks::force::ImpedanceTask:

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW ImpedanceTask (const std::string &surfaceName, const mc_rbdyn::Robots &robots, unsigned robotIndex, double stiffness=5.0, double weight=1000.0)
 Constructor. More...
 
 ImpedanceTask (const mc_rbdyn::RobotFrame &frame, double stiffness=5.0, double weight=1000.0)
 Constructor. More...
 
void reset () override
 Reset the task. More...
 
const ImpedanceGainsgains () const noexcept
 Access the impedance gains. More...
 
ImpedanceGainsgains () noexcept
 Access the impedance gains. More...
 
const sva::PTransformd & targetPose () const noexcept
 Get the target pose of the surface in the world frame. More...
 
void targetPose (const sva::PTransformd &pose)
 Set the target pose of the surface in the world frame. More...
 
const sva::MotionVecd & targetVel () const noexcept
 Get the target velocity of the surface in the world frame. More...
 
void targetVel (const sva::MotionVecd &worldVel) override
 Set the target velocity of the surface in the world frame. More...
 
const sva::MotionVecd & targetAccel () const noexcept
 Get the target acceleration of the surface in the world frame. More...
 
void targetAccel (const sva::MotionVecd &accel)
 Set the target acceleration of the surface in the world frame. More...
 
const sva::PTransformd & deltaCompliancePose () const
 Get the relative pose from target frame to compliance frame represented in the world frame. More...
 
const sva::PTransformd compliancePose () const
 Get the compliance pose of the surface in the world frame. More...
 
const sva::ForceVecd & targetWrench () const noexcept
 Get the target wrench in the surface frame. More...
 
void targetWrenchW (const sva::ForceVecd &wrenchW)
 Set the target wrench in the world frame. This function will convert the wrench from the world frame to the surface frame, and call targetWrench(). More...
 
void targetWrench (const sva::ForceVecd &wrench)
 Set the target wrench in the surface frame. More...
 
const sva::ForceVecd & measuredWrench () const
 Get the measured wrench in the surface frame. More...
 
const sva::ForceVecd & filteredMeasuredWrench () const
 Get the filtered measured wrench in the surface frame. More...
 
double cutoffPeriod () const
 Get the cutoff period for the low-pass filter of measured wrench. More...
 
void cutoffPeriod (double cutoffPeriod)
 Set the cutoff period for the low-pass filter of measured wrench. More...
 
bool hold () const noexcept
 Get whether hold mode is enabled. More...
 
void hold (bool hold) noexcept
 Set hold mode. 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...
 
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 &solver) override
 Update the task. More...
 
void addToSolver (mc_solver::QPSolver &solver) override
 Add the task to a solver. More...
 
void addToGUI (mc_rtc::gui::StateBuilder &gui) override
 
void addToLogger (mc_rtc::Logger &logger) override
 
- 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

ImpedanceGains gains_ = ImpedanceGains::Default()
 
double deltaCompPoseLinLimit_ = 1.0
 
double deltaCompPoseAngLimit_ = mc_rtc::constants::PI
 
double deltaCompVelLinLimit_ = 1e3
 
double deltaCompVelAngLimit_ = 1e3
 
double deltaCompAccelLinLimit_ = 1e3
 
double deltaCompAccelAngLimit_ = 1e3
 
sva::PTransformd targetPoseW_ = sva::PTransformd::Identity()
 
sva::MotionVecd targetVelW_ = sva::MotionVecd::Zero()
 
sva::MotionVecd targetAccelW_ = sva::MotionVecd::Zero()
 
sva::ForceVecd targetWrench_ = sva::ForceVecd::Zero()
 
sva::ForceVecd measuredWrench_ = sva::ForceVecd::Zero()
 
sva::ForceVecd filteredMeasuredWrench_ = sva::ForceVecd::Zero()
 
double cutoffPeriod_ = 0.05
 
mc_filter::LowPass< sva::ForceVecd > lowPass_
 
bool hold_ = false
 
sva::PTransformd deltaCompPoseW_ = sva::PTransformd::Identity()
 
sva::MotionVecd deltaCompVelW_ = sva::MotionVecd::Zero()
 
sva::MotionVecd deltaCompAccelW_ = sva::MotionVecd::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

Impedance control of the end-effector.

ImpedanceTask passes the following "compliance" position and orientation (i.e., \( p_c \)) to the target of the SurfaceTransformTask, which is the base class of this class.

\[ M \Delta \ddot{p}_{cd} + D \Delta \dot{p}_{cd} + K \Delta p_{cd} = K_f (f_m - f_d) {\rm where} \Delta p_{cd} = p_c - p_d \]

where \( p_* \) is the end-effector position and orientation, and \( f_* \) is the end-effector wrench. Subscripts \( d, c, m \) mean the desired, compliance, and measured values, respectively. \( M, D, K \) are the mass, damper, and spring parameters of the impedance, respectively. \( K_f \) is the wrench gain.

Desired values \( p_d, \dot{p}_d, \ddot{p}_d, f_d \) are given from the user. The measured value \( f_m \) is obtained from the sensor.

In the SurfaceTransformTask, the "IK" acceleration (i.e., \( \ddot{p}_{IK} \)) are calculated and passed to the acceleration-level IK.

\[ \ddot{p}_{IK} = \ddot{p}_{c} + K_d ( \dot{p}_c - \dot{p}_a ) + K_s ( p_c - p_a ) \]

\( K_s, K_d \) are the stiffness and damping parameters, respectively. Subscripts \( a \) means actual value.

Reference: Bruno Siciliano and Luigi Villani, Robot Force Control, Springer, 1999 https://www.springer.com/jp/book/9780792377337

Constructor & Destructor Documentation

◆ ImpedanceTask() [1/2]

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

Constructor.

Parameters
surfaceNameName of the surface frame to control
robotsRobots controlled by this task
robotIndexIndex of the robot controlled by this task
stiffnessTask stiffness
weightTask weight
Exceptions
Ifthe body the task is attempting to control does not have a sensor attached to it

◆ ImpedanceTask() [2/2]

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

Constructor.

Parameters
frameFrame controlled by this task
stiffnessTask stiffness
weightTask weight
Exceptions
Ifthe frame does not have a force sensor attached to it

Member Function Documentation

◆ addToGUI()

void mc_tasks::force::ImpedanceTask::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.

◆ addToLogger()

void mc_tasks::force::ImpedanceTask::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.

◆ addToSolver()

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

Add the task to a solver.

Parameters
solverSolver where to add the task

Implements mc_tasks::MetaTask.

◆ compliancePose()

const sva::PTransformd mc_tasks::force::ImpedanceTask::compliancePose ( ) const
inline

Get the compliance pose of the surface in the world frame.

Note
Compliance pose cannot be set by user because it is calculated from the impedance equation internally. See the Constructor description for the definition of compliance pose.

◆ cutoffPeriod() [1/2]

double mc_tasks::force::ImpedanceTask::cutoffPeriod ( ) const
inline

Get the cutoff period for the low-pass filter of measured wrench.

◆ cutoffPeriod() [2/2]

void mc_tasks::force::ImpedanceTask::cutoffPeriod ( double  cutoffPeriod)
inline

Set the cutoff period for the low-pass filter of measured wrench.

◆ deltaCompliancePose()

const sva::PTransformd& mc_tasks::force::ImpedanceTask::deltaCompliancePose ( ) const
inline

Get the relative pose from target frame to compliance frame represented in the world frame.

◆ filteredMeasuredWrench()

const sva::ForceVecd& mc_tasks::force::ImpedanceTask::filteredMeasuredWrench ( ) const
inline

Get the filtered measured wrench in the surface frame.

◆ gains() [1/2]

const ImpedanceGains& mc_tasks::force::ImpedanceTask::gains ( ) const
inlinenoexcept

Access the impedance gains.

◆ gains() [2/2]

ImpedanceGains& mc_tasks::force::ImpedanceTask::gains ( )
inlinenoexcept

Access the impedance gains.

◆ hold() [1/2]

bool mc_tasks::force::ImpedanceTask::hold ( ) const
inlinenoexcept

Get whether hold mode is enabled.

◆ hold() [2/2]

void mc_tasks::force::ImpedanceTask::hold ( bool  hold)
inlinenoexcept

Set hold mode.

In hold mode, the compliance modification (deltaCompPoseW_) is automatically updated so that the final target pose sent to the QP (i.e. compliancePose()) remains constant even if the user-specified target pose changes. A typical use case for hold mode is to set the current end-effector pose as targetPose. Thanks to the hold mode, the robot does not move the end-effector pose, but the deltaCompPoseW_ becomes smaller, and the external force exerted by the spring term of impedance dynamics becomes smaller. Without the hold mode, the mass and damper effects of impedance dynamics would cause the compliancePose to temporarily deviate from the commanded targetPose, causes unintended movement of the end-effector.

◆ load()

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

Load parameters from a Configuration object.

Reimplemented from mc_tasks::MetaTask.

◆ measuredWrench()

const sva::ForceVecd& mc_tasks::force::ImpedanceTask::measuredWrench ( ) const
inline

Get the measured wrench in the surface frame.

◆ reset()

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

Reset the task.

Set the target and compliance poses to the current surface, and reset the target and compliance velocity and acceleration to zero.

Implements mc_tasks::MetaTask.

◆ targetAccel() [1/2]

const sva::MotionVecd& mc_tasks::force::ImpedanceTask::targetAccel ( ) const
inlinenoexcept

Get the target acceleration of the surface in the world frame.

◆ targetAccel() [2/2]

void mc_tasks::force::ImpedanceTask::targetAccel ( const sva::MotionVecd &  accel)
inline

Set the target acceleration of the surface in the world frame.

◆ targetPose() [1/2]

const sva::PTransformd& mc_tasks::force::ImpedanceTask::targetPose ( ) const
inlinenoexcept

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

◆ targetPose() [2/2]

void mc_tasks::force::ImpedanceTask::targetPose ( const sva::PTransformd &  pose)
inline

Set the target pose of the surface in the world frame.

◆ targetVel() [1/2]

const sva::MotionVecd& mc_tasks::force::ImpedanceTask::targetVel ( ) const
inlinenoexcept

Get the target velocity of the surface in the world frame.

◆ targetVel() [2/2]

void mc_tasks::force::ImpedanceTask::targetVel ( const sva::MotionVecd &  worldVel)
inlineoverridevirtual

Set the target velocity of the surface in the world frame.

Reimplemented from mc_tasks::TransformTask.

◆ targetWrench() [1/2]

const sva::ForceVecd& mc_tasks::force::ImpedanceTask::targetWrench ( ) const
inlinenoexcept

Get the target wrench in the surface frame.

◆ targetWrench() [2/2]

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

Set the target wrench in the surface frame.

◆ targetWrenchW()

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

Set the target wrench in the world frame. This function will convert the wrench from the world frame to the surface frame, and call targetWrench().

◆ update()

void mc_tasks::force::ImpedanceTask::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.

Member Data Documentation

◆ cutoffPeriod_

double mc_tasks::force::ImpedanceTask::cutoffPeriod_ = 0.05
protected

◆ deltaCompAccelAngLimit_

double mc_tasks::force::ImpedanceTask::deltaCompAccelAngLimit_ = 1e3
protected

◆ deltaCompAccelLinLimit_

double mc_tasks::force::ImpedanceTask::deltaCompAccelLinLimit_ = 1e3
protected

◆ deltaCompAccelW_

sva::MotionVecd mc_tasks::force::ImpedanceTask::deltaCompAccelW_ = sva::MotionVecd::Zero()
protected

Relative pose, velocity, and acceleration from target frame to compliance frame represented in the world frame. To store these values across control cycles, represent them in a constant world frame instead of the time-varying surface frame.

◆ deltaCompPoseAngLimit_

double mc_tasks::force::ImpedanceTask::deltaCompPoseAngLimit_ = mc_rtc::constants::PI
protected

◆ deltaCompPoseLinLimit_

double mc_tasks::force::ImpedanceTask::deltaCompPoseLinLimit_ = 1.0
protected

◆ deltaCompPoseW_

sva::PTransformd mc_tasks::force::ImpedanceTask::deltaCompPoseW_ = sva::PTransformd::Identity()
protected

Relative pose, velocity, and acceleration from target frame to compliance frame represented in the world frame. To store these values across control cycles, represent them in a constant world frame instead of the time-varying surface frame.

◆ deltaCompVelAngLimit_

double mc_tasks::force::ImpedanceTask::deltaCompVelAngLimit_ = 1e3
protected

◆ deltaCompVelLinLimit_

double mc_tasks::force::ImpedanceTask::deltaCompVelLinLimit_ = 1e3
protected

◆ deltaCompVelW_

sva::MotionVecd mc_tasks::force::ImpedanceTask::deltaCompVelW_ = sva::MotionVecd::Zero()
protected

Relative pose, velocity, and acceleration from target frame to compliance frame represented in the world frame. To store these values across control cycles, represent them in a constant world frame instead of the time-varying surface frame.

◆ filteredMeasuredWrench_

sva::ForceVecd mc_tasks::force::ImpedanceTask::filteredMeasuredWrench_ = sva::ForceVecd::Zero()
protected

◆ gains_

ImpedanceGains mc_tasks::force::ImpedanceTask::gains_ = ImpedanceGains::Default()
protected

◆ hold_

bool mc_tasks::force::ImpedanceTask::hold_ = false
protected

◆ lowPass_

mc_filter::LowPass<sva::ForceVecd> mc_tasks::force::ImpedanceTask::lowPass_
protected

◆ measuredWrench_

sva::ForceVecd mc_tasks::force::ImpedanceTask::measuredWrench_ = sva::ForceVecd::Zero()
protected

◆ targetAccelW_

sva::MotionVecd mc_tasks::force::ImpedanceTask::targetAccelW_ = sva::MotionVecd::Zero()
protected

◆ targetPoseW_

sva::PTransformd mc_tasks::force::ImpedanceTask::targetPoseW_ = sva::PTransformd::Identity()
protected

◆ targetVelW_

sva::MotionVecd mc_tasks::force::ImpedanceTask::targetVelW_ = sva::MotionVecd::Zero()
protected

◆ targetWrench_

sva::ForceVecd mc_tasks::force::ImpedanceTask::targetWrench_ = sva::ForceVecd::Zero()
protected

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