Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | 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 "Shared/RobotInfo.h" 00006 #include "MotionCommand.h" 00007 #include "OutputCmd.h" 00008 #include "Shared/mathutils.h" 00009 #include "roboop/robot.h" 00010 00011 //! This class gives some quick and easy functions to point the head at things 00012 class HeadPointerMC : public MotionCommand { 00013 public: 00014 //! Constructor, defaults to all joints to current value in ::state (i.e. calls takeSnapshot() automatically) 00015 HeadPointerMC(); 00016 00017 //! Destructor 00018 virtual ~HeadPointerMC() {} 00019 00020 //!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 00021 virtual void setHold(bool h=true) { hold=h; } 00022 virtual bool getHold() { return hold; } //!< return #hold 00023 00024 virtual void setTolerance(float t) { tolerance=t; } //!< sets #tolerance 00025 virtual float getTolerance() { return tolerance; } //!< returns #tolerance 00026 virtual void setTimeout(unsigned int delay) { timeout=delay; } //!< sets #timeout 00027 virtual unsigned int getTimeout() { return timeout; } //!< returns #timeout 00028 00029 //! sets the target to last sent commands, and dirty to false; essentially freezes motion in place 00030 /*! This is very similar to takeSnapshot(), but will do the "right thing" (retain current position) when motion blending is involved. 00031 * A status event will be generated if/when the joints reach the currently commanded position. 00032 * Probably should use freezeMotion() if you want to stop a motion underway, but takeSnapshot() if you want to reset/intialize to the current joint positions. */ 00033 virtual void freezeMotion(); 00034 //! sets the target joint positions to current sensor values 00035 /*! Similar to freezeMotion() when a motion is underway, but only if no other MotionCommands are using neck joints. 00036 * A status event will @e not be generated unless a motion was already underway. 00037 * Probably should use freezeMotion() if you want to stop a motion underway, but takeSnapshot() if you want to reset/intialize to the current joint positions. */ 00038 virtual void takeSnapshot(); 00039 00040 //!@name Speed Control 00041 00042 //! Sets #maxSpeed to 0 (no maximum) 00043 void noMaxSpeed() { for(unsigned int i=0; i<NumHeadJoints; i++) maxSpeed[i]=0; } 00044 00045 //! Restores #maxSpeed to default settings from Config::Motion_Config 00046 /*! @param x ratio of the max speed to use; so 0.5 would limit motion to half the recommended upper limit */ 00047 void defaultMaxSpeed(float x=1); 00048 00049 //! Sets #maxSpeed in rad/sec 00050 /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) 00051 * @param x maximum radians per second to move */ 00052 void setMaxSpeed(unsigned int i, float x) { maxSpeed[i]=x*FrameTime/1000; } 00053 00054 //! Returns #maxSpeed in rad/sec 00055 /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) 00056 * @return the maximum speed of joint @a i in radians per second */ 00057 float getMaxSpeed(unsigned int i) { return maxSpeed[i]*1000/FrameTime; } 00058 00059 //@} 00060 00061 //!@name Joint Accessors 00062 00063 //! Sets the weight values for all the neck joints 00064 void setWeight(float w); 00065 00066 //! Request a set of neck joint values 00067 /*! Originally this corresponded directly to the neck joints of the aibo, however 00068 * on other platforms it will use capabilties mapping to try to set correspnding joints if available. 00069 * (we're not doing kinematics here, trying to set joint values directly. If your 00070 * want a more generic/abstract interface, use lookAtPoint()/lookInDirection(). 00071 * 00072 * Note that for a "pan-tilt" camera, you actually want to set the @em last two parameters, 00073 * @em not the first two! 00074 * 00075 * @param tilt value - this is an initial rotation about the camera x axis 00076 * @param pan value - this is a rotation about the camera y axis 00077 * @param tilt2 value - a second rotation about the camera x axis ("nod") 00078 * 00079 * On ERS-210 and 220, the tilt2 is actually a roll about camera z (ugly, but no one's using those anymore, so moot issue) */ 00080 void setJoints(float tilt, float pan, float tilt2); 00081 00082 //! Directly set a single neck joint value 00083 /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) 00084 * @param value the value to be assigned to join @a i, in radians */ 00085 void setJointValue(unsigned int i, float value) { 00086 #ifdef TGT_HAS_HEAD 00087 if(!ensureValidJoint(i)) 00088 return; 00089 headTargets[i]=clipAngularRange(HeadOffset+i,value); 00090 markDirty(); 00091 #endif 00092 } 00093 00094 //! 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 00095 /*! @param i joint offset relative to HeadOffset (i.e. one of TPROffset_t) */ 00096 float getJointValue(unsigned int i) const { 00097 if(ensureValidJoint(i)) 00098 return headTargets[i]; 00099 else 00100 return 0; 00101 } 00102 00103 //! Centers the camera on a point in space, attempting to keep the camera as far away from the point as possible 00104 /*! Point should be relative to the body reference frame (see ::BaseFrameOffset). Returns true if the target is reachable. 00105 * @param x location in millimeters 00106 * @param y location in millimeters 00107 * @param z location in millimeters 00108 * 00109 * @todo this method is an approximation, could be more precise, and perhaps faster, although this is pretty good. */ 00110 bool lookAtPoint(float x, float y, float z); 00111 00112 //! Centers the camera on a point in space, attempting to move the camera @a d millimeters away from the point 00113 /*! Point should be relative to the body reference frame (see ::BaseFrameOffset). Returns true if the target is reachable. 00114 * @param x location in millimeters 00115 * @param y location in millimeters 00116 * @param z location in millimeters 00117 * @param d target distance from point in millimeters */ 00118 bool lookAtPoint(float x, float y, float z, float d); 00119 00120 //! Points the camera in a given direction 00121 /*! Vector should be relative to the body reference frame (see ::BaseFrameOffset). Returns true if the target is reachable. 00122 * @param x component of the direction vector 00123 * @param y component of the direction vector 00124 * @param z component of the direction vector */ 00125 bool lookInDirection(float x, float y, float z); 00126 00127 00128 00129 //@} 00130 00131 public: 00132 //!@name Inherited: 00133 virtual int updateOutputs(); //!< Updates where the head is looking 00134 virtual int isDirty() { return (dirty || !targetReached)?3:0; } //!< true if a change has been made since the last updateJointCmds() and we're active 00135 virtual int isAlive(); //!< Alive while target is not reached 00136 virtual void DoStart() { MotionCommand::DoStart(); markDirty(); } //!< marks this as dirty each time it is added 00137 //@} 00138 00139 protected: 00140 //! checks if target point or direction is actually reachable 00141 bool isReachable(const NEWMAT::ColumnVector& Pobj) { 00142 NEWMAT::ColumnVector poE=headkin.convertLink(0,headkin.get_dof())*Pobj; 00143 const float theta = mathutils::rad2deg(acos(poE(3)/sqrt(poE(1)*poE(1)+poE(2)*poE(2)+poE(3)*poE(3)))); 00144 // cout << "theta: " << theta << " degrees\n"; 00145 return theta < 5.0; 00146 } 00147 00148 //! puts x in the range (-pi,pi) 00149 static float normalizeAngle(float x) { return x-rint(x/(2*M_PI))*(2*M_PI); } 00150 00151 //! 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 00152 static float clipAngularRange(unsigned int i, float x) { 00153 float min=outputRanges[i][MinRange]; 00154 float max=outputRanges[i][MaxRange]; 00155 if(x<min || x>max) { 00156 float mn_dist=fabs(normalizeAngle(min-x)); 00157 float mx_dist=fabs(normalizeAngle(max-x)); 00158 if(mn_dist<mx_dist) 00159 return min; 00160 else 00161 return max; 00162 } else 00163 return x; 00164 } 00165 //! if targetReached, reassigns headCmds from MotionManager::getOutputCmd(), then sets dirty to true and targetReached to false 00166 /*! should be called each time a joint value gets modified in case 00167 * the head isn't where it's supposed to be, it won't jerk around 00168 * 00169 * MotionManager::getOutputCmd() is called instead of 00170 * WorldState::outputs[] because if this is being called rapidly 00171 * (i.e. after every sensor reading) using the sensor values will 00172 * cause problems with very slow acceleration due to sensor lag 00173 * continually resetting the current position. Using the last 00174 * value sent by the MotionManager fixes this.*/ 00175 void markDirty(); 00176 00177 //! 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. 00178 /*! @param[in] i joint offset relative to either HeadOffset (i.e. one of TPROffset_t) or 0 00179 * @param[out] i joint offset relative to HeadOffset (i.e. one of TPROffset_t) 00180 * @return true if the intended joint could be ascertained, false otherwise */ 00181 static bool ensureValidJoint(unsigned int& i); 00182 00183 bool dirty; //!< true if a change has been made since last call to updateJointCmds() 00184 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() 00185 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.05 radian (2.86 degree error) 00186 bool targetReached; //!< false if the head is still moving towards its target 00187 unsigned int targetTimestamp; //!< time at which the targetReached flag was set 00188 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 00189 float headTargets[NumHeadJoints]; //!< stores the target value of each joint 00190 OutputCmd headCmds[NumHeadJoints]; //!< stores the last values we sent from updateOutputs 00191 float maxSpeed[NumHeadJoints]; //!< initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame 00192 ROBOOP::Robot headkin; //!< provides kinematics computations, there's a small leak and safety issue here because ROBOOP::Robot contains pointers, and those pointers typically aren't freed because MotionCommand destructor isn't called when detaching shared region 00193 }; 00194 00195 /*! @file 00196 * @brief Describes HeadPointerMC, a class for various ways to control where the head is looking 00197 * @author ejt (Creator) 00198 * 00199 * $Author: ejt $ 00200 * $Name: tekkotsu-4_0 $ 00201 * $Revision: 1.30 $ 00202 * $State: Exp $ 00203 * $Date: 2007/11/19 21:45:25 $ 00204 */ 00205 00206 #endif |
Tekkotsu v4.0 |
Generated Thu Nov 22 00:54:53 2007 by Doxygen 1.5.4 |