mc_tasks::LookAtTask Struct Reference

Orient a "gaze" vector defined on a body to look towards a world position. This task is a convenience wrapper around VectorOrientationTask. More...

#include <mc_tasks/LookAtTask.h>

Inheritance diagram for mc_tasks::LookAtTask:
Collaboration diagram for mc_tasks::LookAtTask:

Public Member Functions

 LookAtTask (const mc_rbdyn::RobotFrame &frame, const Eigen::Vector3d &frameVector, double stiffness=2.0, double weight=500.0)
 Constructor with a frame. More...
 
 LookAtTask (const std::string &bodyName, const Eigen::Vector3d &bodyVector, const Eigen::Vector3d &targetPos, const mc_rbdyn::Robots &robots, unsigned int robotIndex, double stiffness=2.0, double weight=500)
 Constructor with user-specified target initialization. More...
 
 LookAtTask (const std::string &bodyName, const Eigen::Vector3d &bodyVector, const mc_rbdyn::Robots &robots, unsigned int robotIndex, double stiffness=2.0, double weight=500)
 Constructor with default target initialization. More...
 
void reset () override
 Reset the task. More...
 
void target (const Eigen::Vector3d &pos)
 Look towards the target frame position. More...
 
Eigen::Vector3d target () const
 Gets the target frame position See targetVector() to obtain the gaze vector. 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::VectorOrientationTask
 VectorOrientationTask (const mc_rbdyn::RobotFrame &frame, const Eigen::Vector3d &frameVector, double stiffness=2.0, double weight=500.0)
 Constructor from a robot frame. More...
 
 VectorOrientationTask (const std::string &bodyName, const Eigen::Vector3d &bodyVector, const Eigen::Vector3d &targetVector, const mc_rbdyn::Robots &robots, unsigned int robotIndex, double stiffness=2.0, double weight=500)
 Constructor with user-specified target. More...
 
 VectorOrientationTask (const std::string &bodyName, const Eigen::Vector3d &bodyVector, const mc_rbdyn::Robots &robots, unsigned int robotIndex, double stiffness=2.0, double weight=500)
 Constructor with default target. More...
 
void reset () override
 Reset the task. More...
 
void targetVector (const Eigen::Vector3d &vector)
 Set world target for the controlled vector. More...
 
Eigen::Vector3d targetVector () const
 Get the target orientation. More...
 
Eigen::Vector3d actual () const
 Get the current body orientation. More...
 
std::string body ()
 Return the controlled body. 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::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
 

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
 
- Protected Member Functions inherited from mc_tasks::VectorOrientationTask
void addToGUI (mc_rtc::gui::StateBuilder &gui) override
 
void addToLogger (mc_rtc::Logger &logger) override
 
- Protected Member Functions inherited from mc_tasks::TrajectoryTaskGeneric
template<Backend backend, typename ErrorT , typename... Args>
void finalize (Args &&... args)
 
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 &)
 
- 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::VectorOrientationTask
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
 

Detailed Description

Orient a "gaze" vector defined on a body to look towards a world position. This task is a convenience wrapper around VectorOrientationTask.

Constructor & Destructor Documentation

◆ LookAtTask() [1/3]

mc_tasks::LookAtTask::LookAtTask ( const mc_rbdyn::RobotFrame frame,
const Eigen::Vector3d &  frameVector,
double  stiffness = 2.0,
double  weight = 500.0 
)

Constructor with a frame.

Parameters
frameControl frame
frameVectorFrame vector in control frame
stiffnessTask stiffness
weightTask weight

◆ LookAtTask() [2/3]

mc_tasks::LookAtTask::LookAtTask ( const std::string &  bodyName,
const Eigen::Vector3d &  bodyVector,
const Eigen::Vector3d &  targetPos,
const mc_rbdyn::Robots robots,
unsigned int  robotIndex,
double  stiffness = 2.0,
double  weight = 500 
)

Constructor with user-specified target initialization.

Parameters
bodyNameName of the body to control
bodyVectorGaze vector for the body. For instance [1., 0, 0] will try to align the x axis of the body with the target direction.
targetPosPosition of target frame to look towards in world frame
robotsRobots controlled by this task
robotIndexIndex of the robot controlled by this task
stiffnessTask stiffness
weightTask weight

◆ LookAtTask() [3/3]

mc_tasks::LookAtTask::LookAtTask ( const std::string &  bodyName,
const Eigen::Vector3d &  bodyVector,
const mc_rbdyn::Robots robots,
unsigned int  robotIndex,
double  stiffness = 2.0,
double  weight = 500 
)

Constructor with default target initialization.

Member Function Documentation

◆ load()

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

Load parameters from a Configuration object.

Reimplemented from mc_tasks::MetaTask.

◆ reset()

void mc_tasks::LookAtTask::reset ( )
overridevirtual

Reset the task.

Implements mc_tasks::MetaTask.

◆ target() [1/2]

Eigen::Vector3d mc_tasks::LookAtTask::target ( ) const

Gets the target frame position See targetVector() to obtain the gaze vector.

Returns
Target vector in world frame

◆ target() [2/2]

void mc_tasks::LookAtTask::target ( const Eigen::Vector3d &  pos)

Look towards the target frame position.

Parameters
posTarget position in world frame

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