Tekkotsu: PostureMC Class Reference

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.

Definition at line 28 of file PostureMC.h.


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)
	Restores maxSpeeds to default MaxOutputSpeed values from the robot info file (e.g. ERS7Info.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.
MotionCommand Stuff
virtual int	updateOutputs ()
	is called once per update cycle, can do any processing you need to change your priorities or set output commands on the MotionManager
virtual int	isDirty ()
	not used by MotionManager at the moment, but could be used to reduce recomputation, and you may find it useful
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)
	sets saveFormatCondensed and loadSaveSensors (pass state for ws if you want to use current sensor values)
virtual bool	solveLinkPosition (const NEWMAT::ColumnVector &Ptgt, unsigned int link, const NEWMAT::ColumnVector &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 NEWMAT::ColumnVector &Ptgt, unsigned int link, const NEWMAT::ColumnVector &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]
	initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame

Member Function Documentation

void PostureMC::defaultMaxSpeed ( float x = 1 )

Restores maxSpeeds to default MaxOutputSpeed values from the robot info file (e.g. ERS7Info.h).

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

Parameters:

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

Definition at line 13 of file PostureMC.cc.

Referenced by init().

float PostureMC::getMaxSpeed ( unsigned int i ) [inline]

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

Parameters:

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.

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 59 of file PostureMC.cc.

virtual int PostureMC::isDirty ( ) [inline, virtual]

not used by MotionManager at the moment, but could be used to reduce recomputation, and you may find it useful

Returns:: zero if none of the commands have changed since last getJointCmd(), else non-zero

Implements MotionCommand.

Definition at line 111 of file PostureMC.h.

Referenced by updateOutputs().

virtual PostureEngine& PostureMC::setAverage	(	const PostureEngine &	pe,
		float	w = `0.5`
	)			`[inline, 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 130 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 5 of file PostureMC.cc.

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

void PostureMC::setMaxSpeed	(	unsigned int	i,
		float	x
	)			`[inline]`

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.

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
	)			`[inline, 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 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")

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 135 of file PostureMC.h.

virtual bool PostureMC::solveLinkPosition	(	const NEWMAT::ColumnVector &	Ptgt,
		unsigned int	link,
		const NEWMAT::ColumnVector &	Peff
	)			`[inline, 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 the desired "effector")

Reimplemented from PostureEngine.

Definition at line 134 of file PostureMC.h.

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
	)			`[inline, 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.

Reimplemented from PostureEngine.

Definition at line 137 of file PostureMC.h.

virtual bool PostureMC::solveLinkVector	(	const NEWMAT::ColumnVector &	Ptgt,
		unsigned int	link,
		const NEWMAT::ColumnVector &	Peff
	)			`[inline, 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")

Reimplemented from PostureEngine.

Definition at line 136 of file PostureMC.h.

int PostureMC::updateOutputs ( ) [virtual]

is called once per update cycle, can do any processing you need to change your priorities or set output commands on the MotionManager

Returns:: zero if no changes were made, non-zero otherwise

See also:: RobotInfo::NumFrames
RobotInfo::FrameTime

Implements MotionCommand.

Reimplemented in EmergencyStopMC.

Definition at line 22 of file PostureMC.cc.

PostureMC Class Reference

Detailed Description

Public Member Functions

Protected Member Functions

Protected Attributes

Member Function Documentation