Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

HeadPointerMC.h

Go 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