#include <TrajectoryThread.h>

Inheritance diagram for ocra_recipes::TrajectoryThread:

Collaboration diagram for ocra_recipes::TrajectoryThread:

Public Member Functions
	TrajectoryThread (int period, const std::string &taskPortName, const TRAJECTORY_TYPE=MIN_JERK, const TERMINATION_STRATEGY _terminationStrategy=STOP_THREAD)

	TrajectoryThread (int period, const std::string &taskPortName, const Eigen::MatrixXd &waypoints, const TRAJECTORY_TYPE=MIN_JERK, const TERMINATION_STRATEGY _terminationStrategy=STOP_THREAD)

	TrajectoryThread (int period, const std::string &taskPortName, const std::list< Eigen::VectorXd > &waypoints, const TRAJECTORY_TYPE=MIN_JERK, const TERMINATION_STRATEGY _terminationStrategy=STOP_THREAD)

	~TrajectoryThread ()

virtual bool	threadInit ()

virtual void	threadRelease ()

virtual void	run ()

void	pause ()

void	unpause ()

void	setMaxVelocity (double maxVel)

void	setMaxVelocity (Eigen::VectorXd maxVel)

void	setMaxAcceleration (double maxAcc)

void	setMaxAcceleration (Eigen::VectorXd maxAcc)

bool	setDisplacement (double dispDouble)

bool	setDisplacement (const Eigen::VectorXd &displacementVector)

bool	setTrajectoryWaypoints (const Eigen::MatrixXd &userWaypoints, bool containsStartingWaypoint=false)

bool	setTrajectoryWaypoints (const std::list< Eigen::VectorXd > &waypointList, bool containsStartingWaypoint=false)

void	setTerminationStrategy (const TERMINATION_STRATEGY newTermStrat)

void	setGoalErrorThreshold (const double newErrorThresh)

void	setUseVarianceModulation (bool newVarMod)

bool	goalAttained ()

double	getDiffError ()

double	getDuration ()

std::list< Eigen::VectorXd >	getWaypointList ()

void	returnToHome ()

bool	isReturningHome ()

Eigen::MatrixXd	getWaypoints ()

void	setMaxVelocityAndAcceleration (double maxVel, double maxAcc)

void	setMaxVelocityAndAcceleration (const Eigen::VectorXd &maxVel, const Eigen::VectorXd &maxAcc)

ocra::Trajectory::Ptr	getTrajectory ()

Protected Member Functions
void	init ()

Eigen::VectorXd	varianceToWeights (Eigen::VectorXd &desiredVariance, const double beta=1.0)

void	flipWaypoints ()

void	cycleWaypoints ()

Protected Attributes
std::shared_ptr< TaskConnection >	task

int	weightDimension

bool	waypointsHaveBeenSet

bool	returningHome

Eigen::MatrixXd	userWaypoints

std::list< Eigen::VectorXd >	userWaypointList

TRAJECTORY_TYPE	trajType

TERMINATION_STRATEGY	terminationStrategy

std::shared_ptr< ocra::Trajectory >	trajectory

double	maximumVariance

bool	useVarianceModulation

Eigen::VectorXd	desiredVariance

Eigen::ArrayXd	varianceThresh

Eigen::VectorXd	startStateVector

Eigen::VectorXd	goalStateVector

Eigen::MatrixXd	allWaypoints

std::list< Eigen::VectorXd >	allWaypointList

double	errorThreshold

Eigen::VectorXd	desiredState

yarp::os::Bottle	desStateBottle

bool	printWaitingNoticeOnce

double	deactivationDelay

double	deactivationTimeout

bool	deactivationLatch

bool	isTaskCurrentlyActive

bool	isPaused

bool	isUsingTerminationStrategy

double	timeElapsedDuringPause

double	pauseTime

ocra::Task::META_TASK_TYPE	taskType

Detailed Description

Definition at line 69 of file TrajectoryThread.h.

Constructor & Destructor Documentation

TrajectoryThread::TrajectoryThread	(	int	period,
		const std::string &	taskPortName,
		const TRAJECTORY_TYPE	trajectoryType = `MIN_JERK`,
		const TERMINATION_STRATEGY	_terminationStrategy = `STOP_THREAD`
	)

Definition at line 48 of file TrajectoryThread.cpp.

TrajectoryThread::TrajectoryThread	(	int	period,
		const std::string &	taskPortName,
		const Eigen::MatrixXd &	waypoints,
		const TRAJECTORY_TYPE	trajectoryType = `MIN_JERK`,
		const TERMINATION_STRATEGY	_terminationStrategy = `STOP_THREAD`
	)

Definition at line 61 of file TrajectoryThread.cpp.

TrajectoryThread::TrajectoryThread	(	int	period,
		const std::string &	taskPortName,
		const std::list< Eigen::VectorXd > &	waypoints,
		const TRAJECTORY_TYPE	trajectoryType = `MIN_JERK`,
		const TERMINATION_STRATEGY	_terminationStrategy = `STOP_THREAD`
	)

Definition at line 76 of file TrajectoryThread.cpp.

TrajectoryThread::~TrajectoryThread ( )

Definition at line 40 of file TrajectoryThread.cpp.

Member Function Documentation

void TrajectoryThread::cycleWaypoints ( )

protected

Definition at line 409 of file TrajectoryThread.cpp.

void TrajectoryThread::flipWaypoints ( )

protected

Definition at line 385 of file TrajectoryThread.cpp.

double TrajectoryThread::getDiffError ( )

Computes the task error as the difference between the goal state vector and its current state

Definition at line 380 of file TrajectoryThread.cpp.

double TrajectoryThread::getDuration ( )

Definition at line 744 of file TrajectoryThread.cpp.

ocra::Trajectory::Ptr ocra_recipes::TrajectoryThread::getTrajectory ( )

inline

Definition at line 214 of file TrajectoryThread.h.

std::list< Eigen::VectorXd > TrajectoryThread::getWaypointList ( )

Definition at line 749 of file TrajectoryThread.cpp.

Eigen::MatrixXd ocra_recipes::TrajectoryThread::getWaypoints ( )

inline

Definition at line 198 of file TrajectoryThread.h.

bool TrajectoryThread::goalAttained ( )

Computes the norm of the difference between the goal state vector and its current state.

Returns: True if the difference is less than the error threshold.

Definition at line 371 of file TrajectoryThread.cpp.

void TrajectoryThread::init ( )

protected

Definition at line 91 of file TrajectoryThread.cpp.

bool ocra_recipes::TrajectoryThread::isReturningHome ( )

inline

Definition at line 196 of file TrajectoryThread.h.

void TrajectoryThread::pause ( )

Sets isPaused to true of this class and registers the pausing time.

Definition at line 348 of file TrajectoryThread.cpp.

void TrajectoryThread::returnToHome ( )

Definition at line 589 of file TrajectoryThread.cpp.

void TrajectoryThread::run ( )

virtual

If the control ports are open (for the specific task associated to the trajectory) this method will mainly write to a port the new desired task state. It also handles the trajectory termination strategy. When the goal is reached, there will be 6 possible termination strategies: BACK_AND_FORTH, CYCLE, STOP_THREAD, STOP_THREAD_DEACTIVATE, WAIT, WAIT_DEACTIVATE. BACK_AND_FORTH: will invert the trajectory waypoints once all of them have been met. CYCLE: The trajectory will cycle over. STOP_THREAD: The thread will stop once the trajectory is finished. STOP_THREAD_DEACTIVATE: The task is first deactivated and then thread is stopped. If the task cannot be deactivated, the thread will wait 1 second before trying once more. WAIT: The thread will wait for new commands to arrive. WAIT_DEACTIVATE: It will first deactivate the task and then wait for new commands. If the task cannot be deactivated, the thread will wait 1 second before trying once more.

Definition at line 171 of file TrajectoryThread.cpp.

bool TrajectoryThread::setDisplacement ( double dispDouble )

Sets a displacement vector with the same constant value for each element and consistent with the weight dimension.

Parameters

dispDouble Constant displacement value.

Returns: True when the new trajectory waypoints have been set, false otherwise.

Definition at line 497 of file TrajectoryThread.cpp.

bool TrajectoryThread::setDisplacement ( const Eigen::VectorXd & displacementVector )

Adds a displacement offset to the waypoints.

Parameters

displacementVector Vector of displacement with the same number of rows as the weight dimension.

Returns: true when the new trajectory waypoints have been set, false otherwise.

Definition at line 502 of file TrajectoryThread.cpp.

void ocra_recipes::TrajectoryThread::setGoalErrorThreshold ( const double newErrorThresh )

inline

Stablishes the admissible error for the trajectory at hand. In particular sets the variable errorThreshold of this class.

Parameters

newErrorThresh Error threshold.

Definition at line 171 of file TrajectoryThread.h.

void TrajectoryThread::setMaxAcceleration ( double maxAcc )

Definition at line 682 of file TrajectoryThread.cpp.

void TrajectoryThread::setMaxAcceleration ( Eigen::VectorXd maxAcc )

Definition at line 689 of file TrajectoryThread.cpp.

void TrajectoryThread::setMaxVelocity ( double maxVel )

Definition at line 668 of file TrajectoryThread.cpp.

void TrajectoryThread::setMaxVelocity ( Eigen::VectorXd maxVel )

Definition at line 675 of file TrajectoryThread.cpp.

void TrajectoryThread::setMaxVelocityAndAcceleration	(	double	maxVel,
		double	maxAcc
	)

Definition at line 697 of file TrajectoryThread.cpp.

void TrajectoryThread::setMaxVelocityAndAcceleration	(	const Eigen::VectorXd &	maxVel,
		const Eigen::VectorXd &	maxAcc
	)

Definition at line 706 of file TrajectoryThread.cpp.

void ocra_recipes::TrajectoryThread::setTerminationStrategy ( const TERMINATION_STRATEGY newTermStrat )

inline

Tells the thread what to do when the trajectory has been fulfilled. In particular sets the variable termimationStrategy of this class.

Parameters

newTermStrat Options are: BACK_AND_FORTH, STOP_THREAD, WAIT, STOP_THREAD_DEACTIVE, WAIT_DEACTIVE, CYCLE

Definition at line 164 of file TrajectoryThread.h.

bool TrajectoryThread::setTrajectoryWaypoints	(	const Eigen::MatrixXd &	userWaypoints,
		bool	containsStartingWaypoint = `false`
	)

Sets the trajectory waypoints and if the initial waypoint is not included, it adds it by checking the current state of the task at hand. E.g., if it's a COM trajectory it will check the current 3D position of this point and add it at the beginning of the waypoints matrix.

Parameters

[in]	userWaypoints	Column-wise trajectory waypoints.
[out]	containsStartingWaypoint	True when the starting waypoint is included in the matrix. When false, this method will add it to the waypoints by checking the current state of the task.

Returns: True when the waypoints matrix is consistent with the task dimension and the full waypoints matrix is set. False otherwise.

Definition at line 426 of file TrajectoryThread.cpp.

bool TrajectoryThread::setTrajectoryWaypoints	(	const std::list< Eigen::VectorXd > &	waypointList,
		bool	containsStartingWaypoint = `false`
	)

Sets the trajectory waypoints and if the initial waypoint is not included, it adds it by checking the current state of the task at hand. E.g., if it's a COM trajectory it will check the current 3D position of this point and add it at the beginning of the waypoints matrix.

Parameters

[in]	waypointList	List of trajectory waypoints.
[out]	containsStartingWaypoint	True when the starting waypoint is included in the matrix. When false, this method will add it to the waypoints by checking the current state of the task.

Returns: True when the waypoints matrix is consistent with the task dimension and the full waypoints matrix is set. False otherwise.

Definition at line 468 of file TrajectoryThread.cpp.

void ocra_recipes::TrajectoryThread::setUseVarianceModulation ( bool newVarMod )

inline

Assigns a user-given variance to the weights of the task when created the trajectory. Used only when trajectoryType is GAUSSIAN_PROCESS

Parameters

newVarMod New weight variance modulation.

Definition at line 178 of file TrajectoryThread.h.

bool TrajectoryThread::threadInit ( )

virtual

If the TrajectoryThread object has been instantiated with defined waypoints (i.e. if waypointsHaveBeenSet is true) the waypoints will be set. Note that if an std::list of waypoints is passed to the constructor of this class, the trajectory type must be TIME_OPTIMAL.

Returns: True when initial waypoints have been set.

Definition at line 145 of file TrajectoryThread.cpp.

void TrajectoryThread::threadRelease ( )

virtual

Closes the control ports opened by the task associated to this trajectory.

Definition at line 165 of file TrajectoryThread.cpp.

void TrajectoryThread::unpause ( )

Sets isPaused to false and stores the elapsed time during the pause.

Definition at line 354 of file TrajectoryThread.cpp.

Eigen::VectorXd TrajectoryThread::varianceToWeights	(	Eigen::VectorXd &	desiredVariance,
		const double	beta = `1.0`
	)

protected

Definition at line 363 of file TrajectoryThread.cpp.

Member Data Documentation

std::list<Eigen::VectorXd> ocra_recipes::TrajectoryThread::allWaypointList

protected

Definition at line 248 of file TrajectoryThread.h.

Eigen::MatrixXd ocra_recipes::TrajectoryThread::allWaypoints

protected

Definition at line 247 of file TrajectoryThread.h.

double ocra_recipes::TrajectoryThread::deactivationDelay

protected

Definition at line 256 of file TrajectoryThread.h.

bool ocra_recipes::TrajectoryThread::deactivationLatch

protected

Definition at line 258 of file TrajectoryThread.h.

double ocra_recipes::TrajectoryThread::deactivationTimeout

protected

Definition at line 257 of file TrajectoryThread.h.

Eigen::VectorXd ocra_recipes::TrajectoryThread::desiredState

protected

Definition at line 251 of file TrajectoryThread.h.

Eigen::VectorXd ocra_recipes::TrajectoryThread::desiredVariance

protected

Definition at line 242 of file TrajectoryThread.h.

yarp::os::Bottle ocra_recipes::TrajectoryThread::desStateBottle

protected

Definition at line 252 of file TrajectoryThread.h.

double ocra_recipes::TrajectoryThread::errorThreshold

protected

Definition at line 249 of file TrajectoryThread.h.

Eigen::VectorXd ocra_recipes::TrajectoryThread::goalStateVector

protected

Definition at line 246 of file TrajectoryThread.h.

bool ocra_recipes::TrajectoryThread::isPaused

protected

Definition at line 261 of file TrajectoryThread.h.

bool ocra_recipes::TrajectoryThread::isTaskCurrentlyActive

protected

Definition at line 260 of file TrajectoryThread.h.

bool ocra_recipes::TrajectoryThread::isUsingTerminationStrategy

protected

Definition at line 262 of file TrajectoryThread.h.

double ocra_recipes::TrajectoryThread::maximumVariance

protected

Definition at line 240 of file TrajectoryThread.h.

double ocra_recipes::TrajectoryThread::pauseTime

protected

Definition at line 265 of file TrajectoryThread.h.

bool ocra_recipes::TrajectoryThread::printWaitingNoticeOnce

protected

Definition at line 254 of file TrajectoryThread.h.

bool ocra_recipes::TrajectoryThread::returningHome

protected

Definition at line 229 of file TrajectoryThread.h.

Eigen::VectorXd ocra_recipes::TrajectoryThread::startStateVector

protected

Definition at line 245 of file TrajectoryThread.h.

std::shared_ptr<TaskConnection> ocra_recipes::TrajectoryThread::task

protected

Definition at line 218 of file TrajectoryThread.h.

ocra::Task::META_TASK_TYPE ocra_recipes::TrajectoryThread::taskType

protected

Definition at line 267 of file TrajectoryThread.h.

TERMINATION_STRATEGY ocra_recipes::TrajectoryThread::terminationStrategy

protected

Definition at line 235 of file TrajectoryThread.h.

double ocra_recipes::TrajectoryThread::timeElapsedDuringPause

protected

Definition at line 264 of file TrajectoryThread.h.

std::shared_ptr<ocra::Trajectory> ocra_recipes::TrajectoryThread::trajectory

protected

Definition at line 237 of file TrajectoryThread.h.

TRAJECTORY_TYPE ocra_recipes::TrajectoryThread::trajType

protected

Definition at line 234 of file TrajectoryThread.h.

std::list<Eigen::VectorXd> ocra_recipes::TrajectoryThread::userWaypointList

protected

Definition at line 232 of file TrajectoryThread.h.

Eigen::MatrixXd ocra_recipes::TrajectoryThread::userWaypoints

protected

Definition at line 231 of file TrajectoryThread.h.

bool ocra_recipes::TrajectoryThread::useVarianceModulation

protected

Definition at line 241 of file TrajectoryThread.h.

Eigen::ArrayXd ocra_recipes::TrajectoryThread::varianceThresh

protected

Definition at line 243 of file TrajectoryThread.h.

bool ocra_recipes::TrajectoryThread::waypointsHaveBeenSet

protected

Definition at line 223 of file TrajectoryThread.h.

int ocra_recipes::TrajectoryThread::weightDimension

protected

Definition at line 219 of file TrajectoryThread.h.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation