Track an exact cubic spline, that is a curve passing exactly through waypoints in position, with optional initial and final velocity and acceleration. More...

#include <mc_tasks/ExactCubicTrajectoryTask.h>

Inheritance diagram for mc_tasks::ExactCubicTrajectoryTask:

[legend]

Collaboration diagram for mc_tasks::ExactCubicTrajectoryTask:

[legend]

Public Member Functions
	ExactCubicTrajectoryTask (const mc_rbdyn::RobotFrame &frame, double duration, double stiffness, double weight, const sva::PTransformd &target, const std::vector< std::pair< double, Eigen::Vector3d >> &posWp={}, const Eigen::Vector3d &init_vel=Eigen::Vector3d::Zero(), const Eigen::Vector3d &init_acc=Eigen::Vector3d::Zero(), const Eigen::Vector3d &end_vel=Eigen::Vector3d::Zero(), const Eigen::Vector3d &end_acc=Eigen::Vector3d::Zero(), const std::vector< std::pair< double, Eigen::Matrix3d >> &oriWp={})
	Trajectory following an exact cubic spline with given initial and final acceleration/velocity. The curve will pass exactly through the specified position waypoints (see posWaypoints) and will interpolate between orientation waypoints. Initial and final acceleration/velocity will also be enforced. More...

	ExactCubicTrajectoryTask (const mc_rbdyn::Robots &robots, unsigned int robotIndex, const std::string &surfaceName, double duration, double stiffness, double weight, const sva::PTransformd &target, const std::vector< std::pair< double, Eigen::Vector3d >> &posWp={}, const Eigen::Vector3d &init_vel=Eigen::Vector3d::Zero(), const Eigen::Vector3d &init_acc=Eigen::Vector3d::Zero(), const Eigen::Vector3d &end_vel=Eigen::Vector3d::Zero(), const Eigen::Vector3d &end_acc=Eigen::Vector3d::Zero(), const std::vector< std::pair< double, Eigen::Matrix3d >> &oriWp={})
	Trajectory following an exact cubic spline with given initial and final acceleration/velocity. The curve will pass exactly through the specified position waypoints (see posWaypoints) and will interpolate between orientation waypoints. Initial and final acceleration/velocity will also be enforced. More...

const mc_trajectory::ExactCubic &	spline () const
	const accessor to the underlying spline (used by SplineTrajectoryTask) More...

mc_trajectory::ExactCubic &	spline ()
	accessor to the underlying spline (used by SplineTrajectoryTask) More...

void	addToGUI (mc_rtc::gui::StateBuilder &gui) override
	Add interactive GUI elements to control the curve waypoints. More...

void	posWaypoints (const std::vector< std::pair< double, Eigen::Vector3d >> &posWp)
	Waypoints in position. The curve will pass exactly through these waypoints. More...

void	constraints (const Eigen::Vector3d &init_vel, const Eigen::Vector3d &init_acc, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc)
	Initial and final velocity and acceleration constraints for the curve. More...

Public Member Functions inherited from mc_tasks::SplineTrajectoryTask< ExactCubicTrajectoryTask >
	SplineTrajectoryTask (const mc_rbdyn::RobotFrame &frame, double duration, double stiffness, double weight, const Eigen::Matrix3d &target, const std::vector< std::pair< double, Eigen::Matrix3d >> &oriWp={})
	Constructor. More...

void	load (mc_solver::QPSolver &solver, const mc_rtc::Configuration &config) override
	Load parameters from a Configuration object. More...

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

void	oriWaypoints (const std::vector< std::pair< double, Eigen::Matrix3d >> &oriWp)
	Sets the orientation waypoints. More...

bool	timeElapsed () const
	Whether the trajectory has finished. More...

Eigen::VectorXd	eval () const override
	Returns the transformError between current robot surface pose and its final target. More...

virtual Eigen::VectorXd	evalTracking () const
	Returns the trajectory tracking error: transformError between the current robot surface pose and its next desired pose along the trajectory error. More...

void	target (const sva::PTransformd &target)
	Sets the curve target pose. Translation target will be handled by the Derived curve, while orientation target is interpolated here. More...

const sva::PTransformd	target () const
	Gets the target pose (position/orientation) More...

void	displaySamples (unsigned s)
	Number of points to sample on the spline for the gui display. More...

unsigned	displaySamples () const
	Number of samples for displaying the spline. More...

void	pause (bool paused)
	Allows to pause the task. More...

bool	pause () const noexcept

double	currentTime () const noexcept

double	duration () const noexcept
	Returns the trajectory's duration. More...

void	dimWeight (const Eigen::VectorXd &dimW) override
	Sets the dimensional weights (controls the importance of orientation/translation). More...

Eigen::VectorXd	dimWeight () const override
	Gets the dimensional weights (orientation/translation) More...

void	dimWeight (const Eigen::VectorXd &dimW) override
	Sets the dimensional weights (controls the importance of orientation/translation). More...

Eigen::VectorXd	dimWeight () const override
	Sets the dimensional weights (controls the importance of orientation/translation). More...

void	stiffness (double stiffness)
	Set the task stiffness/damping. More...

void	stiffness (const Eigen::VectorXd &stiffness)
	Set dimensional stiffness. 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...

void	damping (double damping)
	Set the task damping, leaving its stiffness unchanged. More...

void	damping (const Eigen::VectorXd &damping)
	Set dimensional damping. 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	dimWeightInterpolation (const std::vector< std::pair< double, Eigen::Vector6d >> &dimWeights)

const std::vector< std::pair< double, Eigen::Vector6d > > &	dimWeightInterpolation () const noexcept
	Sets the dimensional weights (controls the importance of orientation/translation). More...

void	stiffnessInterpolation (const std::vector< std::pair< double, Eigen::Vector6d >> &stiffnessGains)

const std::vector< std::pair< double, Eigen::Vector6d > > &	stiffnessInterpolation () const noexcept
	Sets the dimensional weights (controls the importance of orientation/translation). More...

void	dampingInterpolation (const std::vector< std::pair< double, Eigen::Vector6d >> &dampingGains)

const std::vector< std::pair< double, Eigen::Vector6d > > &	dampingInterpolation () const noexcept
	Sets the dimensional weights (controls the importance of orientation/translation). 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	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...

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	targetPos (const Eigen::Vector3d &target)
	Sets the curve target pose. More...

const Eigen::Vector3d &	targetPos () const
	Returns the curve's target position. More...

Protected Member Functions inherited from mc_tasks::SplineTrajectoryTask< ExactCubicTrajectoryTask >
void	refPose (const sva::PTransformd &pose)
	Tracks a reference world pose. More...

const sva::PTransformd &	refPose () const
	Returns the trajectory reference world pose. More...

void	addToGUI (mc_rtc::gui::StateBuilder &gui) override
	Add task controls to the GUI. Interactive controls for the trajectory waypoints and end-endpoints automatically updates the curve. More...

void	addToLogger (mc_rtc::Logger &logger) override
	Add information about the task to the logger. More...

void	update (mc_solver::QPSolver &) override
	Update trajectory target. More...

void	interpolateGains ()

void	refAccel (const Eigen::VectorXd &accel)
	Set the trajectory reference acceleration. More...

const Eigen::VectorXd &	refAccel () const
	Get the trajectory reference acceleration. More...

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

Protected Member Functions inherited from mc_tasks::MetaTask
virtual void	removeFromLogger (mc_rtc::Logger &)

virtual void	removeFromGUI (mc_rtc::gui::StateBuilder &)

Protected Attributes
mc_trajectory::ExactCubic	bspline

sva::PTransformd	initialPose_

Protected Attributes inherited from mc_tasks::SplineTrajectoryTask< ExactCubicTrajectoryTask >
mc_rbdyn::ConstRobotFramePtr	frame_

double	duration_

mc_trajectory::InterpolatedRotation	oriSpline_

SequenceInterpolator6d	dimWeightInterpolator_

SequenceInterpolator6d	stiffnessInterpolator_

SequenceInterpolator6d	dampingInterpolator_

bool	paused_

double	currTime_

unsigned	samples_

bool	inSolver_

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

Friends
struct	SplineTrajectoryTask< ExactCubicTrajectoryTask >

Additional Inherited Members
Public Types inherited from mc_tasks::SplineTrajectoryTask< ExactCubicTrajectoryTask >
using	SplineTrajectoryBase = SplineTrajectoryTask< ExactCubicTrajectoryTask >

using	TrajectoryTask = TrajectoryTaskGeneric

using	SequenceInterpolator6d = mc_trajectory::SequenceInterpolator< Eigen::Vector6d, mc_trajectory::LinearInterpolation< Eigen::Vector6d > >

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

Track an exact cubic spline, that is a curve passing exactly through waypoints in position, with optional initial and final velocity and acceleration.

SplineTrajectoryTask takes care of much of the logic (task target updates, orientation waypoint handling, etc.), and brings in all the functionalities from TrajectoryTaskGeneric. This ExactCubicTrajectoryTask only handles specific aspect of the ExactCubic curve.

Constructor & Destructor Documentation

◆ ExactCubicTrajectoryTask() [1/2]

mc_tasks::ExactCubicTrajectoryTask::ExactCubicTrajectoryTask	(	const mc_rbdyn::RobotFrame &	frame,
		double	duration,
		double	stiffness,
		double	weight,
		const sva::PTransformd &	target,
		const std::vector< std::pair< double, Eigen::Vector3d >> &	posWp = `{}`,
		const Eigen::Vector3d &	init_vel = `Eigen::Vector3d::Zero()`,
		const Eigen::Vector3d &	init_acc = `Eigen::Vector3d::Zero()`,
		const Eigen::Vector3d &	end_vel = `Eigen::Vector3d::Zero()`,
		const Eigen::Vector3d &	end_acc = `Eigen::Vector3d::Zero()`,
		const std::vector< std::pair< double, Eigen::Matrix3d >> &	oriWp = `{}`
	)

Trajectory following an exact cubic spline with given initial and final acceleration/velocity. The curve will pass exactly through the specified position waypoints (see posWaypoints) and will interpolate between orientation waypoints. Initial and final acceleration/velocity will also be enforced.

Parameters

frame	Control frame
duration	Duration of motion (eg time it takes to go from the current
stiffness	Task stiffness
weight	Task weight
target	Final world pose to reach
posWp	Waypoints in position specified as pairs of [time, position]
init_vel	Initial velocity of the curve (default: Zero)
init_acc	Initial acceleration of the curve (default: Zero)
end_vel	Final velocity of the curve (default: Zero)
enc_acc	Final acceleration of the curve (default: Zero)
oriWp	Waypoints in orientation, specified as pairs of [time, orientation].

◆ ExactCubicTrajectoryTask() [2/2]

mc_tasks::ExactCubicTrajectoryTask::ExactCubicTrajectoryTask	(	const mc_rbdyn::Robots &	robots,
		unsigned int	robotIndex,
		const std::string &	surfaceName,
		double	duration,
		double	stiffness,
		double	weight,
		const sva::PTransformd &	target,
		const std::vector< std::pair< double, Eigen::Vector3d >> &	posWp = `{}`,
		const Eigen::Vector3d &	init_vel = `Eigen::Vector3d::Zero()`,
		const Eigen::Vector3d &	init_acc = `Eigen::Vector3d::Zero()`,
		const Eigen::Vector3d &	end_vel = `Eigen::Vector3d::Zero()`,
		const Eigen::Vector3d &	end_acc = `Eigen::Vector3d::Zero()`,
		const std::vector< std::pair< double, Eigen::Matrix3d >> &	oriWp = `{}`
	)

Trajectory following an exact cubic spline with given initial and final acceleration/velocity. The curve will pass exactly through the specified position waypoints (see posWaypoints) and will interpolate between orientation waypoints. Initial and final acceleration/velocity will also be enforced.

Parameters

robots	Robots controlled by the task
robotIndex	Which robot is controlled
surfaceName	Surface controlled by the task, should belong to the controlled robot
duration	Duration of motion (eg time it takes to go from the current
stiffness	Task stiffness
weight	Task weight
target	Final world pose to reach
posWp	Waypoints in position specified as pairs of [time, position]
init_vel	Initial velocity of the curve (default: Zero)
init_acc	Initial acceleration of the curve (default: Zero)
end_vel	Final velocity of the curve (default: Zero)
enc_acc	Final acceleration of the curve (default: Zero)
oriWp	Waypoints in orientation, specified as pairs of [time, orientation].

Member Function Documentation

◆ addToGUI()

void mc_tasks::ExactCubicTrajectoryTask::addToGUI ( mc_rtc::gui::StateBuilder & gui )

overridevirtual

Add interactive GUI elements to control the curve waypoints.

Reimplemented from mc_tasks::MetaTask.

◆ constraints()

void mc_tasks::ExactCubicTrajectoryTask::constraints	(	const Eigen::Vector3d &	init_vel,
		const Eigen::Vector3d &	init_acc,
		const Eigen::Vector3d &	end_vel,
		const Eigen::Vector3d &	end_acc
	)

Initial and final velocity and acceleration constraints for the curve.

Parameters

init_vel	Initial velocity of the curve (default: Zero)
init_acc	Initial acceleration of the curve (default: Zero)
end_vel	Final velocity of the curve (default: Zero)
enc_acc	Final acceleration of the curve (default: Zero)

◆ posWaypoints()

void mc_tasks::ExactCubicTrajectoryTask::posWaypoints ( const std::vector< std::pair< double, Eigen::Vector3d >> & posWp )

Waypoints in position. The curve will pass exactly through these waypoints.

◆ spline() [1/2]

mc_trajectory::ExactCubic& mc_tasks::ExactCubicTrajectoryTask::spline ( )

inline

accessor to the underlying spline (used by SplineTrajectoryTask)

Returns: The spline

◆ spline() [2/2]

const mc_trajectory::ExactCubic& mc_tasks::ExactCubicTrajectoryTask::spline ( ) const

inline

const accessor to the underlying spline (used by SplineTrajectoryTask)

Returns: The spline

◆ targetPos() [1/2]

const Eigen::Vector3d& mc_tasks::ExactCubicTrajectoryTask::targetPos ( ) const

protected

Returns the curve's target position.

◆ targetPos() [2/2]

void mc_tasks::ExactCubicTrajectoryTask::targetPos ( const Eigen::Vector3d & target )

protected

Sets the curve target pose.

Parameters

target Target pose for the curve

Friends And Related Function Documentation

◆ SplineTrajectoryTask< ExactCubicTrajectoryTask >

friend struct SplineTrajectoryTask< ExactCubicTrajectoryTask >

friend

Member Data Documentation

◆ bspline

mc_trajectory::ExactCubic mc_tasks::ExactCubicTrajectoryTask::bspline

protected

◆ initialPose_

sva::PTransformd mc_tasks::ExactCubicTrajectoryTask::initialPose_

protected

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

include/mc_tasks/ExactCubicTrajectoryTask.h

Public Member Functions

Protected Member Functions

Protected Attributes

Friends

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ ExactCubicTrajectoryTask() [1/2]

◆ ExactCubicTrajectoryTask() [2/2]

Member Function Documentation

◆ addToGUI()

◆ constraints()

◆ posWaypoints()

◆ spline() [1/2]

◆ spline() [2/2]

◆ targetPos() [1/2]

◆ targetPos() [2/2]

Friends And Related Function Documentation

◆ SplineTrajectoryTask< ExactCubicTrajectoryTask >

Member Data Documentation

◆ bspline

◆ initialPose_