PostureMC Class Reference

a MotionCommand shell for PostureEngine More...

#include <PostureMC.h>

Inheritance diagram for PostureMC:

Detailed Description

Will autoprune by default once it reaches the target pose.

If you want to keep it alive so your behavior can repeatedly use the posture to move around, either call setAutoPrune(false), or (preferably) use MotionManager::addPersistentMotion() when adding the motion.

This class will throw a status event when the sensors confirm the robot has reached a specified target posture, within a configurable range of tolerance. The class will keep trying to reach a target position for timeout milliseconds before giving up.

By default, this class will adhere to the values of MaxOutputSpeed, except for head joints which use Config::motion_config values.

Definition at line 28 of file PostureMC.h.

List of all members.

Public Member Functions
Constructors
timeout will be set to a default value of 2 seconds, tolerance is .035 radians (~2 degrees), hold is true
	PostureMC ()
	constructor
	PostureMC (const std::string &filename)
	constructor, loads from filename
	PostureMC (const WorldState *st)
	constructor, initializes joint positions to the current state of the outputs as defined by state
virtual	~PostureMC ()
	destructor
Physical Implementation Issues
These functions allow you to mediate problems with a physical robot, such as tolerance of joints not going quite where the should, or safe speed of movement
PostureMC &	setDirty (bool d=true)
	Call this if you call PostureEngine::setOutputCmd(), or set values through getOutputCmd()'s reference, since neither will know to set the PostureMC dirty flag.
virtual PostureMC &	setHold (bool h=true)
	Sets hold - if this is set to false, it will allow a persistent motion to behave the same as a pruned motion, without being pruned.
virtual bool	getHold ()
	return hold
virtual PostureMC &	setTolerance (float t)
	sets tolerance, returns `*this`
virtual float	getTolerance ()
	returns tolerance
virtual PostureMC &	setTimeout (unsigned int delay)
	sets timeout, returns `*this`
virtual unsigned int	getTimeout ()
	returns timeout
void	noMaxSpeed ()
	Sets maxSpeeds to 0 (no maximum).
void	defaultMaxSpeed (float x=1)
	Sets maxSpeeds to x% of the default MaxOutputSpeed values from the robot info file (e.g. CalliopeInfo.h).
void	setMaxSpeed (unsigned int i, float x)
	Sets maxSpeeds[i] entry, in rad/sec.
float	getMaxSpeed (unsigned int i)
	Returns maxSpeeds[i] entry, in rad/sec.
float	getCurrentValue (unsigned int i)
	returns curPositions[i], which indicates the last commanded position
MotionCommand Stuff

virtual int	updateOutputs ()
	returns non-zero (true) if PostureEngine::maxdiff() between this and the current position is over tolerance
virtual int	isDirty ()
	returns non-zero (true) if PostureEngine::maxdiff() between this and the current position is over tolerance
virtual int	isAlive ()
	returns non-zero (true) if PostureEngine::maxdiff() between this and the current position is over tolerance
virtual void	doStart ()
	marks this as dirty each time it is added
PostureEngine Stuff
These functions merely set the dirty flag and then call the PostureEngine version of the function
virtual void	takeSnapshot ()
	sets the values of cmds to the current state of the outputs (doesn't change the weights)
virtual void	takeSnapshot (const WorldState &st)
	sets the values of cmds to the current state of the outputs as defined by state (doesn't change the weights)
virtual void	setWeights (float w)
	set the weights of all cmds
virtual void	setWeights (float w, unsigned int lowjoint, unsigned int highjoint)
	the the weights of a range of cmds
virtual void	clear ()
	sets all joints to unused
virtual PostureEngine &	setOverlay (const PostureEngine &pe)
	sets joints of this to all joints of pe which are not equal to unused (layers pe over this) stores into this
virtual PostureEngine &	setUnderlay (const PostureEngine &pe)
	sets joints of this which are equal to unused to pe, (layers this over pe) stores into this
virtual PostureEngine &	setAverage (const PostureEngine &pe, float w=0.5)
	computes a weighted average of this vs. pe, w being the weight towards pe (so w==1 just copies pe)
virtual PostureEngine &	setCombine (const PostureEngine &pe)
	computes a weighted average of this vs. pe, using the weight values of the joints, storing the total weight in the result's weight value
PostureEngine &	setOutputCmd (unsigned int i, const OutputCmd &c)
	sets output i to OutputCmd c, returns `*this` so you can chain them; also remember that OutputCmd support implicit conversion from floats (so you can just pass a float)
virtual unsigned int	loadBuffer (const char buf[], unsigned int len, const char *filename=NULL)
	sets saveFormatCondensed and loadSaveSensors (pass state for ws if you want to use current sensor values)
virtual bool	solveLinkPosition (const fmat::SubVector< 3, const float > &Ptgt, unsigned int link, const fmat::SubVector< 3, const float > &Peff)
	Performs inverse kinematics to solve for positioning Peff on link j as close as possible to Ptgt (base coordinates in homogenous form); if solution found, stores result in this posture and returns true.
virtual bool	solveLinkPosition (float Ptgt_x, float Ptgt_y, float Ptgt_z, unsigned int link, float Peff_x, float Peff_y, float Peff_z)
	Performs inverse kinematics to solve for positioning Peff on link j as close as possible to Ptgt (base coordinates); if solution found, stores result in this posture and returns true.
virtual bool	solveLinkVector (const fmat::SubVector< 3, const float > &Ptgt, unsigned int link, const fmat::SubVector< 3, const float > &Peff)
	Performs inverse kinematics to solve for aligning the vector through Peff on link j and the link's origin to point at Ptgt (base coordinates in homogenous form); if solution found, stores result in this posture and returns true.
virtual bool	solveLinkVector (float Ptgt_x, float Ptgt_y, float Ptgt_z, unsigned int link, float Peff_x, float Peff_y, float Peff_z)
	Performs inverse kinematics to solve for aligning the vector through Peff on link j and the link's origin to point at Ptgt (base coordinates); if solution found, stores result in this posture and returns true.
Protected Member Functions
void	init ()
	initialize curPositions and maxSpeeds
Protected Attributes
bool	dirty
	true if changes have been made since last updateOutputs()
bool	hold
	if set to true, the posture will be kept active; otherwise joints will be marked unused after each posture is achieved (as if the posture was pruned); set through setHold()
float	tolerance
	when autopruning, if the maxdiff() of this posture and the robot's current position is below this value, isAlive() will be false, defaults to 0.01 (5.7 degree error)
bool	targetReached
	false if any joint is still moving towards its target
unsigned int	targetTimestamp
	time at which the targetReached flag was set
unsigned int	timeout
	number of milliseconds to wait before giving up on a target that should have already been reached, a value of -1U will try forever
float	curPositions [NumOutputs]
	stores the last commanded value for each joint
float	maxSpeeds [NumOutputs]
	radians per frame. Initialized from Config::motion_config, but can be overridden by setMaxSpeed()

Constructor & Destructor Documentation

PostureMC::PostureMC ( )

constructor

Definition at line 34 of file PostureMC.h.

PostureMC::PostureMC ( const std::string & filename )

constructor, loads from filename

Definition at line 40 of file PostureMC.h.

PostureMC::PostureMC ( const WorldState * st )

constructor, initializes joint positions to the current state of the outputs as defined by state

Definition at line 46 of file PostureMC.h.

virtual PostureMC::~PostureMC ( ) [virtual]

destructor

Definition at line 52 of file PostureMC.h.

Member Function Documentation

virtual void PostureMC::clear ( ) [virtual]

sets all joints to unused

Reimplemented from PostureEngine.

Definition at line 131 of file PostureMC.h.

void PostureMC::defaultMaxSpeed ( float x = 1 )

Sets maxSpeeds to x% of the default MaxOutputSpeed values from the robot info file (e.g. CalliopeInfo.h).

Neck joints are instead set from the applicable Config::motion_config values.

Parameters:

x

ratio of the max speed to use; so 0.5 would limit motion to half the recommended upper limit

Definition at line 18 of file PostureMC.cc.

Referenced by EmergencyStopMC::EmergencyStopMC(), and init().

virtual void PostureMC::doStart ( ) [virtual]

marks this as dirty each time it is added

Reimplemented from MotionCommand.

Definition at line 122 of file PostureMC.h.

float PostureMC::getCurrentValue ( unsigned int i )

returns curPositions[i], which indicates the last commanded position

This is particularly useful when a maximum speed is set, and you want to know progress towards the target (cmds)

Definition at line 107 of file PostureMC.h.

virtual bool PostureMC::getHold ( ) [virtual]

return hold

Definition at line 79 of file PostureMC.h.

float PostureMC::getMaxSpeed ( unsigned int i )

Returns maxSpeeds[i] entry, in rad/sec.

Parameters:

i

joint offset, see your model's info file (e.g. ERS7Info.h)

Returns:: the maximum speed of joint i in radians per second

Definition at line 103 of file PostureMC.h.

virtual unsigned int PostureMC::getTimeout ( ) [virtual]

returns timeout

Definition at line 84 of file PostureMC.h.

virtual float PostureMC::getTolerance ( ) [virtual]

returns tolerance

Definition at line 82 of file PostureMC.h.

void PostureMC::init ( ) [protected]

initialize curPositions and maxSpeeds

Reimplemented from Kinematics.

Definition at line 101 of file PostureMC.cc.

Referenced by PostureMC().

int PostureMC::isAlive ( ) [virtual]

returns non-zero (true) if PostureEngine::maxdiff() between this and the current position is over tolerance

This is handy so you can set to have the robot go to a position and then automatically remove the MotionCommand when it gets there - but beware fighting Postures which average out and neither succeeds

Implements MotionCommand.

Definition at line 83 of file PostureMC.cc.

virtual int PostureMC::isDirty ( ) [virtual]

returns non-zero (true) if PostureEngine::maxdiff() between this and the current position is over tolerance

This is handy so you can set to have the robot go to a position and then automatically remove the MotionCommand when it gets there - but beware fighting Postures which average out and neither succeeds

Implements MotionCommand.

Definition at line 115 of file PostureMC.h.

Referenced by updateOutputs().

virtual unsigned int PostureMC::loadBuffer	(	const char	buf[],
		unsigned int	len,
		const char *	filename = `NULL`
	)			`[virtual]`

sets saveFormatCondensed and loadSaveSensors (pass state for ws if you want to use current sensor values)

Reimplemented from PostureEngine.

Definition at line 140 of file PostureMC.h.

void PostureMC::noMaxSpeed ( )

Sets maxSpeeds to 0 (no maximum).

Definition at line 87 of file PostureMC.h.

virtual PostureEngine& PostureMC::setAverage	(	const PostureEngine &	pe,
		float	w = `0.5`
	)			`[virtual]`

computes a weighted average of this vs. pe, w being the weight towards pe (so w==1 just copies pe)

joints being averaged with unused joints have their weights averaged, but not their values (so an output can crossfade properly)

Parameters:

pe

the other PostureEngine

w

amount to weight towards pe

if w < .001, nothing is done
if w > .999, a straight copy of pe occurs (sets joints to unused properly at end of fade)
.001 and .999 is used instead of 0 and 1 to allow for slight addition errors in a loop (if using repeated additions of a delta value instead of repeated divisions)

Returns:: *this, stores results into this

Reimplemented from PostureEngine.

Definition at line 134 of file PostureMC.h.

virtual PostureEngine& PostureMC::setCombine ( const PostureEngine & pe ) [virtual]

computes a weighted average of this vs. pe, using the weight values of the joints, storing the total weight in the result's weight value

Reimplemented from PostureEngine.

Definition at line 135 of file PostureMC.h.

PostureMC & PostureMC::setDirty ( bool d = true )

Call this if you call PostureEngine::setOutputCmd(), or set values through getOutputCmd()'s reference, since neither will know to set the PostureMC dirty flag.

This is the downside of making setOutputCmd() not virtual - if you pass this around as just a PostureEngine, calls made to that won't be able to update the dirty flag automatically. However, if the object is typed as a PostureMC at the time the call is made, then you don't have to worry.
These are also not virtual

otherwise you'd have to still check the motion out and it would make this all pointless!

MotionManager::getOutputCmd() is called instead of WorldState::outputs[] because if this is being called rapidly (i.e. after every sensor reading) using the sensor values will cause problems with very slow acceleration due to sensor lag continually resetting the current position. Using the last value sent by the MotionManager fixes this.

Returns:: *this

Definition at line 10 of file PostureMC.cc.

Referenced by clear(), doStart(), init(), loadBuffer(), setAverage(), setCombine(), setOverlay(), setUnderlay(), setWeights(), solveLinkPosition(), solveLinkVector(), and takeSnapshot().

virtual PostureMC& PostureMC::setHold ( bool h = true ) [virtual]

Sets hold - if this is set to false, it will allow a persistent motion to behave the same as a pruned motion, without being pruned.

Definition at line 78 of file PostureMC.h.

void PostureMC::setMaxSpeed	(	unsigned int	i,
		float	x
	)

Sets maxSpeeds[i] entry, in rad/sec.

Parameters:

	i	joint offset, see your model's info file (e.g. ERS7Info.h)
	x	maximum radians per second to move

Definition at line 98 of file PostureMC.h.

PostureEngine& PostureMC::setOutputCmd	(	unsigned int	i,
		const OutputCmd &	c
	)

sets output i to OutputCmd c, returns *this so you can chain them; also remember that OutputCmd support implicit conversion from floats (so you can just pass a float)

Reimplemented from PostureEngine.

Definition at line 137 of file PostureMC.h.

Referenced by BatteryMonitorBehavior::setFlipper().

virtual PostureEngine& PostureMC::setOverlay ( const PostureEngine & pe ) [virtual]

sets joints of this to all joints of pe which are not equal to unused (layers pe over this) stores into this

Reimplemented from PostureEngine.

Definition at line 132 of file PostureMC.h.

virtual PostureMC& PostureMC::setTimeout ( unsigned int delay ) [virtual]

sets timeout, returns *this

Definition at line 83 of file PostureMC.h.

virtual PostureMC& PostureMC::setTolerance ( float t ) [virtual]

sets tolerance, returns *this

Definition at line 81 of file PostureMC.h.

virtual PostureEngine& PostureMC::setUnderlay ( const PostureEngine & pe ) [virtual]

sets joints of this which are equal to unused to pe, (layers this over pe) stores into this

Reimplemented from PostureEngine.

Definition at line 133 of file PostureMC.h.

virtual void PostureMC::setWeights	(	float	w,
		unsigned int	lowjoint,
		unsigned int	highjoint
	)			`[virtual]`

the the weights of a range of cmds

Reimplemented from PostureEngine.

Definition at line 130 of file PostureMC.h.

virtual void PostureMC::setWeights ( float w ) [virtual]

set the weights of all cmds

Reimplemented from PostureEngine.

Definition at line 129 of file PostureMC.h.

Referenced by setWeights().

virtual bool PostureMC::solveLinkPosition	(	float	Ptgt_x,
		float	Ptgt_y,
		float	Ptgt_z,
		unsigned int	link,
		float	Peff_x,
		float	Peff_y,
		float	Peff_z
	)			`[virtual]`

Performs inverse kinematics to solve for positioning Peff on link j as close as possible to Ptgt (base coordinates); if solution found, stores result in this posture and returns true.

Parameters:

	Ptgt_x	the target x position (relative to base frame)
	Ptgt_y	the target y position (relative to base frame)
	Ptgt_z	the target z position (relative to base frame)
	link	the output offset of the joint to move
	Peff_x	the x position (relative to link) which you desire to have moved to Ptgt (it's a point on the effector)
	Peff_y	the y position (relative to link) which you desire to have moved to Ptgt (it's a point on the effector)
	Peff_z	the z position (relative to link) which you desire to have moved to Ptgt (it's a point on the effector)

The difference between solveLinkPosition() and solveLinkVector() is typically small, but critical when you're trying to look at something -- the solution obtained by simplying trying to solve for the position may not align the vector with the target -- solveLinkVector() tries to ensure the vector is aligned with the target, even if that isn't the closest solution position-wise.

Reimplemented from PostureEngine.

Definition at line 146 of file PostureMC.h.

virtual bool PostureMC::solveLinkPosition	(	const fmat::SubVector< 3, const float > &	Ptgt,
		unsigned int	link,
		const fmat::SubVector< 3, const float > &	Peff
	)			`[virtual]`

Performs inverse kinematics to solve for positioning Peff on link j as close as possible to Ptgt (base coordinates in homogenous form); if solution found, stores result in this posture and returns true.

Parameters:

	Ptgt	the target point, in base coordinates
	link	the output offset of the joint to move
	Peff	the point (relative to link) which you desire to have moved to Ptgt (it's a point on the effector, e.g. (0,0,0) if you want to move the effector's origin to the target)

The difference between solveLinkPosition() and solveLinkVector() is typically small, but critical when you're trying to look at something -- the solution obtained by simplying trying to solve for the position may not align the vector with the target -- solveLinkVector() tries to ensure the vector is aligned with the target, even if that isn't the closest solution position-wise.

Reimplemented from PostureEngine.

Definition at line 143 of file PostureMC.h.

Referenced by solveLinkPosition().

virtual bool PostureMC::solveLinkVector	(	float	Ptgt_x,
		float	Ptgt_y,
		float	Ptgt_z,
		unsigned int	link,
		float	Peff_x,
		float	Peff_y,
		float	Peff_z
	)			`[virtual]`

Performs inverse kinematics to solve for aligning the vector through Peff on link j and the link's origin to point at Ptgt (base coordinates); if solution found, stores result in this posture and returns true.

Parameters:

	Ptgt_x	the target x position (relative to base frame)
	Ptgt_y	the target y position (relative to base frame)
	Ptgt_z	the target z position (relative to base frame)
	link	the output offset of the joint to move
	Peff_x	the x position (relative to link) which you desire to have moved to Ptgt (it's the desired "effector")
	Peff_y	the y position (relative to link) which you desire to have moved to Ptgt (it's the desired "effector")
	Peff_z	the z position (relative to link) which you desire to have moved to Ptgt (it's the desired "effector")

Todo:: this method is an approximation, could be more precise, and perhaps faster, although this is pretty good.

The difference between solveLinkPosition() and solveLinkVector() is typically small, but critical when you're trying to look at something -- the solution obtained by simplying trying to solve for the position may not align the vector with the target -- solveLinkVector() tries to ensure the vector is aligned with the target, even if that isn't the closest solution position-wise.

Reimplemented from PostureEngine.

Definition at line 151 of file PostureMC.h.

virtual bool PostureMC::solveLinkVector	(	const fmat::SubVector< 3, const float > &	Ptgt,
		unsigned int	link,
		const fmat::SubVector< 3, const float > &	Peff
	)			`[virtual]`

Performs inverse kinematics to solve for aligning the vector through Peff on link j and the link's origin to point at Ptgt (base coordinates in homogenous form); if solution found, stores result in this posture and returns true.

Parameters:

	Ptgt	the target point, in base coordinates
	link	the output offset of the joint to move
	Peff	the point (relative to link) which you desire to have moved to Ptgt (it's the desired "effector")

The difference between solveLinkPosition() and solveLinkVector() is typically small, but critical when you're trying to look at something -- the solution obtained by simplying trying to solve for the position may not align the vector with the target -- solveLinkVector() tries to ensure the vector is aligned with the target, even if that isn't the closest solution position-wise.

Reimplemented from PostureEngine.

Definition at line 148 of file PostureMC.h.

Referenced by solveLinkVector().

virtual void PostureMC::takeSnapshot ( const WorldState & st ) [virtual]

sets the values of cmds to the current state of the outputs as defined by state (doesn't change the weights)

Reimplemented from PostureEngine.

Definition at line 128 of file PostureMC.h.

virtual void PostureMC::takeSnapshot ( ) [virtual]

sets the values of cmds to the current state of the outputs (doesn't change the weights)

Reimplemented from PostureEngine.

Definition at line 127 of file PostureMC.h.

Referenced by EmergencyStopMC::EmergencyStopMC(), and takeSnapshot().

int PostureMC::updateOutputs ( ) [virtual]

returns non-zero (true) if PostureEngine::maxdiff() between this and the current position is over tolerance

This is handy so you can set to have the robot go to a position and then automatically remove the MotionCommand when it gets there - but beware fighting Postures which average out and neither succeeds

Implements MotionCommand.

Reimplemented in EmergencyStopMC.

Definition at line 46 of file PostureMC.cc.

Member Data Documentation

float PostureMC::curPositions[NumOutputs] [protected]

stores the last commanded value for each joint

Definition at line 164 of file PostureMC.h.

Referenced by EmergencyStopMC::freezeJoints(), getCurrentValue(), setDirty(), setOutputCmd(), updateOutputs(), and EmergencyStopMC::updateOutputs().

bool PostureMC::dirty [protected]

true if changes have been made since last updateOutputs()

Definition at line 158 of file PostureMC.h.

Referenced by EmergencyStopMC::freezeJoints(), isAlive(), isDirty(), EmergencyStopMC::releaseJoints(), setDirty(), setOutputCmd(), updateOutputs(), and EmergencyStopMC::updateOutputs().

bool PostureMC::hold [protected]

if set to true, the posture will be kept active; otherwise joints will be marked unused after each posture is achieved (as if the posture was pruned); set through setHold()

Definition at line 159 of file PostureMC.h.

Referenced by getHold(), isAlive(), setHold(), and updateOutputs().

float PostureMC::maxSpeeds[NumOutputs] [protected]

radians per frame. Initialized from Config::motion_config, but can be overridden by setMaxSpeed()

Definition at line 165 of file PostureMC.h.

Referenced by defaultMaxSpeed(), getMaxSpeed(), noMaxSpeed(), setMaxSpeed(), and updateOutputs().

bool PostureMC::targetReached [protected]

false if any joint is still moving towards its target

Definition at line 161 of file PostureMC.h.

Referenced by EmergencyStopMC::freezeJoints(), isAlive(), isDirty(), EmergencyStopMC::releaseJoints(), setDirty(), setOutputCmd(), updateOutputs(), and EmergencyStopMC::updateOutputs().

unsigned int PostureMC::targetTimestamp [protected]

time at which the targetReached flag was set

Definition at line 162 of file PostureMC.h.

Referenced by isAlive(), and updateOutputs().

unsigned int PostureMC::timeout [protected]

number of milliseconds to wait before giving up on a target that should have already been reached, a value of -1U will try forever

Definition at line 163 of file PostureMC.h.

Referenced by getTimeout(), isAlive(), and setTimeout().

float PostureMC::tolerance [protected]

when autopruning, if the maxdiff() of this posture and the robot's current position is below this value, isAlive() will be false, defaults to 0.01 (5.7 degree error)

Definition at line 160 of file PostureMC.h.

Referenced by getTolerance(), isAlive(), and setTolerance().

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

PostureMC Class Reference

Detailed Description

Public Member Functions

Protected Member Functions

Protected Attributes

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation