Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
ArmMC.hGo 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 |