Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
HeadPointerMC.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_HeadPointerMC_h 00003 #define INCLUDED_HeadPointerMC_h 00004 00005 #include "MotionCommand.h" 00006 #include "OutputCmd.h" 00007 #include "Shared/RobotInfo.h" 00008 #include "roboop/robot.h" 00009 00010 //! This class gives some quick and easy functions to point the head at things 00011 class HeadPointerMC : public MotionCommand { 00012 public: 00013 //! Constructor, defaults to all joints to current value in ::state 00014 HeadPointerMC(); 00015 00016 //! Destructor 00017 virtual ~HeadPointerMC() {} 00018 00019 //!@name Speed Control 00020 00021 //! Sets #maxSpeed to 0 (no maximum) 00022 void noMaxSpeed() { for(unsigned int i=0; i<NumHeadJoints; i++) maxSpeed[i]=0; } 00023 00024 //! Restores #maxSpeed to default settings from Config::Motion_Config 00025 void defaultMaxSpeed(); 00026 00027 //! Sets #maxSpeed in rad/sec 00028 /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) 00029 * @param x maximum radians per second to move */ 00030 void setMaxSpeed(unsigned int i, float x) { maxSpeed[i]=x*FrameTime/1000; } 00031 00032 //! Returns #maxSpeed in rad/sec 00033 /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) 00034 * @return the maximum speed of joint @a i in radians per second */ 00035 float getMaxSpeed(unsigned int i) { return maxSpeed[i]*1000/FrameTime; } 00036 00037 //@} 00038 00039 //!@name Joint Accessors 00040 00041 //! Sets the weight values for all the neck joints 00042 void setWeight(float w); 00043 00044 //! Directly sets the neck values (all values in radians) 00045 /*! @param j1 value for first neck joint (tilt on all ERS models) 00046 * @param j2 value for second neck joint (pan on all ERS models) 00047 * @param j3 value for third neck joint (nod on ERS-7, roll on ERS-2xx) */ 00048 void setJoints(float j1, float j2, float j3); 00049 00050 //! Directly set a single neck joint value 00051 /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) 00052 * @param value the value to be assigned to join @a i, in radians */ 00053 void setJointValue(unsigned int i, float value) { 00054 if(!ensureValidJoint(i)) 00055 return; 00056 headTargets[i]=clipAngularRange(HeadOffset+i,value); 00057 markDirty(); 00058 } 00059 00060 //! Returns the target value of joint @a i. Use this if you want to know the current @b commanded joint value; To get the current joint @b position, look in WorldState::outputs 00061 /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) */ 00062 float getJointValue(unsigned int i) const { 00063 if(ensureValidJoint(i)) 00064 return headTargets[i]; 00065 else 00066 return 0; 00067 } 00068 00069 //! Centers the camera on a point in space, attempting to keep the camera as far away from the point as possible 00070 /*! Point should be relative to the body reference frame (see ::BaseFrameOffset) 00071 * @param x location in millimeters 00072 * @param y location in millimeters 00073 * @param z location in millimeters 00074 * 00075 * @todo this method is an approximation, could be more precise, and perhaps faster, although this is pretty good. */ 00076 bool lookAtPoint(float x, float y, float z); 00077 00078 //! Centers the camera on a point in space, attempting to move the camera @a d millimeters away from the point 00079 /*! Point should be relative to the body reference frame (see ::BaseFrameOffset) 00080 * @param x location in millimeters 00081 * @param y location in millimeters 00082 * @param z location in millimeters 00083 * @param d target distance from point in millimeters */ 00084 bool lookAtPoint(float x, float y, float z, float d); 00085 00086 //! Points the camera in a given direction 00087 /*! Vector should be relative to the body reference frame (see ::BaseFrameOffset) 00088 * @param x component of the direction vector 00089 * @param y component of the direction vector 00090 * @param z component of the direction vector */ 00091 bool lookInDirection(float x, float y, float z); 00092 00093 //@} 00094 00095 00096 //!@name Inherited: 00097 virtual int updateOutputs(); //!< Updates where the head is looking 00098 virtual int isDirty() { return (dirty || !targetReached)?3:0; } //!< true if a change has been made since the last updateJointCmds() and we're active 00099 virtual int isAlive() { return true; } //!< always true, doesn't autoprune (yet) 00100 virtual void DoStart() { MotionCommand::DoStart(); markDirty(); } //!< marks this as dirty each time it is added 00101 //@} 00102 00103 protected: 00104 //! puts x in the range (-pi,pi) 00105 static float normalizeAngle(float x) { return x-rint(x/(2*M_PI))*(2*M_PI); } 00106 00107 //! if @a x is outside of the range of joint @a i, it is set to either the min or the max, whichever is closer 00108 static float clipAngularRange(unsigned int i, float x) { 00109 float min=outputRanges[i][MinRange]; 00110 float max=outputRanges[i][MaxRange]; 00111 if(x<min || x>max) { 00112 float mn_dist=fabs(normalizeAngle(min-x)); 00113 float mx_dist=fabs(normalizeAngle(max-x)); 00114 if(mn_dist<mx_dist) 00115 return min; 00116 else 00117 return max; 00118 } else 00119 return x; 00120 } 00121 //! if targetReached, reassigns headCmds from ::state, then sets dirty to true and targetReached to false 00122 /*! should be called each time a joint value gets modified in case 00123 * the head isn't where it's supposed to be, it won't jerk around */ 00124 void markDirty(); 00125 00126 //! Makes sure @a i is in the range (0,NumHeadJoints). If it is instead in the range (HeadOffset,HeadOffset+NumHeadJoints), output a warning and reset @a i to the obviously intended value. 00127 /*! @param[in] i joint offset relative to either HeadOffset (i.e. one of TPROffset_t) or 0 00128 * @param[out] i joint offset relative to HeadOffset (i.e. one of TPROffset_t) 00129 * @return true if the intended joint could be ascertained, false otherwise */ 00130 static bool ensureValidJoint(unsigned int& i); 00131 00132 bool dirty; //!< true if a change has been made since last call to updateJointCmds() 00133 bool targetReached; //!< false if the head is still moving towards its target 00134 float headTargets[NumHeadJoints]; //!< stores the target value of each joint 00135 OutputCmd headCmds[NumHeadJoints]; //!< stores the last values we sent from updateOutputs 00136 float maxSpeed[NumHeadJoints]; //!< initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame 00137 ROBOOP::Robot headkin; //!< provides kinematics computations 00138 }; 00139 00140 /*! @file 00141 * @brief Describes HeadPointerMC, a class for various ways to control where the head is looking 00142 * @author ejt (Creator) 00143 * 00144 * $Author: ejt $ 00145 * $Name: tekkotsu-2_2_2 $ 00146 * $Revision: 1.18 $ 00147 * $State: Exp $ 00148 * $Date: 2004/12/23 00:59:03 $ 00149 */ 00150 00151 #endif 00152 |
Tekkotsu v2.2.2 |
Generated Tue Jan 4 15:43:14 2005 by Doxygen 1.4.0 |