Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
Kinematics.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_Kinematics_h_ 00003 #define INCLUDED_Kinematics_h_ 00004 00005 #include "Shared/RobotInfo.h" 00006 #include "Wireless/Socket.h" 00007 #include "Shared/newmat/newmat.h" 00008 #include <string> 00009 #include <vector> 00010 #include <ext/hash_map> 00011 00012 namespace ROBOOP { 00013 class Robot; 00014 class Config; 00015 } 00016 00017 //! Provides access to the mathematical functionality of the ROBOOP package using Tekkotsu data structures 00018 /*! 00019 * You should read the <a 00020 * href="http://www.tekkotsu.org/Kinematics.html">Kinematics 00021 * tutorial</a> to get a general understanding of the math involved 00022 * and diagrams for usage with supported robots. 00023 * 00024 * This class involves all aspects of the forward kinematics: 00025 * calculations concerning locations and orientations in space given 00026 * a known set of joint configurations. There is a global 00027 * instantiation of Kinematics named ::kine, which can be used to 00028 * perform these calculations regarding the joint positions currently 00029 * in ::state. 00030 * 00031 * To perform kinematics on a hypothetical set of joint values, 00032 * please see PostureEngine or its subclasses. PostureEngine also 00033 * includes inverse kinematic functions, which will allow you to 00034 * determine joint angles in order to reach a given point. 00035 * 00036 * The underlying ROBOOP library does not currently handle branching 00037 * kinematic chains - in other words, each limb of the robot is a 00038 * separate Robot as far as ROBOOP is concerned. This class 00039 * interfaces the Tekkotsu array index approach to referencing joints 00040 * with ROBOOP's chain based hierarchy. 00041 * 00042 * Thus, wherever a link or reference frame index is requested, you 00043 * can simply supply one of the output indexes in the usual manner: 00044 * @code kine->frameToBase(HeadOffset+TiltOffset); @endcode 00045 * 00046 * However, there are also a number of points on the body which are 00047 * not joints, but should have their own reference frames, such as 00048 * the base frame, or the camera. These frames have their own 00049 * indices, listed in the robot info file for the model in question 00050 * (such as ERS7Info.h), with names ending in @c FrameOffset. 00051 * @code kine->frameToBase(CameraFrameOffset); @endcode 00052 * 00053 * Since newmat matrix library is used by ROBOOP, you will need to 00054 * pass and receive information in newmat matrices. This class 00055 * provides #pack and #unpack functions which can convert individual 00056 * x,y,z variables into a NEWMAT::ColumnVector, and vice versa. 00057 * However, for readability of your code and long-term ease of use, 00058 * we recommend embracing the newmat data structures directly when 00059 * appropriate. 00060 * @code 00061 * // Find the ray from the camera to whatever the near-field IR is hitting: 00062 * // x and y will be in the range -1 to 1 for resolution layer independence 00063 * // This ignores lens distortion - just proof of concept 00064 * NEWMAT::Matrix T = kine->frameToFrame(NearIRFrameOffset,CameraFrameOffset); 00065 * NEWMAT::ColumnVector camera_ray = T*Kinematics::pack(0,0,state->sensors[NearIRDistOffset]); 00066 * float x=atan(camera_ray(1),camera_ray(3))/config->vision.horizFOV/2; 00067 * float y=atan(camera_ray(2),camera_ray(3))/config->vision.vertFOV/2; 00068 * @endcode 00069 * 00070 * Finally, for each model we have created a database of "interest points", 00071 * locations of notable interest on the body of the robot. These may be of 00072 * use to people attempting to use the limbs to manipulate objects. 00073 * To access these interest points, simply call either #getLinkInterestPoint 00074 * or #getFrameInterestPoint with the name of the interest point, obtained 00075 * from the <a href="http://www.tekkotsu.org/Kinematics.html">diagrams</a>. 00076 * 00077 * Note that you can pass a comma separated list of interest point names 00078 * and the result will be the midpoint of those interest points: 00079 * @code kine->getLinkInterestPoint(BaseFrameOffset,"LowerInnerFrontLFrShin,LowerOuterFrontLFrShin"); @endcode 00080 * 00081 * @see PostureEngine for inverse kinematics 00082 */ 00083 class Kinematics { 00084 public: 00085 //!Constructor, pass the full path to the kinematics configuration file 00086 Kinematics() : chains(), chainMaps() { 00087 init(); 00088 } 00089 00090 //!Destructor 00091 virtual ~Kinematics() {} 00092 00093 00094 00095 //! Returns a matrix for transforming from link @a j to base frame 00096 /*! @param[in] j the link number, see class notes for values */ 00097 NEWMAT::ReturnMatrix linkToBase(unsigned int j); 00098 00099 //! Returns a matrix for transforming from frame @a j to base frame 00100 /*! @param[in] j the frame number, see class notes for values */ 00101 NEWMAT::ReturnMatrix frameToBase(unsigned int j); 00102 00103 //! Returns a matrix for transforming from the base frame to link @a j 00104 /*! @param[in] j the link number, see class notes for values */ 00105 NEWMAT::ReturnMatrix baseToLink(unsigned int j); 00106 00107 //! Returns a matrix for transforming from the base frame to frame @a j 00108 /*! @param[in] j the frame number, see class notes for values */ 00109 NEWMAT::ReturnMatrix baseToFrame(unsigned int j); 00110 00111 //! Returns a matrix for transforming from link @a ij to link @a oj frame 00112 /*! @param[in] ij the link number to convert from, see class notes for values 00113 * @param[in] oj the link number to convert to, see class notes for values */ 00114 NEWMAT::ReturnMatrix linkToLink(unsigned int ij, unsigned int oj); 00115 00116 //! Returns a matrix for transforming from link @a ij to frame @a oj 00117 /*! @param[in] ij the link number to convert from, see class notes for values 00118 * @param[in] oj the frame number to convert to, see class notes for values */ 00119 NEWMAT::ReturnMatrix linkToFrame(unsigned int ij, unsigned int oj); 00120 00121 //! Returns a matrix for transforming from frame @a ij to link @a oj 00122 /*! @param[in] ij the frame number to convert from, see class notes for values 00123 * @param[in] oj the link number to convert to, see class notes for values */ 00124 NEWMAT::ReturnMatrix frameToLink(unsigned int ij, unsigned int oj); 00125 00126 //! Returns a matrix for transforming from frame @a ij to frame @a oj frame 00127 /*! @param[in] ij the frame number to convert from, see class notes for values 00128 * @param[in] oj the frame number to convert to, see class notes for values */ 00129 NEWMAT::ReturnMatrix frameToFrame(unsigned int ij, unsigned int oj); 00130 00131 00132 00133 //! Returns the location of a named point and the link it is attached to 00134 /*! @param[in] name the name of the interest point; varies by model, <a href="http://www.tekkotsu.org/Kinematics.html">see the diagrams</a> for your model. 00135 * @param[out] j on exit, joint index of the link, or -1U if not found 00136 * @param[out] ip on exit, a homogenous column vector of the requested point 00137 * 00138 * If @a name is not found, j will be -1 and ip will be all 0's. */ 00139 void getInterestPoint(const std::string& name, unsigned int& j, NEWMAT::Matrix& ip); 00140 00141 //! Returns the location of a named point, relative to any desired reference frame 00142 /*! @param[in] frame the desired reference frame to give results in 00143 * @param[in] name the name of the interest point; varies by model, <a href="http://www.tekkotsu.org/Kinematics.html">see the diagrams</a> for your model. 00144 * 00145 * You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs */ 00146 NEWMAT::ReturnMatrix getFrameInterestPoint(unsigned int frame, const std::string& name); 00147 00148 //! Returns the location of a named point, relative to any desired reference frame 00149 /*! @param[in] link the desired link frame to give results in 00150 * @param[in] name the name of the interest point; varies by model, <a href="http://www.tekkotsu.org/Kinematics.html">see the diagrams</a> for your model. 00151 * 00152 * You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs */ 00153 NEWMAT::ReturnMatrix getLinkInterestPoint(unsigned int link, const std::string& name) { 00154 NEWMAT::ColumnVector p=frameToLink(link,link)*getFrameInterestPoint(link,name); 00155 p.Release(); return p; 00156 } 00157 00158 00159 00160 //! Find the leg which is in least contact with ground (as best we can anyway) 00161 /*! This can be either based on gravity vector from accelerometer readings, 00162 * or if that may be unreliable due to being in motion, we could do some basic 00163 * balance modeling. 00164 * @return index of leg which is highest in reference to gravity vector */ 00165 LegOrder_t findUnusedLeg(const NEWMAT::ColumnVector& down); 00166 00167 //! Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)) 00168 /*! This function merely calls the other version of calculateGroundPlane with the current 00169 * gravity vector as the "down" vector. 00170 * @return vector of the form @f$p_1x + p_2y + p_3z = p_4@f$, relative to the base frame */ 00171 NEWMAT::ReturnMatrix calculateGroundPlane(); 00172 00173 //! Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)) 00174 /*! @return vector of the form @f$p_1x + p_2y + p_3z = p_4@f$, relative to the base frame */ 00175 NEWMAT::ReturnMatrix calculateGroundPlane(const NEWMAT::ColumnVector& down); 00176 00177 //! Find the point of intersection between a ray and a plane 00178 /*! @param j is the link number the ray is relative to 00179 * @param r_j is the line through the origin, in homogenous coordinates 00180 * @param b is the link number the plane is relative to (probably BaseFrameOffset) 00181 * @param p_b represents the plane to be intersected 00182 * @param f is the link number the results should be relative to 00183 * 00184 * @a p_b should be of the form @f$p_1x + p_2y + p_3z = p_4@f$ 00185 * @return homogenous coordinate of intersection (may be infinity) */ 00186 NEWMAT::ReturnMatrix projectToPlane(unsigned int j, const NEWMAT::ColumnVector& r_j, 00187 unsigned int b, const NEWMAT::ColumnVector& p_b, 00188 unsigned int f); 00189 00190 00191 //! A simple utility function, converts x,y,z,h to a NEWMAT::ColumnVector 00192 /*! @param[in] x the value for the first row 00193 * @param[in] y the value for the second row 00194 * @param[in] z the value for the third row 00195 * @param[in] h the value for the fourth row (defaults to 1 if not specified) 00196 * @return @f$ \left[\begin{array}{c} x\\y\\z\\h\\ \end{array}\right] @f$ */ 00197 static NEWMAT::ReturnMatrix pack(float x, float y, float z, float h=1) { 00198 NEWMAT::ColumnVector m(4); 00199 m(1)=x; m(2)=y; m(3)=z; m(4)=h; 00200 m.Release(); return m; 00201 } 00202 //! A simple utility function, pulls the first 3 rows of the first column, divides each by the fourth row, and stores into ox, oy, and oz 00203 /*! @param[in] m the matrix to unpack (only uses first column) 00204 * @param[out] ox set to the first row of the first column of @a m, divided by fourth row 00205 * @param[out] oy set to the second row of the first column of @a m, divided by fourth row 00206 * @param[out] oz set to the third row of the first column of @a m, divided by fourth row */ 00207 static void unpack(NEWMAT::Matrix m, float& ox, float& oy, float& oz) { 00208 ox=m(1,1)/m(4,1); oy=m(2,1)/m(4,1); oz=m(3,1)/m(4,1); 00209 } 00210 //! A simple utility function, pulls the first 4 rows of the first column, stores into ox, oy, oz, oh 00211 /*! @param[in] m the matrix to unpack (only uses first column) 00212 * @param[out] ox set to the first row of the first column of @a m 00213 * @param[out] oy set to the second row of the first column of @a m 00214 * @param[out] oz set to the third row of the first column of @a m 00215 * @param[out] oh set to the fourth row of the first column of @a m */ 00216 static void unpack(NEWMAT::Matrix m, float& ox, float& oy, float& oz, float& oh) { 00217 ox=m(1,1); oy=m(2,1); oz=m(3,1); oh=m(4,1); 00218 } 00219 00220 static ROBOOP::Config * getConfig() { return roconfig; } 00221 00222 protected: 00223 //! Called by constructors to do basic setup - first call will read Config::motion_config::kinematics from disk, future initializes reuse static roconfig 00224 void init(); 00225 00226 //! Returns the location of a named point, relative to the link it is attached to 00227 /*! @param[in] name the name of the interest point; varies by model, <a href="http://www.tekkotsu.org/Kinematics.html">see the diagrams</a> for your model. 00228 * @param[out] c on exit, chain index the IP is on 00229 * @param[out] l on exit, link index the IP is on 00230 * @param[out] ip on exit, a homogenous column vector of the requested point 00231 * 00232 * If @a name is not found, @a c and @a l will be -1 and @a ip will be all 0's. 00233 * This internal version of the function allows us to use @a c and @a l, ourselves, 00234 * but users will probably want to use the getInterestPoint(name,j,ip) version */ 00235 void getInterestPoint(const std::string& name, unsigned int& c, unsigned int& l, NEWMAT::Matrix& ip); 00236 00237 //! Called at the beginning of each function which accesses ROBOOP computations - should make sure the ROBOOP structures are up to date with Tekkotsu structures 00238 /*! This class will pull current values from WorldState, but it is expected 00239 * that subclasses (i.e. PostureEngine) will want to provide their own joint values. 00240 * Updates from link 1 through link @a l. 00241 * @param[in] c the chain to update 00242 * @param[in] l the last link to update (later links in the chain are left untouched) */ 00243 virtual void update(unsigned int c, unsigned int l); 00244 00245 //! converts a Tekkotsu output index to a chain and link 00246 /*! @param[in] tkout the tekkotsu index to lookup 00247 * @param[out] c set to the chain index that @a tkout is in 00248 * @param[out] l set to the link in @a c corresponding to @a tkout */ 00249 bool lookup(unsigned int tkout, unsigned int& c, unsigned int& l) { 00250 if(tkout>=NumReferenceFrames) { 00251 serr->printf("ERROR Kinematics: illegal link %d\n",l); 00252 return false; 00253 } 00254 if(jointMaps[tkout].chain>=chains.size()) { 00255 serr->printf("ERROR Kinematics: no chain available for link %d\n",l); 00256 return false; 00257 } 00258 c=jointMaps[tkout].chain; 00259 l=jointMaps[tkout].link; 00260 return true; 00261 } 00262 00263 //! A separate ROBOOP::Robot instantiation for each chain since ROBOOP package doesn't support branching chains (which would be rather difficult to implement well) 00264 std::vector<ROBOOP::Robot*> chains; 00265 00266 //! holds mapping for each chain's links back to the tekkotsu outputs they represent 00267 std::vector< std::vector<unsigned int> > chainMaps; 00268 00269 //! Allows mapping from tekkotsu output index to chain and link indicies 00270 struct JointMap { 00271 JointMap() : chain(-1U), link(-1U) {} //!< constructor 00272 JointMap(unsigned int c, unsigned int l) : chain(c), link(l) {} //!< constructor 00273 unsigned int chain; //! the chain index 00274 unsigned int link; //!< the link index 00275 }; 00276 //! holds the position and attached link of a given interest point 00277 struct InterestPoint { 00278 InterestPoint() : x(), y(), z(), chain(), link() {} //!< constructor 00279 InterestPoint(float x_, float y_, float z_, unsigned int chain_, unsigned int link_) 00280 : x(x_), y(y_), z(z_), chain(chain_), link(link_) {} //!< constructor 00281 float x; //!< x value 00282 float y; //!< y value 00283 float z; //!< z value 00284 unsigned int chain; //!< chain containing @a link 00285 int link; //!< link in @a chain 00286 }; 00287 00288 //! holds mapping from tekkotsu output index to chain and link indicies 00289 JointMap jointMaps[NumReferenceFrames]; 00290 00291 //! cache of the configuration of the robot for rapid initialization (so we don't have to re-read from disk) 00292 static ROBOOP::Config * roconfig; 00293 //! allows us to use the STL hash_map with strings 00294 struct hashstring { 00295 //! hashes a string by multiplying current total by 5, add new character 00296 /*! not my idea, this is what the STL library does for char*, i've just reimplemented it for strings */ 00297 size_t operator()(const string& s) const { 00298 unsigned long h=0; 00299 for(string::size_type x=s.size(); x!=0; x--) 00300 h=h*5+s[x]; 00301 return (size_t)h; 00302 } 00303 }; 00304 //! we'll be using the hash_map to store named interest points 00305 typedef __gnu_cxx::hash_map<const string,InterestPoint,hashstring> InterestPointMap; 00306 //! these interest points are shared by all Kinematics classes (i.e. all PostureEngines) 00307 /*! this is to reduce initialization time, but does mean one robot can't do 00308 * kinematic calculations regarding a different model robot... */ 00309 static InterestPointMap * ips; 00310 }; 00311 00312 //! a global instance of Kinematics, joint values reference those of WorldState so users can easily query the current spatial locations of joints 00313 extern Kinematics * kine; 00314 00315 /*! @file 00316 * @brief Describes Kinematics, which provides access to the mathematical functionality of the roboop package using Tekkotsu data structures 00317 * @author ejt (Creator) 00318 * 00319 * $Author: ejt $ 00320 * $Name: tekkotsu-2_2 $ 00321 * $Revision: 1.23 $ 00322 * $State: Exp $ 00323 * $Date: 2004/10/19 17:06:31 $ 00324 */ 00325 00326 #endif |
Tekkotsu v2.2 |
Generated Tue Oct 19 14:19:14 2004 by Doxygen 1.3.9.1 |