Tekkotsu: PostureEngine Class Reference

Handy for any class which wants to deal with setting joints and postures without writing a custom class

Definition at line 21 of file PostureEngine.h.


Public Member Functions
Constructors
	PostureEngine ()
	constructor
	PostureEngine (const std::string &filename)
	constructor, loads a position from a file
	PostureEngine (const WorldState *st)
	constructor, initializes joint positions to the current state of the outputs as defined by state
	PostureEngine (const PostureEngine &pe)
	copy constructor
PostureEngine &	operator= (const PostureEngine &pe)
	assignment operator
virtual	~PostureEngine ()
	destructor
Output Value Access/Control
You should be able to call the non-virtual functions without checking out, just a MotionManager::peekMotion(). Theoretically.
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
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)
OutputCmd &	operator() (unsigned int i)
	returns output i, returns a reference so you can also set through an assignment to this call, e.g. pose(MouthOffset)=.1; (remember that OutputCmd support implicit conversion from floats)
const OutputCmd &	operator() (unsigned int i) const
	returns output i
OutputCmd &	getOutputCmd (unsigned int i)
	returns output i, returns a reference so you can also set through an assignment
const OutputCmd &	getOutputCmd (unsigned int i) const
	returns output i
LoadSave
Uses LoadSave interface so you can load/save to files, uses a human-readable storage format
virtual void	setSaveFormat (bool condensed, WorldState *ws)
	sets saveFormatCondensed and loadSaveSensors (pass state for ws if you want to use current sensor values)
virtual void	setLoadedSensors (WorldState *ws)
	if ws is non-NULL, any sensor values in loaded postures will be stored there (otherwise they are ignored)
virtual WorldState *	getLoadedSensors () const
	returns value previously stored by setLoadSensors()
virtual unsigned int	getBinSize () const
	sets saveFormatCondensed and loadSaveSensors (pass state for ws if you want to use current sensor values)
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 unsigned int	saveBuffer (char buf[], unsigned int len) const
	sets saveFormatCondensed and loadSaveSensors (pass state for ws if you want to use current sensor values)
virtual unsigned int	loadFile (const char filename[])
	sets saveFormatCondensed and loadSaveSensors (pass state for ws if you want to use current sensor values)
virtual unsigned int	saveFile (const char filename[]) const
	sets saveFormatCondensed and loadSaveSensors (pass state for ws if you want to use current sensor values)
Kinematics
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.
Combining Postures
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	createOverlay (const PostureEngine &pe) const
	sets joints of this to all joints of pe which are not equal to unused (layers pe over this) returns new PostureEngine
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	createUnderlay (const PostureEngine &pe) const
	sets joints of this which are equal to unused to pe, (layers this over pe) returns new PostureEngine
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	createAverage (const PostureEngine &pe, float w=0.5) const
	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
virtual PostureEngine	createCombine (const PostureEngine &pe) const
	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
virtual float	diff (const PostureEngine &pe) const
	returns the sum squared error between this and pe's output values, but only between outputs which are both not unused
virtual float	avgdiff (const PostureEngine &pe) const
	returns the average sum squared error between this and pe's output values for outputs which are both not unused
virtual float	maxdiff (const PostureEngine &pe) const
	returns the max error between this and pe's output values for outputs which are both not unused
Protected Member Functions
virtual void	update (unsigned int c, unsigned int l)
	Called at the beginning of each function which accesses ROBOOP computations - should make sure the ROBOOP structures are up to date with Tekkotsu structures.
Protected Attributes
OutputCmd	cmds [NumOutputs]
	the table of outputs' values and weights, can be accessed through setOutputCmd() and getOutputCmd()
bool	saveFormatCondensed
	requests a condensed file format, smaller but less readable
WorldState *	loadSaveSensors
	If non-null, saves will include sensor readings from here, and loads will store any read sensors into here.

Constructor & Destructor Documentation

PostureEngine::PostureEngine ( ) [inline]

constructor

Definition at line 27 of file PostureEngine.h.

PostureEngine::PostureEngine ( const std::string & filename ) [inline]

constructor, loads a position from a file

Todo:: might want to make a library stored in memory of common positions so they don't have to be loaded repeatedly from memstick

Definition at line 30 of file PostureEngine.h.

PostureEngine::PostureEngine ( const WorldState * st ) [inline]

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

Definition at line 32 of file PostureEngine.h.

PostureEngine::PostureEngine ( const PostureEngine & pe ) [inline]

copy constructor

Definition at line 35 of file PostureEngine.h.

PostureEngine::~PostureEngine ( ) [virtual]

destructor

Definition at line 8 of file PostureEngine.cc.

Member Function Documentation

float PostureEngine::avgdiff ( const PostureEngine & pe ) const [virtual]

returns the average sum squared error between this and pe's output values for outputs which are both not unused

Todo:: create a version which does weighted summing? This treats weights as all or nothing

Definition at line 107 of file PostureEngine.cc.

void PostureEngine::clear ( ) [virtual]

sets all joints to unused

Reimplemented in PostureMC.

Definition at line 24 of file PostureEngine.cc.

Referenced by PostureMC::clear(), and loadBuffer().

PostureEngine PostureEngine::createAverage	(	const PostureEngine &	pe,
		float	w = `0.5`
	)			const `[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 weight<=0 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:: a new posture containing the results

Definition at line 81 of file PostureEngine.cc.

PostureEngine PostureEngine::createCombine ( const PostureEngine & pe ) const [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

Definition at line 92 of file PostureEngine.cc.

PostureEngine PostureEngine::createOverlay ( const PostureEngine & pe ) const [virtual]

sets joints of this to all joints of pe which are not equal to unused (layers pe over this) returns new PostureEngine

Definition at line 35 of file PostureEngine.cc.

PostureEngine PostureEngine::createUnderlay ( const PostureEngine & pe ) const [virtual]

sets joints of this which are equal to unused to pe, (layers this over pe) returns new PostureEngine

Definition at line 45 of file PostureEngine.cc.

float PostureEngine::diff ( const PostureEngine & pe ) const [virtual]

returns the sum squared error between this and pe's output values, but only between outputs which are both not unused

Todo:: create a version which does weighted summing? This treats weights as all or nothing

Definition at line 97 of file PostureEngine.cc.

unsigned int PostureEngine::getBinSize ( ) const [virtual]

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

Implements LoadSave.

Definition at line 135 of file PostureEngine.cc.

Referenced by CameraStreamBehavior::sendSensors().

virtual WorldState* PostureEngine::getLoadedSensors ( ) const [inline, virtual]

returns value previously stored by setLoadSensors()

Definition at line 79 of file PostureEngine.h.

const OutputCmd& PostureEngine::getOutputCmd ( unsigned int i ) const [inline]

returns output i

Definition at line 70 of file PostureEngine.h.

OutputCmd& PostureEngine::getOutputCmd ( unsigned int i ) [inline]

returns output i, returns a reference so you can also set through an assignment

Definition at line 69 of file PostureEngine.h.

Referenced by MotionSequenceEngine::setExplicitPose(), MotionSequenceEngine::setPose(), and update().

unsigned int PostureEngine::loadBuffer	(	const char	buf[],
		unsigned int	len
	)			`[virtual]`

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

Implements LoadSave.

Reimplemented in PostureMC.

Definition at line 201 of file PostureEngine.cc.

Referenced by PostureMC::loadBuffer(), and MotionSequenceEngine::loadBuffer().

unsigned int PostureEngine::loadFile ( const char filename[] ) [virtual]

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

Definition at line 609 of file PostureEngine.cc.

Referenced by MotionSequenceEngine::loadBuffer(), PostureEngine(), and PostureEditor::refresh().

float PostureEngine::maxdiff ( const PostureEngine & pe ) const [virtual]

returns the max error between this and pe's output values for outputs which are both not unused

Todo:: create a version which does weighted summing? This treats weights as all or nothing

Definition at line 119 of file PostureEngine.cc.

const OutputCmd& PostureEngine::operator() ( unsigned int i ) const [inline]

returns output i

Definition at line 68 of file PostureEngine.h.

OutputCmd& PostureEngine::operator() ( unsigned int i ) [inline]

returns output i, returns a reference so you can also set through an assignment to this call, e.g. pose(MouthOffset)=.1; (remember that OutputCmd support implicit conversion from floats)

Definition at line 67 of file PostureEngine.h.

PostureEngine& PostureEngine::operator= ( const PostureEngine & pe ) [inline]

assignment operator

Definition at line 43 of file PostureEngine.h.

unsigned int PostureEngine::saveBuffer	(	char	buf[],
		unsigned int	len
	)			const `[virtual]`

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

Implements LoadSave.

Definition at line 516 of file PostureEngine.cc.

Referenced by CameraStreamBehavior::sendSensors().

unsigned int PostureEngine::saveFile ( const char filename[] ) const [virtual]

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

Definition at line 612 of file PostureEngine.cc.

Referenced by Kinematics::calculateGroundPlane(), PostureEditor::refresh(), and SavePostureControl::takeInput().

PostureEngine & PostureEngine::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 in PostureMC.

Definition at line 57 of file PostureEngine.cc.

Referenced by createAverage(), and PostureMC::setAverage().

PostureEngine & PostureEngine::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 in PostureMC.

Definition at line 85 of file PostureEngine.cc.

Referenced by createCombine(), and PostureMC::setCombine().

virtual void PostureEngine::setLoadedSensors ( WorldState * ws ) [inline, virtual]

if ws is non-NULL, any sensor values in loaded postures will be stored there (otherwise they are ignored)

Definition at line 78 of file PostureEngine.h.

PostureEngine& PostureEngine::setOutputCmd	(	unsigned int	i,
		const OutputCmd &	c
	)			`[inline]`

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 in PostureMC.

Definition at line 66 of file PostureEngine.h.

Referenced by MotionSequenceEngine::getPose(), PostureMC::setOutputCmd(), and solveLinkPosition().

PostureEngine & PostureEngine::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 in PostureMC.

Definition at line 29 of file PostureEngine.cc.

Referenced by createOverlay(), and PostureMC::setOverlay().

void PostureEngine::setSaveFormat	(	bool	condensed,
		WorldState *	ws
	)			`[virtual]`

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

Definition at line 130 of file PostureEngine.cc.

Referenced by Kinematics::calculateGroundPlane(), and CameraStreamBehavior::sendSensors().

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

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

Reimplemented in PostureMC.

Definition at line 39 of file PostureEngine.cc.

Referenced by createUnderlay(), and PostureMC::setUnderlay().

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

the the weights of a range of cmds

Reimplemented in PostureMC.

Definition at line 19 of file PostureEngine.cc.

virtual void PostureEngine::setWeights ( float w ) [inline, virtual]

set the weights of all cmds

Reimplemented in PostureMC.

Definition at line 63 of file PostureEngine.h.

Referenced by Kinematics::calculateGroundPlane(), CameraStreamBehavior::sendSensors(), PostureMC::setWeights(), and SavePostureControl::takeInput().

virtual bool PostureEngine::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 in PostureMC.

Definition at line 119 of file PostureEngine.h.

bool PostureEngine::solveLinkPosition	(	const NEWMAT::ColumnVector &	Ptgt,
		unsigned int	link,
		const NEWMAT::ColumnVector &	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 the desired "effector")

Reimplemented in PostureMC.

Definition at line 649 of file PostureEngine.cc.

Referenced by PostureMC::solveLinkPosition(), solveLinkPosition(), and solveLinkVector().

virtual bool PostureEngine::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 in PostureMC.

Definition at line 152 of file PostureEngine.h.

bool PostureEngine::solveLinkVector	(	const NEWMAT::ColumnVector &	Ptgt,
		unsigned int	link,
		const NEWMAT::ColumnVector &	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")

Reimplemented in PostureMC.

Definition at line 663 of file PostureEngine.cc.

Referenced by PostureMC::solveLinkVector(), and solveLinkVector().

void PostureEngine::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 in PostureMC.

Definition at line 14 of file PostureEngine.cc.

void PostureEngine::takeSnapshot ( ) [virtual]

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

Reimplemented in PostureMC.

Definition at line 10 of file PostureEngine.cc.

Referenced by Kinematics::calculateGroundPlane(), PostureEngine(), CameraStreamBehavior::sendSensors(), SavePostureControl::takeInput(), and PostureMC::takeSnapshot().

void PostureEngine::update	(	unsigned int	c,
		unsigned int	l
	)			`[protected, virtual]`

Called at the beginning of each function which accesses ROBOOP computations - should make sure the ROBOOP structures are up to date with Tekkotsu structures.

This class will pull current values from WorldState, but it is expected that subclasses (i.e. PostureEngine) will want to provide their own joint values. Updates from link 1 through link l.

Parameters: