PostureMC Class Reference#include <PostureMC.h>
Inheritance diagram for PostureMC:
[legend]List of all members.
Detailed Description
a MotionCommand shell for PostureEngine
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 27 of file PostureMC.h.
|
Public Member Functions |
|
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
|
|
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.
|
|
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
|
|
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) |
| Load from a saved buffer.
|
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 to Ptgt (expects 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 to Ptgt; 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 positioning Peff on link j to point at Ptgt (expects 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 positioning Peff on link j to point at Ptgt; 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
|
Constructor & Destructor Documentation
PostureMC::PostureMC |
( |
|
) |
[inline] |
|
PostureMC::PostureMC |
( |
const std::string & |
filename |
) |
[inline] |
|
|
constructor, loads from filename
Definition at line 39 of file PostureMC.h. |
PostureMC::PostureMC |
( |
const WorldState * |
st |
) |
[inline] |
|
|
constructor, initializes joint positions to the current state of the outputs as defined by state
Definition at line 45 of file PostureMC.h. |
virtual PostureMC::~PostureMC |
( |
|
) |
[inline, virtual] |
|
Member Function Documentation
virtual void PostureMC::clear |
( |
|
) |
[inline, virtual] |
|
void PostureMC::defaultMaxSpeed |
( |
float |
x = 1 |
) |
|
|
virtual void PostureMC::DoStart |
( |
|
) |
[inline, virtual] |
|
virtual bool PostureMC::getHold |
( |
|
) |
[inline, virtual] |
|
float PostureMC::getMaxSpeed |
( |
unsigned int |
i |
) |
[inline] |
|
|
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 102 of file PostureMC.h. |
virtual unsigned int PostureMC::getTimeout |
( |
|
) |
[inline, virtual] |
|
virtual float PostureMC::getTolerance |
( |
|
) |
[inline, virtual] |
|
void PostureMC::init |
( |
|
) |
[protected] |
|
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 60 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 110 of file PostureMC.h.
Referenced by updateOutputs(). |
virtual unsigned int PostureMC::LoadBuffer |
( |
const char |
buf[], |
|
|
unsigned int |
len |
|
) |
[inline, virtual] |
|
|
Load from a saved buffer.
- Parameters:
-
| buf | pointer to the memory where you should begin loading |
| len | length of buf available (this isn't all yours, might be more stuff saved after yours) |
- Returns:
- the number of bytes actually used
Reimplemented from PostureEngine.
Definition at line 132 of file PostureMC.h. |
void PostureMC::noMaxSpeed |
( |
|
) |
[inline] |
|
|
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 129 of file PostureMC.h. |
|
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 130 of file PostureMC.h. |
|
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 6 of file PostureMC.cc.
Referenced by clear(), init(), LoadBuffer(), setAverage(), setCombine(), setOutputCmd(), setOverlay(), setUnderlay(), setWeights(), solveLinkPosition(), solveLinkVector(), and takeSnapshot(). |
virtual PostureMC& PostureMC::setHold |
( |
bool |
h = true |
) |
[inline, 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 77 of file PostureMC.h. |
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 97 of file PostureMC.h. |
|
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 127 of file PostureMC.h. |
virtual PostureMC& PostureMC::setTimeout |
( |
unsigned int |
delay |
) |
[inline, virtual] |
|
virtual PostureMC& PostureMC::setTolerance |
( |
float |
t |
) |
[inline, 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 128 of file PostureMC.h. |
virtual void PostureMC::setWeights |
( |
float |
w, |
|
|
unsigned int |
lowjoint, |
|
|
unsigned int |
highjoint |
|
) |
[inline, virtual] |
|
virtual void PostureMC::setWeights |
( |
float |
w |
) |
[inline, virtual] |
|
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 to Ptgt; 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") |
Reimplemented from PostureEngine.
Definition at line 134 of file PostureMC.h. |
|
Performs inverse kinematics to solve for positioning Peff on link j to Ptgt (expects 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 133 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 positioning Peff on link j to point at Ptgt; 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 136 of file PostureMC.h. |
|
Performs inverse kinematics to solve for positioning Peff on link j to point at Ptgt (expects 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 135 of file PostureMC.h. |
virtual void PostureMC::takeSnapshot |
( |
const WorldState * |
st |
) |
[inline, 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 123 of file PostureMC.h. |
virtual void PostureMC::takeSnapshot |
( |
|
) |
[inline, virtual] |
|
int PostureMC::updateOutputs |
( |
|
) |
[virtual] |
|
Member Data Documentation
The documentation for this class was generated from the following files:
|