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

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:41 2016 by Doxygen 1.6.3