The TrajectoryManager class handles trajectories. A pure virtual writeJointState function is used to actually send the next point. More...

#include <TrajectoryManager.h>

Inheritance diagram for TrajectoryManager:

[legend]

List of all members.

Classes
struct	ForceGains
struct	GoalIdCompare
Public Member Functions
double	addCartesianWaypoints (const r2_msgs::PoseTrajectory &trajectory, const sensor_msgs::JointState &startJointPositions, const sensor_msgs::JointState &startJointVels, const sensor_msgs::JointState &prevJointVels, const r2_msgs::PoseState &startPoseState, const r2_msgs::PoseState &startPoseVels)
double	addForceWaypoints (const r2_msgs::PoseTrajectory &trajectory, const sensor_msgs::JointState &startJointPositions, const sensor_msgs::JointState &startJointVels, const sensor_msgs::JointState &prevJointVels, const r2_msgs::PoseState &startPoseState, const r2_msgs::PoseState &startPoseVels, const r2_msgs::ForceControlAxisArray &forceAxes)
	addCartesianWaypoints adds a Cartesian trajectory unless there are conflict, then it stops the conflicts and tells the caller how long to wait before trying the trajectory again
void	addJointBreadcrumbs (const trajectory_msgs::JointTrajectory &trajectory)
void	addJointWaypoints (const trajectory_msgs::JointTrajectory &trajectory, const sensor_msgs::JointState &startJointPositions, const sensor_msgs::JointState &startJointVels, const sensor_msgs::JointState &prevJointVels, const std::vector< r2_msgs::ReplanType > &replanVec=std::vector< r2_msgs::ReplanType >())
void	getJointSettings (r2_msgs::ControllerJointSettings &settingsMsg)
void	initialize (const std::string &urdfFile, double timeStep_in)
void	initialize_sim (const std::string &urdfDesc, double timeStepIn)
void	setBases (const r2_msgs::StringArray &basesMsg)
void	setForceParameters (struct ForceGains new_gains)
void	setIkParameters (double mBar, double kr, unsigned int maxIts, double maxTwist)
void	setJointCapabilities (const r2_msgs::JointCapability &capabilitiesMsg)
void	setJointSettings (const r2_msgs::ControllerJointSettings &settingsMsg)
void	setPoseSettings (const r2_msgs::ControllerPoseSettings &settingsMsg)
void	setPriorityTol (std::vector< double > priorityNum, std::vector< double > priorityLinearTol, std::vector< double > priorityAngularTol)
void	setSensorNameMap (std::vector< std::string > sensorKeys, std::vector< std::string > sensorNames)
void	setVelocityFactor (double velocityFactor_in)
	TrajectoryManager (const std::string &name)
void	updateActualPoseState (const r2_msgs::PoseState &actualPoseState)
void	updateInertia (const sensor_msgs::JointState &inertia_in)
void	updateSensorForces (const r2_msgs::WrenchState &wrenchSensors)
void	updateTrajectories (const r2_msgs::JointCommand &defaultCommandMsg, const r2_msgs::JointControlDataArray &defaultControlMsg, const sensor_msgs::JointState &startJointPositions, const sensor_msgs::JointState &startJointVels)
void	updateTree (const r2_msgs::ToolFrame &toolFrameMsg)
virtual	~TrajectoryManager ()
Protected Member Functions
void	abort (const actionlib_msgs::GoalID &goalId, const std::string &msg=std::string())
void	abort (const actionlib_msgs::GoalID &goalId, std::set< std::string > &abortedJoints, const std::string &msg=std::string())
bool	isActive (const actionlib_msgs::GoalID &goalId) const
	is goal active
void	kasquish (const std::vector< std::string > &jointNames, const sensor_msgs::JointState &startJointPositions, const sensor_msgs::JointState &startJointVels)
void	kasquishAll (const sensor_msgs::JointState &startJointPositions, const sensor_msgs::JointState &startJointVels, const std::string &msg=std::string())
void	preempt (const std::vector< std::string > &jointNames, std::set< std::string > &preemptedJoints, const std::string &msg=std::string())
void	resetAll (const std::string &msg=std::string())
virtual void	writeJointControl (const r2_msgs::JointControlDataArray &jointControl_out)=0
virtual void	writeJointState (const sensor_msgs::JointState &jointState_out)=0
Protected Attributes
bool	basesValid
bool	jointSettingsValid
bool	poseSettingsValid
std::string	sensor
double	stopAccelerationLimit
double	stopVelocityLimit
double	timeStep
ros::Duration	timestep_in
double	velocityFactor
double	zeroLimit
Private Types
typedef TrapezoidalVelocitySynchedCartesianTrajectoryFactory	cartTrajFactory_type
typedef boost::ptr_map < actionlib_msgs::GoalID, JointTrajectoryFollower, GoalIdCompare >	GoalFollowerMap_type
typedef std::map < actionlib_msgs::GoalID, std::vector < r2_msgs::ReplanType > , GoalIdCompare >	GoalReplanMap_type
typedef std::map < actionlib_msgs::GoalID, actionlib_msgs::GoalStatus, GoalIdCompare >	GoalStatusMap_type
typedef TrapezoidalVelocityJointTrajectoryFactory	jointTrajFactory_type
Private Member Functions
void	setupJointControl (const r2_msgs::JointControlDataArray &jointControl)
void	setupJointState (const r2_msgs::JointCommand &jointCommand)
Private Attributes
std::vector< std::string >	actualFrameNames
std::map< std::string, KDL::FrameVel >	actualFrameVelMap
boost::shared_ptr < cartTrajFactory_type >	cartTrajFactory
std::map< std::string, bool >	controllableJoints
boost::shared_ptr < Cartesian_HybCntrl >	forceController
std::map< std::string, KDL::Wrench >	forceSensorMap
GoalFollowerMap_type	goalFollowerMap
GoalReplanMap_type	goalReplanMap
GoalStatusMap_type	goalStatusMap
r2_msgs::JointControlDataArray	jointControlOut
std::map< std::string, unsigned int >	jointIndexMap
std::map< std::string, double >	jointInertiaMap
r2_msgs::ControllerJointSettings	jointSettings
sensor_msgs::JointState	jointStateOut
boost::shared_ptr < jointTrajFactory_type >	jointTrajFactory
boost::shared_ptr < JointNamePositionLimiter >	positionLimiter
RosMsgCartesianTrajectoryFactory	rosCartTrajFactory
RosMsgForceTrajectoryFactory	rosForceTrajFactory
RosMsgJointTrajectoryFactory	rosJointTrajFactory
RosMsgTreeIkTrajectoryFactory	rosTreeIkTrajFactory
RosMsgToolFrameManager	toolFrameManager
boost::shared_ptr< MobileTreeIk >	treeIkPtr

Detailed Description

The TrajectoryManager class handles trajectories. A pure virtual writeJointState function is used to actually send the next point.

Definition at line 41 of file TrajectoryManager.h.

Member Typedef Documentation

typedef TrapezoidalVelocitySynchedCartesianTrajectoryFactory TrajectoryManager::cartTrajFactory_type [private]

Definition at line 171 of file TrajectoryManager.h.

typedef boost::ptr_map<actionlib_msgs::GoalID, JointTrajectoryFollower, GoalIdCompare> TrajectoryManager::GoalFollowerMap_type [private]

Definition at line 194 of file TrajectoryManager.h.

typedef std::map<actionlib_msgs::GoalID, std::vector<r2_msgs::ReplanType>, GoalIdCompare> TrajectoryManager::GoalReplanMap_type [private]

Definition at line 198 of file TrajectoryManager.h.

typedef std::map<actionlib_msgs::GoalID, actionlib_msgs::GoalStatus, GoalIdCompare> TrajectoryManager::GoalStatusMap_type [private]

Definition at line 196 of file TrajectoryManager.h.

typedef TrapezoidalVelocityJointTrajectoryFactory TrajectoryManager::jointTrajFactory_type [private]

Definition at line 170 of file TrajectoryManager.h.

Constructor & Destructor Documentation

TrajectoryManager::TrajectoryManager ( const std::string & name )

Definition at line 3 of file TrajectoryManager.cpp.

TrajectoryManager::~TrajectoryManager ( ) [virtual]

Definition at line 16 of file TrajectoryManager.cpp.

Member Function Documentation

void TrajectoryManager::abort	(	const actionlib_msgs::GoalID &	goalId,
		const std::string &	msg = `std::string()`
	)		`[protected]`

Definition at line 802 of file TrajectoryManager.cpp.

void TrajectoryManager::abort	(	const actionlib_msgs::GoalID &	goalId,
		std::set< std::string > &	abortedJoints,
		const std::string &	msg = `std::string()`
	)		`[protected]`

Definition at line 808 of file TrajectoryManager.cpp.

double TrajectoryManager::addCartesianWaypoints	(	const r2_msgs::PoseTrajectory &	trajectory,
		const sensor_msgs::JointState &	startJointPositions,
		const sensor_msgs::JointState &	startJointVels,
		const sensor_msgs::JointState &	prevJointVels,
		const r2_msgs::PoseState &	startPoseState,
		const r2_msgs::PoseState &	startPoseVels
	)

Definition at line 575 of file TrajectoryManager.cpp.

double TrajectoryManager::addForceWaypoints	(	const r2_msgs::PoseTrajectory &	trajectory,
		const sensor_msgs::JointState &	startJointPositions,
		const sensor_msgs::JointState &	startJointVels,
		const sensor_msgs::JointState &	prevJointVels,
		const r2_msgs::PoseState &	startPoseState,
		const r2_msgs::PoseState &	startPoseVels,
		const r2_msgs::ForceControlAxisArray &	forceAxes
	)

addCartesianWaypoints adds a Cartesian trajectory unless there are conflict, then it stops the conflicts and tells the caller how long to wait before trying the trajectory again

Parameters:

trajectory	trajectory to add
startJointPositions	starting joint positions
startJointVels	starting joint velocities
prevJointVels	previous joint velocities
startPoseState	starting Cartesian position
startPoseVels	starting Cartesian velocity

Returns:: time to wait for conflicts to be resolved

Definition at line 463 of file TrajectoryManager.cpp.

void TrajectoryManager::addJointBreadcrumbs ( const trajectory_msgs::JointTrajectory & trajectory )

Definition at line 753 of file TrajectoryManager.cpp.

void TrajectoryManager::addJointWaypoints	(	const trajectory_msgs::JointTrajectory &	trajectory,
		const sensor_msgs::JointState &	startJointPositions,
		const sensor_msgs::JointState &	startJointVels,
		const sensor_msgs::JointState &	prevJointVels,
		const std::vector< r2_msgs::ReplanType > &	replanVec = `std::vector<r2_msgs::ReplanType>()`
	)

Definition at line 675 of file TrajectoryManager.cpp.

void TrajectoryManager::getJointSettings ( r2_msgs::ControllerJointSettings & settingsMsg )

Definition at line 293 of file TrajectoryManager.cpp.

void TrajectoryManager::initialize	(	const std::string &	urdfFile,
		double	timeStep_in
	)

Definition at line 43 of file TrajectoryManager.cpp.

void TrajectoryManager::initialize_sim	(	const std::string &	urdfDesc,
		double	timeStepIn
	)

Definition at line 20 of file TrajectoryManager.cpp.

bool TrajectoryManager::isActive ( const actionlib_msgs::GoalID & goalId ) const [protected, virtual]

is goal active

Parameters:

goalId goal ID to check

Returns:: bool active state

derived class must provide a way to check this

Implements JointTrajectoryManager.

Definition at line 797 of file TrajectoryManager.cpp.

void TrajectoryManager::kasquish	(	const std::vector< std::string > &	jointNames,
		const sensor_msgs::JointState &	startJointPositions,
		const sensor_msgs::JointState &	startJointVels
	)		`[protected]`

Definition at line 930 of file TrajectoryManager.cpp.

void TrajectoryManager::kasquishAll	(	const sensor_msgs::JointState &	startJointPositions,
		const sensor_msgs::JointState &	startJointVels,
		const std::string &	msg = `std::string()`
	)		`[protected]`

Definition at line 848 of file TrajectoryManager.cpp.

void TrajectoryManager::preempt	(	const std::vector< std::string > &	jointNames,
		std::set< std::string > &	preemptedJoints,
		const std::string &	msg = `std::string()`
	)		`[protected]`

Definition at line 887 of file TrajectoryManager.cpp.

void TrajectoryManager::resetAll ( const std::string & msg = std::string() ) [protected]

Definition at line 1046 of file TrajectoryManager.cpp.

void TrajectoryManager::setBases ( const r2_msgs::StringArray & basesMsg )

Definition at line 322 of file TrajectoryManager.cpp.

void TrajectoryManager::setForceParameters ( struct ForceGains new_gains )

Definition at line 415 of file TrajectoryManager.cpp.

void TrajectoryManager::setIkParameters	(	double	mBar,
		double	kr,
		unsigned int	maxIts,
		double	maxTwist
	)

Definition at line 387 of file TrajectoryManager.cpp.

void TrajectoryManager::setJointCapabilities ( const r2_msgs::JointCapability & capabilitiesMsg )

Definition at line 317 of file TrajectoryManager.cpp.

void TrajectoryManager::setJointSettings ( const r2_msgs::ControllerJointSettings & settingsMsg )

Definition at line 274 of file TrajectoryManager.cpp.

void TrajectoryManager::setPoseSettings ( const r2_msgs::ControllerPoseSettings & settingsMsg )

Definition at line 298 of file TrajectoryManager.cpp.

void TrajectoryManager::setPriorityTol	(	std::vector< double >	priorityNum,
		std::vector< double >	priorityLinearTol,
		std::vector< double >	priorityAngularTol
	)

Definition at line 367 of file TrajectoryManager.cpp.

void TrajectoryManager::setSensorNameMap	(	std::vector< std::string >	sensorKeys,
		std::vector< std::string >	sensorNames
	)

Definition at line 395 of file TrajectoryManager.cpp.

void TrajectoryManager::setupJointControl ( const r2_msgs::JointControlDataArray & jointControl ) [private]

Definition at line 1030 of file TrajectoryManager.cpp.

void TrajectoryManager::setupJointState ( const r2_msgs::JointCommand & jointCommand ) [private]

Definition at line 1005 of file TrajectoryManager.cpp.

void TrajectoryManager::setVelocityFactor ( double velocityFactor_in ) [inline]

Definition at line 89 of file TrajectoryManager.h.

void TrajectoryManager::updateActualPoseState ( const r2_msgs::PoseState & actualPoseState )

Definition at line 443 of file TrajectoryManager.cpp.

void TrajectoryManager::updateInertia ( const sensor_msgs::JointState & inertia_in )

Definition at line 346 of file TrajectoryManager.cpp.

void TrajectoryManager::updateSensorForces ( const r2_msgs::WrenchState & wrenchSensors )

Definition at line 424 of file TrajectoryManager.cpp.

void TrajectoryManager::updateTrajectories	(	const r2_msgs::JointCommand &	defaultCommandMsg,
		const r2_msgs::JointControlDataArray &	defaultControlMsg,
		const sensor_msgs::JointState &	startJointPositions,
		const sensor_msgs::JointState &	startJointVels
	)