Track center-of-pressure (CoP) references at contact. More...

#include <mc_tasks/CoPTask.h>

Inheritance diagram for mc_tasks::force::CoPTask:

Collaboration diagram for mc_tasks::force::CoPTask:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	CoPTask (const mc_rbdyn::RobotFrame &frame, double stiffness=5.0, double weight=1000.0)
	Initialize a new CoP task. More...

	CoPTask (const std::string &robotSurface, const mc_rbdyn::Robots &robots, unsigned robotIndex, double stiffness=5.0, double weight=1000.0)
	Initialize a new CoP task. More...

void	reset () override
	Reset the task. More...

std::function< bool(const mc_tasks::MetaTask &, std::string &)>	buildCompletionCriteria (double dt, const mc_rtc::Configuration &config) const override

void	useTargetPressure (bool s) noexcept
	Wether to compute the desired torque with the target pressure (true) of the measured one (false) More...

bool	useTargetPressure () const noexcept
	Wether the computeddesired torque is done with the target pressure (true) of the measured one (false) More...

Eigen::Vector2d	measuredCoP () const
	Measured CoP in target frame. More...

Eigen::Vector3d	measuredCoPW () const
	Measured CoP in world frame. More...

void	setZeroTargetWrench ()
	Set targent wrench to zero. More...

const Eigen::Vector2d &	targetCoP () const
	Get target CoP in the control frame. More...

Eigen::Vector3d	targetCoPW () const
	Get target CoP in the world frame. More...

void	targetCoP (const Eigen::Vector2d &targetCoP)
	Set target CoP in the control frame. More...

const Eigen::Vector3d &	targetForce () const
	Get target force in the control frame. More...

void	targetForce (const Eigen::Vector3d &targetForce)
	Set target force in the control frame. More...

void	targetForceW (const Eigen::Vector3d &targetForceW)
	Set target force in the world frame. More...

const sva::ForceVecd &	targetWrench () const
	Get target wrench in the control frame. 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::force::DampingTask
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	DampingTask (const mc_rbdyn::RobotFrame &frame, double stiffness=5.0, double weight=1000.0)
	Initialize a new damping task. More...

	DampingTask (const std::string &robotSurface, const mc_rbdyn::Robots &robots, unsigned robotIndex, double stiffness=5.0, double weight=1000.0)
	Initialize a new damping task. More...

Public Member Functions inherited from mc_tasks::force::AdmittanceTask
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...

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...

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::RobotFrame &	frame () 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	addToLogger (mc_rtc::Logger &logger) override

void	addToGUI (mc_rtc::gui::StateBuilder &gui) override

Protected Member Functions inherited from mc_tasks::force::DampingTask
void	update (mc_solver::QPSolver &) override
	Update the task. More...

Protected Member Functions inherited from mc_tasks::force::AdmittanceTask
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 &)

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)

Protected Attributes inherited from mc_tasks::force::AdmittanceTask
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::Robots &	robots

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

Detailed Description

Track center-of-pressure (CoP) references at contact.

The CoPTask is basically an AdmittanceTask where contact wrenches are expressed in terms of center of pressure (a.k.a. ZMP, see e.g. [1]) rather than torques. This is better-suited to locomotion applications.

The CoP is well-defined only when the contact pressure is strictly positive. When there is no contact pressure, the CoPTask will automatically disable torque tracking in the underlying AdmittanceTask.

[1] https://scaron.info/teaching/zero-tilting-moment-point.html

Constructor & Destructor Documentation

◆ CoPTask() [1/2]

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

Initialize a new CoP task.

Parameters

frame	Control frame
stiffness	Stiffness of the underlying Transform task
weight	Weight of the underlying Transform task

Exceptions

If	the frame does not have a force sensor attached

◆ CoPTask() [2/2]

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

Initialize a new CoP task.

Parameters

frame	Control frame
stiffness	Stiffness of the underlying Transform task
weight	Weight of the underlying Transform task

Exceptions

If	the frame does not have a force sensor attached

Initialize a new CoP task.

Parameters

robotSurface	Name of the surface frame to control, in which the desired wrench will also be expressed
robots	Robots where the task will be applied
robotIndex	Which robot among the robots
stiffness	Stiffness of the underlying Transform task
weight	Weight of the underlying Transform task

Exceptions

If	the body the task is attempting to control does not have a sensor attached to it

Member Function Documentation

◆ addToGUI()

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

◆ addToLogger()

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

◆ buildCompletionCriteria()

std::function<bool(const mc_tasks::MetaTask &, std::string &)> mc_tasks::force::CoPTask::buildCompletionCriteria	(	double	dt,
		const mc_rtc::Configuration &	config
	)		const

overridevirtual

Add support for the following entries

copError Threshold for (targetCoP - measureCoP).norm()
force Force threshold, similar to wrench for AdmittanceTask

Reimplemented from mc_tasks::MetaTask.

◆ load()

void mc_tasks::force::CoPTask::load	(	mc_solver::QPSolver &	solver,
		const mc_rtc::Configuration &	config
	)

overridevirtual

Load parameters from a Configuration object.

Reimplemented from mc_tasks::force::AdmittanceTask.

◆ measuredCoP()

Eigen::Vector2d mc_tasks::force::CoPTask::measuredCoP ( ) const

inline

Measured CoP in target frame.

◆ measuredCoPW()

Eigen::Vector3d mc_tasks::force::CoPTask::measuredCoPW ( ) const

inline

Measured CoP in world frame.

◆ reset()

void mc_tasks::force::CoPTask::reset ( )

overridevirtual

Reset the task.

Set the end effector objective to the current position of the end-effector, disable CoP tracking and reset admittance and target CoP to zero.

Reimplemented from mc_tasks::force::AdmittanceTask.

◆ setZeroTargetWrench()

void mc_tasks::force::CoPTask::setZeroTargetWrench ( )

inline

Set targent wrench to zero.

◆ targetCoP() [1/2]

const Eigen::Vector2d& mc_tasks::force::CoPTask::targetCoP ( ) const

inline

Get target CoP in the control frame.

◆ targetCoP() [2/2]

void mc_tasks::force::CoPTask::targetCoP ( const Eigen::Vector2d & targetCoP )

inline

Set target CoP in the control frame.

Parameters

targetCoP 2D vector of CoP coordinates in the control frame

◆ targetCoPW()

Eigen::Vector3d mc_tasks::force::CoPTask::targetCoPW ( ) const

inline

Get target CoP in the world frame.

◆ targetForce() [1/2]

const Eigen::Vector3d& mc_tasks::force::CoPTask::targetForce ( ) const

inline

Get target force in the control frame.

◆ targetForce() [2/2]

void mc_tasks::force::CoPTask::targetForce ( const Eigen::Vector3d & targetForce )

inline

Set target force in the control frame.

Parameters

targetForce 3D vector of target force in the control frame

◆ targetForceW()

void mc_tasks::force::CoPTask::targetForceW ( const Eigen::Vector3d & targetForceW )

inline

Set target force in the world frame.

Parameters

targetForce 3D vector of target force in the world frame

◆ targetWrench()

const sva::ForceVecd& mc_tasks::force::CoPTask::targetWrench ( ) const

inline

Get target wrench in the control frame.

◆ useTargetPressure() [1/2]

bool mc_tasks::force::CoPTask::useTargetPressure ( ) const

inlinenoexcept

Wether the computeddesired torque is done with the target pressure (true) of the measured one (false)

◆ useTargetPressure() [2/2]

void mc_tasks::force::CoPTask::useTargetPressure ( bool s )

inlinenoexcept

Wether to compute the desired torque with the target pressure (true) of the measured one (false)

Parameters

s

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

include/mc_tasks/CoPTask.h

Public Member Functions

Protected Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CoPTask() [1/2]

◆ CoPTask() [2/2]

Member Function Documentation

◆ addToGUI()

◆ addToLogger()

◆ buildCompletionCriteria()

◆ load()

◆ measuredCoP()

◆ measuredCoPW()

◆ reset()

◆ setZeroTargetWrench()

◆ targetCoP() [1/2]

◆ targetCoP() [2/2]

◆ targetCoPW()

◆ targetForce() [1/2]

◆ targetForce() [2/2]

◆ targetForceW()

◆ targetWrench()

◆ useTargetPressure() [1/2]

◆ useTargetPressure() [2/2]