|
Public Types |
| | BodyRelative |
| | holds neck at a specified position, like a PostureEngine, but neck specific
|
| enum | CoordFrame_t { BodyRelative
} |
| | Various modes the head can be in. In the future may want to add ability to explicitly track an object or point in the world model. More...
|
Public Member Functions |
|
| OldHeadPointerMC () |
| | constructor, defaults to active, BodyRelative, all joints at 0
|
|
virtual | ~OldHeadPointerMC () |
| | destructor
|
|
void | setWeight (double w) |
| | sets the weight values for all the neck joints
|
|
void | setWeight (RobotInfo::TPROffset_t i, double weight) |
| | set a specific head joint weight, pass one of RobotInfo::TPROffset_t, not a full offset!
|
|
void | setActive (bool a) |
| | sets active flag; see isDirty()
|
|
bool | getActive () const |
| | returns active flag, see isDirty()
|
|
void | noMaxSpeed () |
| | sets maxSpeed to 0 (no maximum)
|
|
void | defaultMaxSpeed () |
| | restores maxSpeed to default settings from Config::Motion_Config
|
|
void | setMaxSpeed (TPROffset_t i, float x) |
| | sets maxSpeed in rad/sec
|
|
float | getMaxSpeed (TPROffset_t i) |
| | returns maxSpeed in rad/sec
|
|
double | convert (RobotInfo::TPROffset_t i, double v, CoordFrame_t srcmode, CoordFrame_t tgtmode) const |
| | converts a value v in srcmode to a value in tgtmode that would leave the joint angle for joint i constant (you probably won't need to call this directly)
|
|
Note that none of these are virtual, so you don't have to checkout to use them, you should be able to use MotionManager::peekMotion() |
|
void | setJoints (double tilt, double pan, double roll) |
| | Directly sets the neck values (radians), uses current mode.
|
|
void | setMode (CoordFrame_t m, bool convert=true) |
| | sets all the joints to the given mode, will convert the values to the new mode if convert is true
|
|
void | setJointMode (RobotInfo::TPROffset_t i, CoordFrame_t m, bool convert=true) |
| | sets a specific head joint's mode; will convert from previous mode's value to next mode's value if convert is true. Pass one of RobotInfo::TPROffset_t, not a full offset!
|
|
void | setJointValue (RobotInfo::TPROffset_t i, double value) |
| | set a specific head joint's value (in radians, for whatever mode it's in), pass one of RobotInfo::TPROffset_t, not a full offset!
|
|
void | setJointValueAndMode (RobotInfo::TPROffset_t i, double value, CoordFrame_t m) |
| | set a specific head joint (in radians), pass one of RobotInfo::TPROffset_t, not a full offset!
|
|
void | setJointValueFromMode (RobotInfo::TPROffset_t i, double value, CoordFrame_t m) |
| | set a specific head joint (in radians) auto converting value from mode m to the current mode, pass one of RobotInfo::TPROffset_t, not a full offset!
|
|
CoordFrame_t | getJointMode (RobotInfo::TPROffset_t i) const |
| | returns the current mode for joint i (use RobotInfo::TPROffset_t, not global offset)
|
|
double | getJointValue (RobotInfo::TPROffset_t i) const |
| | returns the target value (relative to the current mode) of joint i. Use getOutputCmd() if you want to know the current commanded joint value; To get the current joint position, look in WorldState
|
|
|
virtual int | updateOutputs () |
| | Updates where the head is looking.
|
|
virtual const OutputCmd & | getOutputCmd (unsigned int i) |
| | returns one of the headCmds entries or unusedJoint if not a head joint
|
|
virtual int | isDirty () |
| | true if a change has been made since the last updateJointCmds() and we're active
|
|
virtual int | isAlive () |
| | Updates where the head is looking.
|
|
virtual void | DoStart () |
| | Updates where the head is looking.
|
Protected Member Functions |
| double | convToBodyRelative (TPROffset_t i, double v, CoordFrame_t mode) const |
| | converts to a body relative measurement for joint i
|
| double | convFromBodyRelative (TPROffset_t i, double v, CoordFrame_t mode) const |
| | converts from a body relative measurement for joint i
|
Static Protected Member Functions |
| static float | normalizeAngle (float x) |
|
static float | clipAngularRange (unsigned int i, float x) |
| | if x is outside of the range of joint i, it is set to either the min or the max, whichever is closer
|
Protected Attributes |
|
bool | dirty |
| | true if a change has been made since last call to updateJointCmds()
|
|
bool | active |
| | set by accessor functions, defaults to true
|
|
bool | targetReached |
| | false if the head is still moving towards its target
|
|
OutputCmd | headCmds [NumHeadJoints] |
| | stores the last values we sent from updateOutputs
|
|
float | headTargets [NumHeadJoints] |
| | stores the target value of each joint, relative to headModes
|
|
CoordFrame_t | headModes [NumHeadJoints] |
| | stores the current mode of each joint, for instance so tilt can be GravityRelative while pan is static
|
|
float | maxSpeed [NumHeadJoints] |
| | initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame
|