mc_tasks::TrajectoryTaskGeneric Struct Reference

Generic wrapper for a trajectory dynamic over an error function. More...

#include <mc_tasks/TrajectoryTaskGeneric.h>

Inheritance diagram for mc_tasks::TrajectoryTaskGeneric:
Collaboration diagram for mc_tasks::TrajectoryTaskGeneric:

Public Types

using TrajectoryBase = TrajectoryTaskGeneric
 
- Public Types inherited from mc_tasks::MetaTask
using Backend = mc_solver::QPSolver::Backend
 

Public Member Functions

 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 reset () override
 Reset task target velocity and acceleration to zero. More...
 
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
 
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::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

template<Backend backend, typename ErrorT , typename... Args>
void finalize (Args &&... args)
 
void addToGUI (mc_rtc::gui::StateBuilder &) override
 
void addToLogger (mc_rtc::Logger &logger) override
 
std::function< bool(const mc_tasks::MetaTask &task, std::string &)> buildCompletionCriteria (double dt, const mc_rtc::Configuration &config) const override
 
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

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

- 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

Generic wrapper for a trajectory dynamic over an error function.

This task is meant to be derived to build actual tasks

Member Typedef Documentation

◆ TrajectoryBase

Constructor & Destructor Documentation

◆ TrajectoryTaskGeneric() [1/2]

mc_tasks::TrajectoryTaskGeneric::TrajectoryTaskGeneric ( const mc_rbdyn::Robots robots,
unsigned int  robotIndex,
double  stiffness,
double  weight 
)

Constructor (auto damping)

This is a simple constructor alternative. Damping is set to 2*sqrt(stiffness). This is the most appropriate constructor to use TrajectoryTask as a SetPointTask

Parameters
robotsRobots used in the task
robotIndexIndex of the robot controlled by the task
stiffnessStiffness of the task
weightWeight of the task

◆ TrajectoryTaskGeneric() [2/2]

mc_tasks::TrajectoryTaskGeneric::TrajectoryTaskGeneric ( const mc_rbdyn::RobotFrame frame,
double  stiffness,
double  weight 
)

Constructor (auto damping)

This is a simple constructor alternative. Damping is set to 2*sqrt(stiffness). This is the most appropriate constructor to use TrajectoryTask as a SetPointTask

It is a useful shortcut when the task is controlling a frame

Parameters
frameFrame used in the task
stiffnessStiffness of the task
weightWeight of the task

◆ ~TrajectoryTaskGeneric()

virtual mc_tasks::TrajectoryTaskGeneric::~TrajectoryTaskGeneric ( )
virtualdefault

Member Function Documentation

◆ addToGUI()

void mc_tasks::TrajectoryTaskGeneric::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::SplineTrajectoryTask< ExactCubicTrajectoryTask >, mc_tasks::SplineTrajectoryTask< BSplineTrajectoryTask >, mc_tasks::TransformTask, and mc_tasks::VectorOrientationTask.

◆ addToLogger()

void mc_tasks::TrajectoryTaskGeneric::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::SplineTrajectoryTask< ExactCubicTrajectoryTask >, mc_tasks::SplineTrajectoryTask< BSplineTrajectoryTask >, mc_tasks::TransformTask, and mc_tasks::VectorOrientationTask.

◆ addToSolver()

void mc_tasks::TrajectoryTaskGeneric::addToSolver ( mc_solver::QPSolver solver)
overrideprotectedvirtual

Add the task to a solver.

Parameters
solverSolver where to add the task

Implements mc_tasks::MetaTask.

◆ buildCompletionCriteria()

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

Add additional completion criterias to mc_control::CompletionCriteria object

Based on the input data, this should return a function that operates on the task and return true and complete the output if the criteria has been reached. In this function parameter, the output string should be completed by the function and not overwritten. The task passed to this function is always of the MetaTask type you're implementing and thus can be safely casted.

The default implementation is a function that returns true whatever happens.

Parameters
dtTimestep of the completion criteria
configConfiguration for the CompletionCriteria

Reimplemented from mc_tasks::MetaTask.

Reimplemented in mc_tasks::TransformTask, mc_tasks::SplineTrajectoryTask< ExactCubicTrajectoryTask >, and mc_tasks::SplineTrajectoryTask< BSplineTrajectoryTask >.

◆ damping() [1/3]

double mc_tasks::TrajectoryTaskGeneric::damping ( ) const

Get the current task damping.

◆ damping() [2/3]

void mc_tasks::TrajectoryTaskGeneric::damping ( const Eigen::VectorXd &  damping)

Set dimensional damping.

The caller should be sure that the dimension of the vector fits the task dimension.

Parameters
dampingDimensional damping

◆ damping() [3/3]

void mc_tasks::TrajectoryTaskGeneric::damping ( double  damping)

Set the task damping, leaving its stiffness unchanged.

Parameters
dampingTask stiffness

◆ dimDamping()

const Eigen::VectorXd& mc_tasks::TrajectoryTaskGeneric::dimDamping ( ) const

Get the current task dimensional damping.

◆ dimStiffness()

const Eigen::VectorXd& mc_tasks::TrajectoryTaskGeneric::dimStiffness ( ) const

Get the current task dimensional stiffness.

◆ dimWeight() [1/2]

Eigen::VectorXd mc_tasks::TrajectoryTaskGeneric::dimWeight ( ) const
overridevirtual

Get the current task's dim weight vector.

Implements mc_tasks::MetaTask.

◆ dimWeight() [2/2]

void mc_tasks::TrajectoryTaskGeneric::dimWeight ( const Eigen::VectorXd &  dimW)
overridevirtual

Set the task's dimension weight vector.

It is the caller responsibility to ensure the dimensionality fits the underlying tasks' error function

Parameters
dimWThe new tasks's dimension weight vector

Implements mc_tasks::MetaTask.

Reimplemented in mc_tasks::SplineTrajectoryTask< ExactCubicTrajectoryTask >, and mc_tasks::SplineTrajectoryTask< BSplineTrajectoryTask >.

◆ eval()

Eigen::VectorXd mc_tasks::TrajectoryTaskGeneric::eval ( ) const
overridevirtual

Returns the task error.

The vector's dimensions depend on the underlying task

Implements mc_tasks::MetaTask.

◆ finalize()

template<Backend backend, typename ErrorT , typename... Args>
void mc_tasks::TrajectoryTaskGeneric::finalize ( Args &&...  args)
inlineprotected

This function should be called to finalize the task creation, it will create the actual tasks objects This function must be called by the derived class to finalize the creation of the task

Template Parameters
ErrorTActual type of errorT
Parameters
argsArguments passed to the constructor of errorT

◆ load()

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

◆ normalAcc()

const Eigen::VectorXd& mc_tasks::TrajectoryTaskGeneric::normalAcc ( ) const

◆ refAccel() [1/2]

const Eigen::VectorXd& mc_tasks::TrajectoryTaskGeneric::refAccel ( ) const

Get the trajectory reference acceleration.

◆ refAccel() [2/2]

void mc_tasks::TrajectoryTaskGeneric::refAccel ( const Eigen::VectorXd &  accel)

Set the trajectory reference acceleration.

Parameters
accelNew reference acceleration

◆ refVel() [1/2]

const Eigen::VectorXd& mc_tasks::TrajectoryTaskGeneric::refVel ( ) const

Get the trajectory reference velocity.

◆ refVel() [2/2]

void mc_tasks::TrajectoryTaskGeneric::refVel ( const Eigen::VectorXd &  vel)

Set the trajectory reference velocity.

Parameters
velNew reference velocity

◆ removeFromSolver()

void mc_tasks::TrajectoryTaskGeneric::removeFromSolver ( mc_solver::QPSolver solver)
overrideprotectedvirtual

Remove the task from a solver.

Parameters
solverSolver from which to remove the task

Implements mc_tasks::MetaTask.

◆ reset()

void mc_tasks::TrajectoryTaskGeneric::reset ( )
overridevirtual

Reset task target velocity and acceleration to zero.

Implements mc_tasks::MetaTask.

Reimplemented in mc_tasks::VectorOrientationTask, and mc_tasks::TransformTask.

◆ resetJointsSelector() [1/2]

virtual void mc_tasks::TrajectoryTaskGeneric::resetJointsSelector ( )
virtual

◆ resetJointsSelector() [2/2]

void mc_tasks::TrajectoryTaskGeneric::resetJointsSelector ( mc_solver::QPSolver solver)
overridevirtual

Reset active joints selection.

Parameters
solverSolver where the task is involved

Implements mc_tasks::MetaTask.

◆ selectActiveJoints() [1/2]

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

Warning
This function should only be called if the task hasn't yet been added to the solver. If the tasks is already in the solver it does nothing, and warns that it had no effect. Call void selectActiveJoints(mc_solver::QPSolver &, const std::vector<std::string> &, const std::map<std::string, std::vector<std::array<int, 2>>> &) instead.
Parameters
activeJointsNameName of the joints activated for this task
activeDofsAllow to select only part of the dofs of a joint
checkJointsWhen true, checks whether the joints exist in the robot
Exceptions
IfcheckJoints is true and a joint name in activeJointsName is not part of the robot

◆ selectActiveJoints() [2/2]

void mc_tasks::TrajectoryTaskGeneric::selectActiveJoints ( mc_solver::QPSolver solver,
const std::vector< std::string > &  activeJointsName,
const std::map< std::string, std::vector< std::array< int, 2 >>> &  activeDofs = {} 
)
overridevirtual

Create an active joints selector.

Note
Calling this function is a bit expensive. If the task is already in the solver, it will be removed first, then recreated with the joint selector and added to the solver again. If possible, consider calling void selectActiveJoints(const std::vector<std::string> &, const std::map<std::string, std::vector<std::array<int, 2>>> &) before adding the task to the solver.
Parameters
activeJointsNameName of the joints activated for this task
activeDofsAllow to select only part of the dofs of a joint
Exceptions
Ifa joint name in activeJointsName is not part of the robot

Implements mc_tasks::MetaTask.

◆ selectUnactiveJoints() [1/2]

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

Warning
This function should only be called if the task hasn't yet been added to the solver. If the tasks is already in the solver it does nothing, and warns that it had no effect. Call void selectUnactiveJoints(mc_solver::QPSolver &, const std::vector<std::string> &, const std::map<std::string, std::vector<std::array<int, 2>>> &) instead.
Parameters
unactiveJointsNameName of the joints not activated for this task
unactiveDofsAllow to select only part of the dofs of a joint
checkJointsWhen true, checks whether the joints exist in the robot
Exceptions
IfcheckJoints is true and a joint name in unactiveJointsName is not part of the robot

◆ selectUnactiveJoints() [2/2]

void mc_tasks::TrajectoryTaskGeneric::selectUnactiveJoints ( mc_solver::QPSolver solver,
const std::vector< std::string > &  unactiveJointsName,
const std::map< std::string, std::vector< std::array< int, 2 >>> &  unactiveDofs = {} 
)
overridevirtual

Create an unactive joints selector.

Note
Calling this function is a bit expensive. If the task is already in the solver, it will be removed first, then recreated with the joint selector and added to the solver again. If possible, consider calling void selectUnactiveJoints(const std::vector<std::string> &, const std::map<std::string, std::vector<std::array<int, 2>>> &) before adding the task to the solver.
Parameters
unactiveJointsNameName of the joints not activated for this task
unactiveDofsAllow to select only part of the dofs of a joint
Exceptions
Ifa joint name in unactiveJointsName is not part of the robot

Implements mc_tasks::MetaTask.

◆ setGains() [1/2]

void mc_tasks::TrajectoryTaskGeneric::setGains ( const Eigen::VectorXd &  stiffness,
const Eigen::VectorXd &  damping 
)

Set dimensional stiffness and damping.

The caller should be sure that the dimensions of the vectors fit the task dimension.

Parameters
stiffnessDimensional stiffness
dampingDimensional damping

◆ setGains() [2/2]

void mc_tasks::TrajectoryTaskGeneric::setGains ( double  stiffness,
double  damping 
)

Set both stiffness and damping.

Parameters
stiffnessTask stiffness
dampingTask damping

◆ speed()

Eigen::VectorXd mc_tasks::TrajectoryTaskGeneric::speed ( ) const
overridevirtual

Returns the task velocity.

The vector's dimensions depend on the underlying task

Implements mc_tasks::MetaTask.

◆ stiffness() [1/3]

double mc_tasks::TrajectoryTaskGeneric::stiffness ( ) const

Get the current task stiffness.

◆ stiffness() [2/3]

void mc_tasks::TrajectoryTaskGeneric::stiffness ( const Eigen::VectorXd &  stiffness)

Set dimensional stiffness.

The caller should be sure that the dimension of the vector fits the task dimension.

Damping is automatically set to 2*sqrt(stiffness)

Parameters
stiffnessDimensional stiffness

◆ stiffness() [3/3]

void mc_tasks::TrajectoryTaskGeneric::stiffness ( double  stiffness)

Set the task stiffness/damping.

Damping is automatically set to 2*sqrt(stiffness)

Parameters
stiffnessTask stiffness

◆ update()

void mc_tasks::TrajectoryTaskGeneric::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::SplineTrajectoryTask< ExactCubicTrajectoryTask >, mc_tasks::SplineTrajectoryTask< BSplineTrajectoryTask >, and mc_tasks::LookAtTFTask.

◆ weight() [1/2]

double mc_tasks::TrajectoryTaskGeneric::weight ( ) const

Returns the task weight.

◆ weight() [2/2]

void mc_tasks::TrajectoryTaskGeneric::weight ( double  w)

Set the task weight.

Parameters
wTask weight

Member Data Documentation

◆ damping_

Eigen::VectorXd mc_tasks::TrajectoryTaskGeneric::damping_
protected

◆ errorT

mc_rtc::void_ptr mc_tasks::TrajectoryTaskGeneric::errorT {nullptr, nullptr}
protected

Pointer to the error function

The actual type depends on the implementation

◆ inSolver_

bool mc_tasks::TrajectoryTaskGeneric::inSolver_ = false
protected

◆ refAccel_

Eigen::VectorXd mc_tasks::TrajectoryTaskGeneric::refAccel_
protected

◆ refVel_

Eigen::VectorXd mc_tasks::TrajectoryTaskGeneric::refVel_
protected

◆ rIndex

unsigned int mc_tasks::TrajectoryTaskGeneric::rIndex
protected

◆ robots

const mc_rbdyn::Robots& mc_tasks::TrajectoryTaskGeneric::robots
protected

◆ selectorT_

mc_rtc::void_ptr mc_tasks::TrajectoryTaskGeneric::selectorT_ {nullptr, nullptr}
protected

Pointer to a dynamic wrapper that select specific joints

In Tasks backend:

  • tasks::qp::JointSelector

In TVM backend: mc_tvm::JointsSelectorFunction

◆ stiffness_

Eigen::VectorXd mc_tasks::TrajectoryTaskGeneric::stiffness_
protected

◆ trajectoryT_

mc_rtc::void_ptr mc_tasks::TrajectoryTaskGeneric::trajectoryT_ {nullptr, nullptr}
protected

Pointer to the trajectory dynamic

In Tasks backend:

  • tasks::qp::TrajectoryTask

In TVM backend:

◆ weight_

double mc_tasks::TrajectoryTaskGeneric::weight_
protected

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