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) { headTargets[i]=clipAngularRange(HeadOffset+i,value); markDirty(); } 00054 00055 //! 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 00056 /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) */ 00057 float getJointValue(unsigned int i) const { return headTargets[i]; } 00058 00059 //! Centers the camera on a point in space, attempting to keep the camera as far away from the point as possible 00060 /*! @param x location in millimeters 00061 * @param y location in millimeters 00062 * @param z location in millimeters */ 00063 bool lookAtPoint(float x, float y, float z); 00064 00065 //! Centers the camera on a point in space, attempting to move the camera @a d millimeters away from the point 00066 /*! @param x location in millimeters 00067 * @param y location in millimeters 00068 * @param z location in millimeters 00069 * @param d target distance from point in millimeters */ 00070 bool lookAtPoint(float x, float y, float z, float d); 00071 00072 //! Points the camera in a given direction 00073 /*! @param x component of the direction vector 00074 * @param y component of the direction vector 00075 * @param z component of the direction vector */ 00076 bool lookInDirection(float x, float y, float z); 00077 00078 //@} 00079 00080 00081 //!@name Inherited: 00082 virtual int updateOutputs(); //!< Updates where the head is looking 00083 virtual int isDirty() { return (dirty || !targetReached)?3:0; } //!< true if a change has been made since the last updateJointCmds() and we're active 00084 virtual int isAlive() { return true; } //!< always true, doesn't autoprune (yet) 00085 virtual void DoStart() { MotionCommand::DoStart(); markDirty(); } //!< marks this as dirty each time it is added 00086 //@} 00087 00088 protected: 00089 //! puts x in the range (-pi,pi) 00090 static float normalizeAngle(float x) { return x-rint(x/(2*M_PI))*(2*M_PI); } 00091 00092 //! 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 00093 static float clipAngularRange(unsigned int i, float x) { 00094 float min=outputRanges[i][MinRange]; 00095 float max=outputRanges[i][MaxRange]; 00096 if(x<min || x>max) { 00097 float mn_dist=fabs(normalizeAngle(min-x)); 00098 float mx_dist=fabs(normalizeAngle(max-x)); 00099 if(mn_dist<mx_dist) 00100 return min; 00101 else 00102 return max; 00103 } else 00104 return x; 00105 } 00106 //! if targetReached, reassigns headCmds from ::state, then sets dirty to true and targetReached to false 00107 /*! should be called each time a joint value gets modified in case 00108 * the head isn't where it's supposed to be, it won't jerk around */ 00109 void markDirty(); 00110 00111 bool dirty; //!< true if a change has been made since last call to updateJointCmds() 00112 bool targetReached; //!< false if the head is still moving towards its target 00113 float headTargets[NumHeadJoints]; //!< stores the target value of each joint 00114 OutputCmd headCmds[NumHeadJoints]; //!< stores the last values we sent from updateOutputs 00115 float maxSpeed[NumHeadJoints]; //!< initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame 00116 ROBOOP::Robot headkin; //!< provides kinematics computations 00117 }; 00118 00119 /*! @file 00120 * @brief Describes HeadPointerMC, a class for various ways to control where the head is looking 00121 * @author ejt (Creator) 00122 * 00123 * $Author: ejt $ 00124 * $Name: tekkotsu-2_2 $ 00125 * $Revision: 1.14 $ 00126 * $State: Exp $ 00127 * $Date: 2004/10/14 23:02:53 $ 00128 */ 00129 00130 #endif 00131 |
Tekkotsu v2.2 |
Generated Tue Oct 19 14:19:14 2004 by Doxygen 1.3.9.1 |