MotionPlanner: MotionPlanner Class Reference

List of all members.


Public Member Functions
	MotionPlanner (const OSCAR::DHData &robotData, const OSCAR::Vector &jointOffsets, OSCAR::JointVector &initialJoints, OSCAR::Matrix &jointLimits, OSCAR::Vector &velLimits, OSCAR::Vector &accLimits, OSCAR::Vector &handVelLimits, OSCAR::Vector &handAccLimits, OSCAR::OSCARError &err=OSCAR::DUMMY_ERROR(OSCAR::noError))
	Constructor. This is the constructor for an object of type MotionPlanner.
bool	PlanMove (const OSCAR::JointVector currentJoints, const OSCAR::JointVector targetJoints, MPTrajectoryType trajType, double &moveTime)
	Generates a Trajectory to a target Joint Position This method will generate a trajectory to a target Joint Position. This trajectory can be either JointInterpolated or CartesianInterpolated.
bool	PlanMove (const OSCAR::JointVector currentJoints, const OSCAR::Xform targetHand, MPTrajectoryType trajType, double &moveTime)
	Generates a Trajectory to a target EEF position. This method will generate a trajectory to a target Joint Position. This trajectory can be either JointInterpolated or CartesianInterpolated.
bool	PlanMoveJogJoint (const OSCAR::JointVector &currentJoints, const OSCAR::JointVector &currentVelocity, const std::vector< float > directions)
	Use this method to jog the joints. This method can be used to jog the joints as in teleoperation. When a joint is set to jog, it accelerates to its maximum velocity and coasts until another PlanMoveJogJoint() or a Stop() is called.
bool	PlanMoveJogCartesian (const OSCAR::JointVector &currentJoints, const OSCAR::JointVector &currentVelocity, std::vector< double > directions)
	Use this method to perform a Cartesian jog. This method can be used to jog the EEF as in teleoperation. When an axis is set to jog, it accelerates to its maximum velocity and coasts until another PlanMoveJogCartesian() or a Stop() is called.
bool	Stop (const OSCAR::JointVector &currentJoints, const OSCAR::JointVector &currentVelocity, bool fastest=true)
	Use this method to stop the manipulator motion. This method will decelerate each individual axis to a velocity of 0. If called during a Cartesian move, the robot will not continue in a straight line motion. If called during a jogged move, the robot will stop its motion and return a TrajectoryComplete.
bool	GetJointPosition (OSCAR::JointVector &jointPosVector, OSCAR::JointVector &currentVelocity, TrajectoryGenerator::TrajectoryState &state)
	Use this method to retrieve the current joint position/velocity. This method is used to continually get the current joint position and velocity state. By design, this method should be called repeatedly inside a loop running at the sample rate designated in SetCycleRate(). All computed velocities and accelerations are assuming the joint positions are being updated at this rate.
bool	GetHandPosition (OSCAR::JointVector &joints, OSCAR::Xform &handPosition)
	Use this to calculate the hand position for a given joint configuration. This method can be used to perform the forward kinematics to retrieve a hand position for a provided joint configuration. This method does not change the internal state of the MotionPlanner.
bool	GetHandPosition (OSCAR::Xform &handPosition)
	Use this to retrieve the internal Cartesian handpose state. This method will return the internal Cartesian state of the MotionPlanner. No calculations are performed.
bool	SetJointPosition (const OSCAR::JointVector &joints)
	Use this to set the current joint/Cartesian states of the robot. This method will update all internal kinematics using the provided joint configuration. For revolute joints, the values should be in radians. For prismatic joints, the values should be in the same units as the DH parameters.
bool	SetToolPose (const OSCAR::Xform &toolPose)
	Use this to set the tool pose of the manipulator.
bool	SetBasePose (const OSCAR::Xform &basePose)
	Use this to set the base pose of the manipulator.
bool	SetCycleRate (float rate)
	Use this to set the cycle rate of the manipulator controller.
bool	SetTrajectoryShape (TrajectoryShapeType shape)
	Use this method to switch between Trapezoidal and S-Curve velocity profiles. This method changes the shape of the velocity profile during the acceleration period. Trapezoid uses a constant acceleration profile while SCurve uses a smoother acceleration profile. For joint interpolated motions, the amount of time used to acceleration/deceleration is based on provided limits data. For Cartesian interpolated motions, the amount of time used for acceleration/deceleration can be set using the SetRampTime() method.
bool	SetRampTime (float rampTime)
	Use this method to set the ramp time for Cartesian motions. This method sets the % of the trajectory time to use for the acceleration/deceleration motions for Cartesian . As this value increases, the start-up/slow-down will become smoother but the coast velocity will increase.
bool	SetCartesianControlMode (OSCAR::CartesianCoordinateMode _coordMode)
	Use this method to set the Cartesian Coordinate mode for teleoperation. This method can be used to switch between controlled the EEF in World coordinates or Tool coordinates for teleoperation using the PlanMoveJogCartesian() method. Note: this method will not change the way point-to-point moves are performed.
bool	SetSpeedScale (float scale)
	Use this to set the Speed Scale for the MotionPlanner. This method can be used to slow down the motions for debugging/testing purposes. When the speed scale is set, all subsequent motions (both point-to-point and teleoperation) will be scaled slower based on this value. For example, if the speed scale is set to 0.5, all subsequent motions will execute in exactly twice the time. Note: this value works on top of the velocity/acceleration scales set in SetVelocityScale(float scale) and SetAccelerationScale(float scale). It is mainly designed for testing new trajectories at slower, safer speeds.
bool	SetVelocityScale (float scale)
	Use this to set the Velocity Scale for the MotionPlanner. This value sets the % of max hardware velocities (as set in the constructor) to use for planning trajectories. This applies to the joint limits for both point-to-point and teleoperation motions. For Cartesian motions, this will affect the hand velocity limits provided in the constructor that are used for jogging.
bool	SetAccelerationScale (float scale)
	Use this to set the Acceleration Scale for the MotionPlanner. This value sets the % of max hardware accelerations (as set in the constructor) to use for planning trajectories. This applies to the joint limits for both point-to-point and teleoperation motions. For Cartesian motions, this will affect the hand acceleration limits provided in the constructor that are used for jogging.
bool	ComputeHandVelocity (const OSCAR::JointVector &currentVelocity, OSCAR::HandPose &handVel)
Protected Member Functions
bool	checkLimits (const OSCAR::JointVector &joints, bool checkVelocity=true)
double	estimateMoveTime (const OSCAR::Vector &start, const OSCAR::Vector &end)
bool	getJointPosition (std::vector< OSCAR::Vector > &jointPosBuffer, TrajectoryGenerator::TrajectoryState &state)
bool	cancelMotion ()
Protected Attributes
OSCAR::JointVector	currentJoints
OSCAR::JointVector	prevJoints
OSCAR::JointVector	prevJoints2
OSCAR::JointVector	currentVelocity
OSCAR::JointVector	targetJoints
OSCAR::JointVector	tempJointPosVector
OSCAR::JointVector	tempJointPosVector2
std::list< OSCAR::Vector >	finalBuffer
std::string	robotName
TrajectoryGenerator::TrajectoryState	prevState
OSCAR::GeneralKinematicsHandler *	kinPtr
PathPlanner *	ppPtr
unsigned int	DOF
OSCAR::Vector	velLimits
OSCAR::Vector	accLimits
OSCAR::Vector	handVelLimits
OSCAR::Vector	handAccLimits
OSCAR::Vector	minVelLimits
OSCAR::Vector	minAccLimits
TrajectoryShapeType	trajShape
OSCAR::CartesianCoordinateMode	coordMode
double	cycleRate
double	velScale
double	accScale
double	rampTime
double	maxLinVel
double	timeScale

Detailed Description

Constructor & Destructor Documentation

MotionPlanner::MotionPlanner	(	const OSCAR::DHData &	robotData,
		const OSCAR::Vector &	jointOffsets,
		OSCAR::JointVector &	initialJoints,
		OSCAR::Matrix &	jointLimits,
		OSCAR::Vector &	velLimits,
		OSCAR::Vector &	accLimits,
		OSCAR::Vector &	handVelLimits,
		OSCAR::Vector &	handAccLimits,
		OSCAR::OSCARError &	err = `OSCAR::DUMMY_ERROR(OSCAR::noError)`
	)

Constructor. This is the constructor for an object of type MotionPlanner.

TODO: Limits/DH Parameters are expressed in Degrees while everything else defaults to Radians.

Parameters:

	dhData	A DHData object that defines the DH parameters for the robot. The units of angles in the DHData should be Degrees. The robot DOF is determined from dhData.
	offset	A vector whose size should match the DOF defined in the dhData parameter. The values of offset should define the offset between the zero position of the robot as defined by the DH parameters and as represented by the physical robot zero position. Offset should be defined in Degrees for all joints that are revolute. If the DH paramater defined 0 position is the same as the real robot zero position, then all offsets will be zero.
	jointLimits	A Limits object that defines the position travel limits of the robot arm. The size of the limits object should be the same as the DOF defined in the dhData parameter. The limits should also be expressed in the robot coordinates instead of the DH parameter coordinates. For all revolute joints, the limits should be expressed in Degrees.
	initialJoints	The initial joint state of the manipulator. These joints will be used to initialize the Kinematics of the manipulator and must be a valid joint position. For all revolute joints, the initial position should be express in Radians.
	velLimits	The absolute value of the maximum joint velocities for each joint. These velocities should be express in Radians/s.
	accLimits	The absolute value of the maximum joint accelerations for each joint. These accelerations should be express in Radians/s^2.
	handVelLimits	The absolute value of the maximum cartesian velocities. The translational elements should be in mm/s and the rotational in Radians/s. These limits are used for teleoperation only.
	handAccLimits	The absolute value of the maximum cartesian accelerations. The translational elements should be in mm/s^2 and the rotational in Radians/s^2. These limits are used for teleoperation only.
	err	An OSCARError object that on return will hold the value of any errors that were generated during the constructor call. If err is not equal to 'noError' you can call GetError() to get the details of the error code.

Exceptions:

argumentSizeIncorrect

argumentSizeIncorrect. This error is generated when the size of the input parameter offset and limits does not match the DOF defined by the dhData parameter.

Member Function Documentation

bool MotionPlanner::PlanMove	(	const OSCAR::JointVector	currentJoints,
		const OSCAR::JointVector	targetJoints,
		MPTrajectoryType	trajType,
		double &	moveTime
	)

Generates a Trajectory to a target Joint Position This method will generate a trajectory to a target Joint Position. This trajectory can be either JointInterpolated or CartesianInterpolated.

TODO: Revisit interface - provide speed parameters instead of time?

Parameters:

	currentJoints	This is the current position of the manipulator in Radians.
	targetJoints	The target joint position for the manipulator.
	trajType	This is set to either JointInterpolated or CartesianInterpolated. In the case of CartesianInterpolated, the final hand position is calculated from the final joint position
	moveTime	This parameter contains the move time for the trajector. If this move time is positive, a JointInterpolated move will return true/false based on if the trajectory can be completed without violating constraints while CartesianInterpolated move will will simply generate a trajectory for the given time (in this case, constraint errors will be found during execution). If the move time is 0.0, a JointInterpolated move will complete the move using the max velocity/acceleration constraints while a CartesianInterpolated move will estimate a fastest move time.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::PlanMove	(	const OSCAR::JointVector	currentJoints,
		const OSCAR::Xform	targetHand,
		MPTrajectoryType	trajType,
		double &	moveTime
	)

Generates a Trajectory to a target EEF position. This method will generate a trajectory to a target Joint Position. This trajectory can be either JointInterpolated or CartesianInterpolated.

TODO: Revisit interface - provide speed parameters instead of time?

Parameters:

	currentJoints	This is the current position of the manipulator in Radians.
	targetHand	An Xform containing the target hand position for the manipulator.
	trajType	This is set to either JointInterpolated or CartesianInterpolated. In the case of JointInterpolated, the final joint position is calculated from the final hand position
	moveTime	This parameter contains the move time for the trajector. If this move time is positive, a JointInterpolated move will return true/false based on if the trajectory can be completed without violating constraints while CartesianInterpolated move will will simply generate a trajectory for the given time (in this case, constraint errors will be found during execution). If the move time is 0.0, a JointInterpolated move will complete the move using the max velocity/acceleration constraints while a CartesianInterpolated move will estimate a fastest move time.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::PlanMoveJogJoint	(	const OSCAR::JointVector &	currentJoints,
		const OSCAR::JointVector &	currentVelocity,
		const std::vector< float >	directions
	)

Use this method to jog the joints. This method can be used to jog the joints as in teleoperation. When a joint is set to jog, it accelerates to its maximum velocity and coasts until another PlanMoveJogJoint() or a Stop() is called.

Parameters:

	currentJoints	The current joint state of the manipulator in Radians.
	currentVelocity	The current velocity state of the manipulator in Rad/s.
	directions	A vector of length DOF. The valid values are -1 to 1 with each value corresponding to one joint. If the value is negative, the axis will decelerate from its current velocity to a % of its maximum negative velocity as determined by the velocity limits and velocity scale. For example, a value of -0.5 will decelerate to 50% of the maximum velocity in the negative direction. Similarly, a positive value will accelerate to a % of its maximum positive velocity (once again determined by hardware limits and velocity scale). If the value is 0, the axis will decelerate to a zero velocity.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::PlanMoveJogCartesian	(	const OSCAR::JointVector &	currentJoints,
		const OSCAR::JointVector &	currentVelocity,
		std::vector< double >	directions
	)

Use this method to perform a Cartesian jog. This method can be used to jog the EEF as in teleoperation. When an axis is set to jog, it accelerates to its maximum velocity and coasts until another PlanMoveJogCartesian() or a Stop() is called.

Parameters:

	currentJoints	The current joint state of the manipulator in Radians.
	currentVelocity	The current velocity state of the manipulator in Rad/s.
	directions	A vector of length 6. The 6 values indiciate the x,y,z axes and the three angles in a FixedXYZ orientation description. The valid values are -1 to 1. If the value is negative, the axis will decelerate from its current velocity to a % of its maximum negative velocity. For example, a value of -0.5 will decelerate to 50% of the maximum velocity in the negative direction. If the value is positive, the axis will accelerate from its current velocity to a % of its maximum positive velocity. For example, a value of 0.5 will accelerate to 50% of the maximum velocity in the positive direction. If the value is 0, the axis will decelerate to a zero velocity.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::Stop	(	const OSCAR::JointVector &	currentJoints,
		const OSCAR::JointVector &	currentVelocity,
		bool	fastest = `true`
	)

Use this method to stop the manipulator motion. This method will decelerate each individual axis to a velocity of 0. If called during a Cartesian move, the robot will not continue in a straight line motion. If called during a jogged move, the robot will stop its motion and return a TrajectoryComplete.

Parameters:

	currentJoints	The current joint state of the manipulator in Radians.
	currentVelocity	The current velocity state of the manipulator in Rad/s.
	fastest	If set to true, the manipulator will ignore the Speed Scale value and stop the manipulator as fast as possible. If set set to false, the manipulator will coast to a slower stop if the Speed Scale value is less than 1.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::GetJointPosition	(	OSCAR::JointVector &	jointPosVector,
		OSCAR::JointVector &	currentVelocity,
		TrajectoryGenerator::TrajectoryState &	state
	)

Use this method to retrieve the current joint position/velocity. This method is used to continually get the current joint position and velocity state. By design, this method should be called repeatedly inside a loop running at the sample rate designated in SetCycleRate(). All computed velocities and accelerations are assuming the joint positions are being updated at this rate.

Parameters:

	jointPosVector	The current joint state of the manipulator in Radians.
	currentVelocity	The current velocity state of the manipulator in Rad/s.
	state	This returns the current state of the MotionPlanner. The valid values are: Inactive - The manipulator is currently idle. In this state, the MotionPlanner will be just returning the current position over and over. Active - The manipulator is current moving either through a point-to-point trajectory or through teleoperation. TrajectoryComplete - The manipulator has just complete a point-to-point trajectory

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::GetHandPosition	(	OSCAR::JointVector &	joints,
		OSCAR::Xform &	handPosition
	)

Use this to calculate the hand position for a given joint configuration. This method can be used to perform the forward kinematics to retrieve a hand position for a provided joint configuration. This method does not change the internal state of the MotionPlanner.

Parameters:

	joints	The desired joint configuration.
	handPosition	The resulting hand position.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::GetHandPosition ( OSCAR::Xform & handPosition )

Use this to retrieve the internal Cartesian handpose state. This method will return the internal Cartesian state of the MotionPlanner. No calculations are performed.

Parameters:

handPosition

The resulting hand position.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::SetJointPosition ( const OSCAR::JointVector & joints ) [inline]

Use this to set the current joint/Cartesian states of the robot. This method will update all internal kinematics using the provided joint configuration. For revolute joints, the values should be in radians. For prismatic joints, the values should be in the same units as the DH parameters.

Parameters:

joints

The desired manipulator joint configuration.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::SetToolPose ( const OSCAR::Xform & toolPose ) [inline]

Use this to set the tool pose of the manipulator.

Parameters:

toolPose

The desired manipulator tool pose.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::SetBasePose ( const OSCAR::Xform & basePose ) [inline]

Use this to set the base pose of the manipulator.

Parameters:

basePose

The desired manipulator base pose.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::SetCycleRate ( float rate ) [inline]

Use this to set the cycle rate of the manipulator controller.

Parameters:

rate

The desired cycle rate in hz. The default value is 100. This value can be changed anytime the trajectory status is Inactive.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::SetTrajectoryShape ( TrajectoryShapeType shape )

Use this method to switch between Trapezoidal and S-Curve velocity profiles. This method changes the shape of the velocity profile during the acceleration period. Trapezoid uses a constant acceleration profile while SCurve uses a smoother acceleration profile. For joint interpolated motions, the amount of time used to acceleration/deceleration is based on provided limits data. For Cartesian interpolated motions, the amount of time used for acceleration/deceleration can be set using the SetRampTime() method.

Parameters:

shape

This can be set to Trapezoid or SCurve. The default setting is Trapezoid.

See also:: SetRampTime()

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::SetRampTime ( float rampTime )

Use this method to set the ramp time for Cartesian motions. This method sets the % of the trajectory time to use for the acceleration/deceleration motions for Cartesian . As this value increases, the start-up/slow-down will become smoother but the coast velocity will increase.

Parameters:

rampTime

Valid range is 0.0-0.5. The default setting is 0.15.

See also:: SetTrajectoryShape()

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::SetCartesianControlMode ( OSCAR::CartesianCoordinateMode _coordMode ) [inline]

Use this method to set the Cartesian Coordinate mode for teleoperation. This method can be used to switch between controlled the EEF in World coordinates or Tool coordinates for teleoperation using the PlanMoveJogCartesian() method. Note: this method will not change the way point-to-point moves are performed.

Parameters:

_coordMode

This can be set to World or Tool.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::SetSpeedScale ( float scale ) [inline]

Use this to set the Speed Scale for the MotionPlanner. This method can be used to slow down the motions for debugging/testing purposes. When the speed scale is set, all subsequent motions (both point-to-point and teleoperation) will be scaled slower based on this value. For example, if the speed scale is set to 0.5, all subsequent motions will execute in exactly twice the time. Note: this value works on top of the velocity/acceleration scales set in SetVelocityScale(float scale) and SetAccelerationScale(float scale). It is mainly designed for testing new trajectories at slower, safer speeds.

Parameters:

scale

The desired speed scale. This value must be greater than 0 and less than or equal to 1. A value of 1 represents a full-speed motion.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::SetVelocityScale ( float scale )

Use this to set the Velocity Scale for the MotionPlanner. This value sets the % of max hardware velocities (as set in the constructor) to use for planning trajectories. This applies to the joint limits for both point-to-point and teleoperation motions. For Cartesian motions, this will affect the hand velocity limits provided in the constructor that are used for jogging.

Parameters:

scale

The desired velocity scale. This value must be greater than 0 and less than or equal to 1. A value of 1 will use the maximum hardware velocity in trajectory planning. The default value is 0.95.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

bool MotionPlanner::SetAccelerationScale ( float scale )

Use this to set the Acceleration Scale for the MotionPlanner. This value sets the % of max hardware accelerations (as set in the constructor) to use for planning trajectories. This applies to the joint limits for both point-to-point and teleoperation motions. For Cartesian motions, this will affect the hand acceleration limits provided in the constructor that are used for jogging.

Parameters:

scale

The desired acceleration scale. This value must be greater than 0 and less than or equal to 1. A value of 1 will use the maximum hardware acceleration in trajectory planning. The default value is 0.5.

Returns:: True if no error. False if an error. Call GetError() for detailed error information.

MotionPlanner Class Reference

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation