Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | 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 * use PostureEngine or one of its subclasses. PostureEngine also 00033 * adds 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 ROBOOP::Robot as far as ROBOOP is concerned. This 00039 * Kinematics class interfaces the Tekkotsu array index approach to 00040 * referencing joints with ROBOOP's chain based hierarchy. 00041 * 00042 * Thus, wherever a reference frame index is requested, you can 00043 * simply supply one of the output indexes in the usual manner: 00044 * @code kine->jointToBase(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->jointToBase(CameraFrameOffset); @endcode 00052 * Note that for these non-joint-associated reference frames, the 00053 * link and joint frames are always identical, so you can use either 00054 * version of the corresponding functions. 00055 * 00056 * Since newmat matrix library is used by ROBOOP, you will need to 00057 * pass and receive information in newmat matrices. Kinematics class 00058 * provides static #pack and #unpack functions which can convert 00059 * individual x,y,z variables into a NEWMAT::ColumnVector, and vice 00060 * versa. However, for readability of your code and long-term ease 00061 * of use, we recommend embracing the newmat data structures directly 00062 * when appropriate. 00063 * @code 00064 * // Find the ray from the camera to whatever the near-field IR is hitting: 00065 * NEWMAT::Matrix T = kine->jointToJoint(NearIRFrameOffset,CameraFrameOffset); 00066 * NEWMAT::ColumnVector camera_ray = T*Kinematics::pack(0,0,state->sensors[NearIRDistOffset]); 00067 * float x,y; // x and y will be in the range -1 to 1 for resolution layer independence 00068 * config->vision.computePixel(camera_ray(1),camera_ray(2),camera_ray(3),x,y); 00069 * @endcode 00070 * 00071 * Finally, for each model we have created a database of "interest points" -- 00072 * locations of notable interest on the body of the robot. These may be of 00073 * use to people attempting to use the limbs to manipulate objects. 00074 * To access these interest points, call either #getLinkInterestPoint 00075 * or #getJointInterestPoint with the name of the interest point, obtained 00076 * from the <a href="http://www.tekkotsu.org/Kinematics.html">diagrams</a>. 00077 * 00078 * Note that you can pass a comma separated list of interest point names 00079 * and the result will be the midpoint of those interest points: 00080 * @code kine->getLinkInterestPoint(BaseFrameOffset,"LowerInnerFrontLFrShin,LowerOuterFrontLFrShin"); @endcode 00081 * 00082 * @see PostureEngine for inverse kinematics 00083 * 00084 * 00085 * 00086 * <a name="config_file_format"></a><h2>Configuration File Format</h2> 00087 * 00088 * The file is actually read by ROBOOP::Config, and thus the syntax 00089 * of the file is defined by that class. However, Tekkotsu will look 00090 * for some additional sections beyond what is expected by ROBOOP. 00091 * 00092 * In any give link section, a <tt>tekkotsu_output</tt> field may 00093 * appear, which specifies the index (aka offset) of the 00094 * corresponding joint in Tekkotsu, as defined by the model's Info.h 00095 * file (e.g. ERS7Info.h). Alternatively, <tt>tekkotsu_frame</tt> 00096 * may be specified, which should specify the offset of an abstract 00097 * reference frame, which does not correspond to any joint. 00098 * Typically this is used for things such as the camera, or 00099 * un-actuated joints, such as the spring-loaded ankles. 00100 * 00101 * Additionally, Kinematics will look for an custom 00102 * <tt>InterestPoints</tt> section, which should contain a 00103 * <tt>Length</tt> field for specifying the number of interest 00104 * points. Kinematics will then attempt to read 00105 * <tt>%InterestPoint</tt><i>N</i> for 1 through <i>Length</i>. 00106 * 00107 * Each <tt>%InterestPoint</tt><i>N</i> section should contain: 00108 * - <tt>name</tt> - (string) name which will be passed to get*InterestPoint() to retrieve this IP 00109 * - <tt>chain</tt> - (string) name of the chain the IP is in, must match one of the kinematic chains loaded from the file 00110 * - <tt>link</tt> - (unsigned int) the index of the link the IP is connected to 00111 * - <tt>x</tt> - (float) the x location of the point, in link-relative coordinates 00112 * - <tt>y</tt> - (float) the y location of the point, in link-relative coordinates 00113 * - <tt>z</tt> - (float) the z location of the point, in link-relative coordinates 00114 */ 00115 class Kinematics { 00116 public: 00117 //!Constructor, pass the full path to the kinematics configuration file 00118 Kinematics() 00119 #ifdef THREADSAFE_KINEMATICS 00120 : chains() 00121 #endif 00122 { 00123 init(); 00124 } 00125 //!Copy constructor, everything is either update-before-use or static, copy is normal init 00126 Kinematics(const Kinematics&) 00127 #ifdef THREADSAFE_KINEMATICS 00128 : chains() 00129 #endif 00130 { 00131 init(); 00132 } 00133 //!Assignment operator, everything is either update-before-use or static, assignment is no-op 00134 Kinematics& operator=(const Kinematics&) { return *this; } 00135 00136 //!Destructor 00137 virtual ~Kinematics(); 00138 00139 00140 00141 //! Returns a matrix for transforming from link frame @a j to base frame 00142 /*! @param[in] link the output offset, see class notes for values */ 00143 NEWMAT::ReturnMatrix linkToBase(unsigned int link); 00144 00145 //! Returns a matrix for transforming from joint frame @a j to base frame 00146 /*! @param[in] joint the output offset, see class notes for values */ 00147 NEWMAT::ReturnMatrix jointToBase(unsigned int joint); 00148 00149 //! Returns a matrix for transforming from the base frame to link @a j frame 00150 /*! @param[in] link the output offset, see class notes for values */ 00151 NEWMAT::ReturnMatrix baseToLink(unsigned int link); 00152 00153 //! Returns a matrix for transforming from the base frame to joint @a j frame 00154 /*! @param[in] joint the output offset, see class notes for values */ 00155 NEWMAT::ReturnMatrix baseToJoint(unsigned int joint); 00156 00157 //! Returns a matrix for transforming from link @a ij to link @a oj 00158 /*! @param[in] iL the output offset to convert from, see class notes for values 00159 * @param[in] oL the output offset to convert to, see class notes for values */ 00160 NEWMAT::ReturnMatrix linkToLink(unsigned int iL, unsigned int oL); 00161 00162 //! Returns a matrix for transforming from link frame @a ij to joint frame @a oj 00163 /*! @param[in] iL the output offset to convert from, see class notes for values 00164 * @param[in] oJ the output offset to convert to, see class notes for values */ 00165 NEWMAT::ReturnMatrix linkToJoint(unsigned int iL, unsigned int oJ); 00166 00167 //! Returns a matrix for transforming from joint frame @a ij to link frame @a oj 00168 /*! @param[in] iJ the output offset to convert from, see class notes for values 00169 * @param[in] oL the output offset to convert to, see class notes for values */ 00170 NEWMAT::ReturnMatrix jointToLink(unsigned int iJ, unsigned int oL); 00171 00172 //! Returns a matrix for transforming from joint @a ij to joint @a oj 00173 /*! @param[in] iJ the output offset to convert from, see class notes for values 00174 * @param[in] oJ the output offset to convert to, see class notes for values */ 00175 NEWMAT::ReturnMatrix jointToJoint(unsigned int iJ, unsigned int oJ); 00176 00177 00178 00179 //! Returns the location of a named point and the link it is attached to 00180 /*! @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. 00181 * @param[out] link on exit, offset of the link, or -1U if not found 00182 * @param[out] ip on exit, a homogeneous column vector of the requested point, relative to the link frame returned in @a j 00183 * 00184 * If @a name is not found, j will be -1 and ip will be all 0's. */ 00185 void getInterestPoint(const std::string& name, unsigned int& link, NEWMAT::Matrix& ip); 00186 00187 //! Returns the location of a named point, relative to any desired joint reference frame 00188 /*! @param[in] joint the desired joint reference frame to give results in 00189 * @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. 00190 * 00191 * You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs */ 00192 NEWMAT::ReturnMatrix getJointInterestPoint(unsigned int joint, const std::string& name); 00193 00194 //! Returns the location of a named point, relative to any desired reference frame 00195 /*! @param[in] link the desired link reference frame to give results in 00196 * @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. 00197 * 00198 * You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs */ 00199 NEWMAT::ReturnMatrix getLinkInterestPoint(unsigned int link, const std::string& name) { 00200 NEWMAT::ColumnVector p=jointToLink(link,link)*getJointInterestPoint(link,name); 00201 p.Release(); return p; 00202 } 00203 00204 00205 00206 #ifdef TGT_HAS_LEGS 00207 //! Calculate the leg heights along a given "down" vector (0 is level with base frame) 00208 /*! This can be based on either the gravity vector from accelerometer readings, 00209 * or if that may be unreliable due to being in motion, you could do some basic 00210 * balance modeling and pass a predicted vector. This uses the interest point database 00211 * to find the lowest interest point for each leg 00212 * @note on Aibo platforms, if packing accelerometer readings, don't forget to negate the "left" accelerometer! */ 00213 void calcLegHeights(const NEWMAT::ColumnVector& down, float heights[NumLegs]); 00214 00215 //! Find the leg which is in least contact with ground 00216 /*! @see calcLegHeights() 00217 * @note on Aibo platforms, if packing accelerometer readings, don't forget to negate the "left" accelerometer! 00218 * @return index of leg which is highest in reference to gravity vector */ 00219 LegOrder_t findUnusedLeg(const NEWMAT::ColumnVector& down); 00220 #endif 00221 00222 //! Find the ground plane by fitting a plane to the lowest 3 interest points 00223 /*! This function merely calls the other version of calculateGroundPlane with the current 00224 * gravity vector as the "down" vector. 00225 * @return vector of the form @f$p_1x + p_2y + p_3z = p_4@f$, relative to the base frame */ 00226 NEWMAT::ReturnMatrix calculateGroundPlane(); 00227 00228 //! Find the ground plane by fitting a plane to the lowest 3 interest points 00229 /*! @note on Aibo platforms, if packing accelerometer readings, don't forget to negate the "left" accelerometer! 00230 * @return vector of the form @f$p_1x + p_2y + p_3z = p_4@f$, relative to the base frame */ 00231 NEWMAT::ReturnMatrix calculateGroundPlane(const NEWMAT::ColumnVector& down); 00232 00233 //! Find the point of intersection between a ray and a plane 00234 /*! @param j is the link number the ray is relative to 00235 * @param r_j is the line through the origin, in homogeneous coordinates 00236 * @param b is the link number the plane is relative to (probably BaseFrameOffset) 00237 * @param p_b represents the plane to be intersected 00238 * @param f is the link number the results should be relative to 00239 * 00240 * @a p_b should be of the form @f$p_1x + p_2y + p_3z = p_4@f$ 00241 * @return homogeneous coordinate of intersection (may be infinity) */ 00242 NEWMAT::ReturnMatrix projectToPlane(unsigned int j, const NEWMAT::ColumnVector& r_j, 00243 unsigned int b, const NEWMAT::ColumnVector& p_b, 00244 unsigned int f); 00245 00246 00247 //! A simple utility function, converts x,y,z,h to a NEWMAT::ColumnVector 00248 /*! @param[in] x the value for the first row 00249 * @param[in] y the value for the second row 00250 * @param[in] z the value for the third row 00251 * @param[in] h the value for the fourth row (defaults to 1 if not specified) 00252 * @return @f$ \left[\begin{array}{c} x\\y\\z\\h\\ \end{array}\right] @f$ */ 00253 static NEWMAT::ReturnMatrix pack(float x, float y, float z, float h=1) { 00254 NEWMAT::ColumnVector m(4); 00255 m(1)=x; m(2)=y; m(3)=z; m(4)=h; 00256 m.Release(); return m; 00257 } 00258 //! 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 00259 /*! @param[in] m the matrix to unpack (only uses first column) 00260 * @param[out] ox set to the first row of the first column of @a m, divided by fourth row 00261 * @param[out] oy set to the second row of the first column of @a m, divided by fourth row 00262 * @param[out] oz set to the third row of the first column of @a m, divided by fourth row */ 00263 static void unpack(NEWMAT::Matrix m, float& ox, float& oy, float& oz) { 00264 ox=m(1,1)/m(4,1); oy=m(2,1)/m(4,1); oz=m(3,1)/m(4,1); 00265 } 00266 //! A simple utility function, pulls the first 4 rows of the first column, stores into ox, oy, oz, oh 00267 /*! @param[in] m the matrix to unpack (only uses first column) 00268 * @param[out] ox set to the first row of the first column of @a m 00269 * @param[out] oy set to the second row of the first column of @a m 00270 * @param[out] oz set to the third row of the first column of @a m 00271 * @param[out] oh set to the fourth row of the first column of @a m */ 00272 static void unpack(NEWMAT::Matrix m, float& ox, float& oy, float& oz, float& oh) { 00273 ox=m(1,1); oy=m(2,1); oz=m(3,1); oh=m(4,1); 00274 } 00275 00276 //! returns the global ROBOOP::Config object which Kinematics classes initialize themselves from (#roconfig) 00277 static ROBOOP::Config * getConfig() { return roconfig; } 00278 00279 protected: 00280 //! Called by constructors to do basic setup - first call will read Config::motion_config::kinematics from disk, future initializes reuse static roconfig 00281 void init(); 00282 00283 //! initializes static variables -- only call if not #staticsInited 00284 static void initStatics(); 00285 00286 //! checks that statics have been initialized, and calls initStatics if they are missing 00287 static void checkStatics() { if(!staticsInited) initStatics(); } 00288 00289 //! called by init to allocate/initialize each of #chains 00290 static ROBOOP::Robot* newChain(unsigned int chainIdx); 00291 00292 //! Returns the location of a named point, relative to the link it is attached to 00293 /*! @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. 00294 * @param[out] c on exit, chain index the IP is on 00295 * @param[out] l on exit, link index the IP is on 00296 * @param[out] ip on exit, a homogeneous column vector of the requested point 00297 * 00298 * If @a name is not found, @a c and @a l will be -1 and @a ip will be all 0's. 00299 * This internal version of the function allows us to use @a c and @a l, ourselves, 00300 * but users will probably want to use the getInterestPoint(name,j,ip) version */ 00301 void getInterestPoint(const std::string& name, unsigned int& c, unsigned int& l, NEWMAT::Matrix& ip); 00302 00303 //! Called at the beginning of each function which accesses ROBOOP computations - should make sure the ROBOOP structures are up to date with Tekkotsu structures 00304 /*! This class will pull current values from WorldState, but it is expected 00305 * that subclasses (i.e. PostureEngine) will want to provide their own joint values. 00306 * Updates from link 1 through link @a l. 00307 * @param[in] c the chain to update 00308 * @param[in] l the last link to update (later links in the chain are left untouched) */ 00309 virtual void update(unsigned int c, unsigned int l); 00310 00311 //! converts a Tekkotsu output index to a chain and link 00312 /*! @param[in] tkout the tekkotsu index to lookup 00313 * @param[out] c set to the chain index that @a tkout is in 00314 * @param[out] l set to the link in @a c corresponding to @a tkout */ 00315 bool lookup(unsigned int tkout, unsigned int& c, unsigned int& l) { 00316 if(tkout>=NumReferenceFrames) { 00317 serr->printf("ERROR Kinematics: illegal link %d\n",l); 00318 return false; 00319 } 00320 if(jointMaps[tkout].chain>=chains.size()) { 00321 serr->printf("ERROR Kinematics: no chain available for link %d\n",l); 00322 return false; 00323 } 00324 c=jointMaps[tkout].chain; 00325 l=jointMaps[tkout].link; 00326 return true; 00327 } 00328 00329 //! initially false, set to true after first Kinematics is initialized 00330 static bool staticsInited; 00331 00332 #ifdef THREADSAFE_KINEMATICS 00333 //! A separate ROBOOP::Robot instantiation for each chain since ROBOOP package doesn't support branching chains (which would be rather difficult to implement well) 00334 std::vector<ROBOOP::Robot*> chains; 00335 #else 00336 //! A separate ROBOOP::Robot instantiation for each chain since ROBOOP package doesn't support branching chains (which would be rather difficult to implement well) 00337 /*! static allocation solves problems with shared memory regions, but becomes thread-UNsafe... */ 00338 static std::vector<ROBOOP::Robot*> chains; 00339 #endif 00340 00341 //! holds mapping for each chain's links back to the tekkotsu outputs and reference frames they represent 00342 static std::vector< std::vector<unsigned int> > chainMaps; 00343 00344 //! Allows mapping from tekkotsu output index to chain and link indicies 00345 struct JointMap { 00346 JointMap() : chain(-1U), link(-1U) {} //!< constructor 00347 JointMap(unsigned int c, unsigned int l) : chain(c), link(l) {} //!< constructor 00348 unsigned int chain; //!< the chain index 00349 unsigned int link; //!< the link index 00350 }; 00351 //! holds the position and attached link of a given interest point 00352 struct InterestPoint { 00353 InterestPoint() : x(), y(), z(), chain(), link() {} //!< constructor 00354 InterestPoint(float x_, float y_, float z_, unsigned int chain_, unsigned int link_) 00355 : x(x_), y(y_), z(z_), chain(chain_), link(link_) {} //!< constructor 00356 float x; //!< x value 00357 float y; //!< y value 00358 float z; //!< z value 00359 unsigned int chain; //!< chain containing @a link 00360 int link; //!< link in @a chain 00361 }; 00362 00363 //! holds mapping from tekkotsu output index to chain and link indicies 00364 static JointMap jointMaps[NumReferenceFrames]; 00365 00366 //! cache of the configuration of the robot for rapid initialization (so we don't have to re-read from disk) 00367 static ROBOOP::Config * roconfig; 00368 //! allows us to use the STL hash_map with strings 00369 struct hashstring { 00370 //! hashes a string by multiplying current total by 5, add new character 00371 /*! not my idea, this is what the STL library does for char*, i've just reimplemented it for strings */ 00372 size_t operator()(const std::string& s) const { 00373 unsigned long h=0; 00374 for(std::string::size_type x=s.size(); x!=0; x--) 00375 h=h*5+s[x]; 00376 return (size_t)h; 00377 } 00378 }; 00379 //! we'll be using the hash_map to store named interest points 00380 typedef __gnu_cxx::hash_map<const std::string,InterestPoint,hashstring> InterestPointMap; 00381 //! these interest points are shared by all Kinematics classes (i.e. all PostureEngines) 00382 /*! this is to reduce initialization time, but does mean one robot can't do 00383 * kinematic calculations regarding a different model robot... */ 00384 static InterestPointMap * ips; 00385 }; 00386 00387 //! a global instance of Kinematics, joint values reference those of WorldState so users can easily query the current spatial locations of joints 00388 extern Kinematics * kine; 00389 00390 /*! @file 00391 * @brief Describes Kinematics, which provides access to the mathematical functionality of the roboop package using Tekkotsu data structures 00392 * @author ejt (Creator) 00393 * 00394 * $Author: ejt $ 00395 * $Name: tekkotsu-4_0 $ 00396 * $Revision: 1.38 $ 00397 * $State: Exp $ 00398 * $Date: 2007/11/11 23:57:23 $ 00399 */ 00400 00401 #endif |
Tekkotsu v4.0 |
Generated Thu Nov 22 00:54:53 2007 by Doxygen 1.5.4 |