Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ArmMC.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_ArmMC_h
00003 #define INCLUDED_ArmMC_h
00004 
00005 #include "MotionCommand.h"
00006 #include "OutputCmd.h"
00007 #include "PostureEngine.h"
00008 #include "PostureMC.h"
00009 #include "Shared/RobotInfo.h"
00010 #include "Shared/mathutils.h"
00011 
00012 class ArmMC : public MotionCommand {
00013 public:
00014   //! Constructor, defaults to all joints to current value in ::state (i.e. calls takeSnapshot() automatically)
00015   ArmMC();
00016   virtual ~ArmMC() {}
00017 
00018   //! Constructor
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 
00035   //! sets the target joint positions to current sensor values
00036   /*! Similar to freezeMotion() when a motion is underway, but only if no other MotionCommands are using neck joints.
00037     *  A status event will @e not be generated unless a motion was already underway.
00038     *  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. */
00039   virtual void takeSnapshot();
00040 
00041   //!@name Speed Control
00042   
00043   //! Sets #maxSpeed to 0 (no maximum)
00044   void noMaxSpeed() { for(unsigned int i=0; i<NumArmJoints; i++) maxSpeed[i]=0; }
00045 
00046   //! Restores #maxSpeed to default settings from Config::Motion_Config
00047   /*! @param x ratio of the max speed to use; so 0.5 would limit motion to half the recommended upper limit */
00048   void defaultMaxSpeed(float x=1);
00049 
00050   //! Sets #maxSpeed for all joints in rad/sec
00051   void setMaxSpeed(float x) { for(unsigned int i=0; i<NumArmJoints; i++) setMaxSpeed(i,x); }
00052 
00053   //! Sets #maxSpeed for join @a i in rad/sec
00054   /*! @param i joint offset relative to ArmOffset (i.e. one of TPROffset_t)
00055    *  @param x maximum radians per second to move */
00056   void setMaxSpeed(unsigned int i, float x) { maxSpeed[i]=x*FrameTime/1000; }
00057 
00058   //! Set speed of gripper joints
00059   void setGripperSpeed(float x);
00060 
00061   //! Returns #maxSpeed in rad/sec
00062   /*! @param i joint offset relative to ArmOffset (i.e. one of TPROffset_t)
00063    *  @return the maximum speed of joint @a i in radians per second */
00064   float getMaxSpeed(unsigned int i) { return maxSpeed[i]*1000/FrameTime; }
00065 
00066   //@}
00067   
00068   //!@name Joint Accessors
00069   
00070   //! Sets the weight values for all the arm joints
00071   void setWeight(float w);  
00072   void setWeight(int x, float w);
00073   float armJointValue(unsigned int i);
00074 
00075 
00076   void setJointValue(unsigned int i, float value) {
00077 #ifdef TGT_HAS_ARMS
00078     if(!ensureValidJoint(i)) return;
00079     setWeight(i, 1);
00080     armTargets[i]=clipAngularRange(ArmOffset+i,value);
00081     setDirty();
00082 #endif
00083   }
00084 
00085   //! 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
00086   /*! @param i joint offset relative to ArmOffset (i.e. one of TPROffset_t) */
00087   float getJointValue(unsigned int i) const {
00088     if(ensureValidJoint(i))
00089       return armTargets[i];
00090     else
00091       return 0;
00092   }
00093 
00094   void setJoints(float shoulder, float elbow, float wrist);  //!< Set joint values for a three-link arm
00095   void setJoints(float shoulder, float elbow, float yaw, float pitch, float roll, float gripper); //!< Set joint values for a Chiara-style arm
00096   void setWrist(float pitch, float roll, float gripper);
00097 
00098   //! Pulse the gripper to prevent load errors
00099   void setGripperPulse(unsigned int onPeriod, unsigned int offPeriod);
00100   void clearGripperPulse();
00101 
00102   //-----------------------------NEW CODE-----------------------------
00103   //! Sets the desired load value for subsequent grip operations
00104   void requestGripperLoad(int newLoad);
00105   //! Returns the current active load on the gripper's servos
00106   int getGripperLoad();
00107   //! Sets the member #angleIncrement to the specified value (default = 0.05)
00108   /*! This is only applicable for CALLIOPE2 and will do nothing for any other robot.  */
00109   void setGraspSpeed(float speed);
00110   //! Sets the member #idleCycles to the specified value (default = 8)
00111   /*! This is only applicable for CALLIOPE2 and will do nothing for any other robot.
00112       The purpose of this parameter is to control how fast often #angleIncrement is used.
00113       If it is used too often, the gripper will move too fast, generate an invalid load,
00114       and then not close correctly.*/
00115   void setGraspWait(unsigned int cycles);
00116   //! Returns the value of desiredLoad last set by the user
00117   int getDesiredLoad() { return desiredLoad; }
00118   //------------------------------------------------------------------
00119 
00120   //! Move arm to specified point in base frame coordinates; return false if unreachable
00121   bool moveToPoint(float x, float y, float z) { return moveOffsetToPoint(fmat::Column<3>(), fmat::pack(x,y,z)); }
00122   bool moveToPoint(const fmat::Column<3>& tgt) { return moveOffsetToPoint(fmat::Column<3>(), tgt); }
00123   bool moveOffsetToPoint(const fmat::Column<3>& offset, const fmat::Column<3>& tgt);
00124   bool moveOffsetToPointWithOrientation(const fmat::Column<3>& offset, const fmat::Column<3>& tgt, const fmat::Quaternion& ori);
00125   
00126   //! Move the fingers or gripper to achieve the specified gap
00127   bool setFingerGap(float dist);
00128 
00129   //! Open the gripper to a certain percentage of its range of travel.
00130   bool openGripper(float percentage=0.5);
00131 
00132   void setOutputCmd(unsigned int i, const OutputCmd& c) { 
00133 #ifdef TGT_HAS_ARMS
00134     dirty=true; 
00135     completionReported=false; 
00136     armTargets[i-ArmOffset]=c.value;
00137     if(armCmds[i-ArmOffset].weight==0)
00138       armCmds[i-ArmOffset].value = state->outputs[i];
00139     armCmds[i-ArmOffset].weight = c.weight;
00140 #endif
00141   }
00142   
00143 public:
00144   //!@name Inherited:
00145   virtual int updateOutputs(); //!< Updates where the arm is looking
00146   virtual int isDirty() { return (dirty || !completionReported) ? NumArmJoints : 0; } //!< true if a change has been made since the last updateJointCmds() and we're active
00147   virtual int isAlive(); //!< Alive while target is not reached
00148   virtual void doStart() { MotionCommand::doStart(); setDirty(); } //!< marks this as dirty each time it is added
00149   //@}
00150 
00151 
00152   //! if completionReported, copies armCmds from MotionManager::getOutputCmd(), then sets dirty to true and completionReported to false
00153   /*! should be called each time a joint value gets modified in case
00154    *  the arm isn't where it's supposed to be, it won't jerk around
00155    * 
00156    *  MotionManager::getOutputCmd() is called instead of
00157    *  WorldState::outputs[] because if this is being called rapidly
00158    *  (i.e. after every sensor reading) using the sensor values will
00159    *  cause problems with very slow acceleration due to sensor lag
00160    *  continually resetting the current position.  Using the last
00161    *  value sent by the MotionManager fixes this.*/
00162   void setDirty();
00163 
00164 protected:
00165          /*//! checks if target point or direction is actually reachable
00166         bool isReachable(const NEWMAT::ColumnVector& Pobj) {
00167     NEWMAT::ColumnVector poE=armkin.convertLink(0,armkin.get_dof())*Pobj;
00168     const float theta = mathutils::rad2deg(acos(poE(3)/sqrt(poE(1)*poE(1)+poE(2)*poE(2)+poE(3)*poE(3))));
00169     //    cout << "theta: " << theta << " degrees\n";
00170     return theta < 5.0;
00171   }*/
00172 
00173   //! puts x in the range (-pi,pi)
00174   static float normalizeAngle(float x) { return x - static_cast<float>( rint(x/(2*M_PI)) * (2*M_PI) ); }
00175   
00176   //! 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
00177   static float clipAngularRange(unsigned int i, float x) {
00178     float min=outputRanges[i][MinRange];
00179     float max=outputRanges[i][MaxRange];
00180     if(x<min || x>max) {
00181       float mn_dist=std::abs(normalizeAngle(min-x));
00182       float mx_dist=std::abs(normalizeAngle(max-x));
00183       if(mn_dist<mx_dist)
00184         return min;
00185       else
00186         return max;
00187     } else
00188       return x;
00189   }
00190 
00191   //! Makes sure @a i is in the range (0,NumArmJoints).  If it is instead in the range (ArmOffset,ArmOffset+NumArmJoints), output a warning and reset @a i to the obviously intended value.
00192   /*! @param[in] i joint offset relative to either ArmOffset (i.e. one of TPROffset_t) or 0
00193    *  @param[out] i joint offset relative to ArmOffset (i.e. one of TPROffset_t)
00194    *  @return true if the intended joint could be ascertained, false otherwise */
00195   static bool ensureValidJoint(unsigned int& i);
00196 
00197   void incrementGrasp(); //!< Helper function to execute a grasp
00198   
00199 
00200   bool dirty;                          //!< true if a change has been made since last call to updateJointCmds()
00201   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()
00202   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.01 (5.7 degree error)
00203   bool completionReported;                  //!< true if the most recent movement request has completed
00204   unsigned int targetTimestamp;        //!< time at which the completionReported flag was set
00205   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
00206   float armTargets[NumArmJoints];    //!< stores the target value of each joint
00207   OutputCmd armCmds[NumArmJoints];   //!< stores the last values we sent from updateOutputs
00208   float maxSpeed[NumArmJoints];       //!< initialized from Config::motion_config, but can be overridden by setMaxSpeed(); rad per frame
00209   unsigned int pulseOnPeriod; //!< number of milliseconds to keep servo power on, when pulsing
00210   unsigned int pulseOffPeriod; //!< number of milliseconds to keep servo power off, when pulsing
00211   unsigned int pulseStartTime; //!< when the current pulse cycle started
00212   signed int desiredLoad; //!< how tightly to grasp an object (negative is for tighter loads; -280 is a good value)
00213   float angleIncrement; //!< How fast to close the gripper (0.05 by default)
00214   unsigned int idleCycles;  //!< How long to wait before updating the gripper angle by #angleIncrement (8 by default)
00215 };
00216 
00217 #endif

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