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 * 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(x,y,camera_ray(1),camera_ray(2),camera_ray(3)); 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() : chains(), chainMaps() { 00119 init(); 00120 } 00121 00122 //!Destructor 00123 virtual ~Kinematics() {} 00124 00125 00126 00127 //! Returns a matrix for transforming from link frame @a j to base frame 00128 /*! @param[in] link the output offset, see class notes for values */ 00129 NEWMAT::ReturnMatrix linkToBase(unsigned int link); 00130 00131 //! Returns a matrix for transforming from joint frame @a j to base frame 00132 /*! @param[in] joint the output offset, see class notes for values */ 00133 NEWMAT::ReturnMatrix jointToBase(unsigned int joint); 00134 00135 //! Returns a matrix for transforming from the base frame to link @a j frame 00136 /*! @param[in] link the output offset, see class notes for values */ 00137 NEWMAT::ReturnMatrix baseToLink(unsigned int link); 00138 00139 //! Returns a matrix for transforming from the base frame to joint @a j frame 00140 /*! @param[in] joint the output offset, see class notes for values */ 00141 NEWMAT::ReturnMatrix baseToJoint(unsigned int joint); 00142 00143 //! Returns a matrix for transforming from link @a ij to link @a oj 00144 /*! @param[in] iL the output offset to convert from, see class notes for values 00145 * @param[in] oL the output offset to convert to, see class notes for values */ 00146 NEWMAT::ReturnMatrix linkToLink(unsigned int iL, unsigned int oL); 00147 00148 //! Returns a matrix for transforming from link frame @a ij to joint frame @a oj 00149 /*! @param[in] iL the output offset to convert from, see class notes for values 00150 * @param[in] oJ the output offset to convert to, see class notes for values */ 00151 NEWMAT::ReturnMatrix linkToJoint(unsigned int iL, unsigned int oJ); 00152 00153 //! Returns a matrix for transforming from joint frame @a ij to link frame @a oj 00154 /*! @param[in] iJ the output offset to convert from, see class notes for values 00155 * @param[in] oL the output offset to convert to, see class notes for values */ 00156 NEWMAT::ReturnMatrix jointToLink(unsigned int iJ, unsigned int oL); 00157 00158 //! Returns a matrix for transforming from joint @a ij to joint @a oj 00159 /*! @param[in] iJ the output offset to convert from, see class notes for values 00160 * @param[in] oJ the output offset to convert to, see class notes for values */ 00161 NEWMAT::ReturnMatrix jointToJoint(unsigned int iJ, unsigned int oJ); 00162 00163 00164 00165 //! Returns the location of a named point and the link it is attached to 00166 /*! @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. 00167 * @param[out] link on exit, offset of the link, or -1U if not found 00168 * @param[out] ip on exit, a homogeneous column vector of the requested point, relative to the link frame returned in @a j 00169 * 00170 * If @a name is not found, j will be -1 and ip will be all 0's. */ 00171 void getInterestPoint(const std::string& name, unsigned int& link, NEWMAT::Matrix& ip); 00172 00173 //! Returns the location of a named point, relative to any desired joint reference frame 00174 /*! @param[in] joint the desired joint reference frame to give results in 00175 * @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. 00176 * 00177 * You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs */ 00178 NEWMAT::ReturnMatrix getJointInterestPoint(unsigned int joint, const std::string& name); 00179 00180 //! Returns the location of a named point, relative to any desired reference frame 00181 /*! @param[in] link the desired link reference frame to give results in 00182 * @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. 00183 * 00184 * You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs */ 00185 NEWMAT::ReturnMatrix getLinkInterestPoint(unsigned int link, const std::string& name) { 00186 NEWMAT::ColumnVector p=jointToLink(link,link)*getJointInterestPoint(link,name); 00187 p.Release(); return p; 00188 } 00189 00190 00191 00192 //! Find the leg which is in least contact with ground (as best we can anyway) 00193 /*! This can be either based on gravity vector from accelerometer readings, 00194 * or if that may be unreliable due to being in motion, we could do some basic 00195 * balance modeling. 00196 * @return index of leg which is highest in reference to gravity vector */ 00197 LegOrder_t findUnusedLeg(const NEWMAT::ColumnVector& down); 00198 00199 //! Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)) 00200 /*! This function merely calls the other version of calculateGroundPlane with the current 00201 * gravity vector as the "down" vector. 00202 * @return vector of the form @f$p_1x + p_2y + p_3z = p_4@f$, relative to the base frame */ 00203 NEWMAT::ReturnMatrix calculateGroundPlane(); 00204 00205 //! Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)) 00206 /*! @return vector of the form @f$p_1x + p_2y + p_3z = p_4@f$, relative to the base frame */ 00207 NEWMAT::ReturnMatrix calculateGroundPlane(const NEWMAT::ColumnVector& down); 00208 00209 //! Find the point of intersection between a ray and a plane 00210 /*! @param j is the link number the ray is relative to 00211 * @param r_j is the line through the origin, in homogeneous coordinates 00212 * @param b is the link number the plane is relative to (probably BaseFrameOffset) 00213 * @param p_b represents the plane to be intersected 00214 * @param f is the link number the results should be relative to 00215 * 00216 * @a p_b should be of the form @f$p_1x + p_2y + p_3z = p_4@f$ 00217 * @return homogeneous coordinate of intersection (may be infinity) */ 00218 NEWMAT::ReturnMatrix projectToPlane(unsigned int j, const NEWMAT::ColumnVector& r_j, 00219 unsigned int b, const NEWMAT::ColumnVector& p_b, 00220 unsigned int f); 00221 00222 00223 //! A simple utility function, converts x,y,z,h to a NEWMAT::ColumnVector 00224 /*! @param[in] x the value for the first row 00225 * @param[in] y the value for the second row 00226 * @param[in] z the value for the third row 00227 * @param[in] h the value for the fourth row (defaults to 1 if not specified) 00228 * @return @f$ \left[\begin{array}{c} x\\y\\z\\h\\ \end{array}\right] @f$ */ 00229 static NEWMAT::ReturnMatrix pack(float x, float y, float z, float h=1) { 00230 NEWMAT::ColumnVector m(4); 00231 m(1)=x; m(2)=y; m(3)=z; m(4)=h; 00232 m.Release(); return m; 00233 } 00234 //! 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 00235 /*! @param[in] m the matrix to unpack (only uses first column) 00236 * @param[out] ox set to the first row of the first column of @a m, divided by fourth row 00237 * @param[out] oy set to the second row of the first column of @a m, divided by fourth row 00238 * @param[out] oz set to the third row of the first column of @a m, divided by fourth row */ 00239 static void unpack(NEWMAT::Matrix m, float& ox, float& oy, float& oz) { 00240 ox=m(1,1)/m(4,1); oy=m(2,1)/m(4,1); oz=m(3,1)/m(4,1); 00241 } 00242 //! A simple utility function, pulls the first 4 rows of the first column, stores into ox, oy, oz, oh 00243 /*! @param[in] m the matrix to unpack (only uses first column) 00244 * @param[out] ox set to the first row of the first column of @a m 00245 * @param[out] oy set to the second row of the first column of @a m 00246 * @param[out] oz set to the third row of the first column of @a m 00247 * @param[out] oh set to the fourth row of the first column of @a m */ 00248 static void unpack(NEWMAT::Matrix m, float& ox, float& oy, float& oz, float& oh) { 00249 ox=m(1,1); oy=m(2,1); oz=m(3,1); oh=m(4,1); 00250 } 00251 00252 //! returns the global ROBOOP::Config object which Kinematics classes initialize themselves from (#roconfig) 00253 static ROBOOP::Config * getConfig() { return roconfig; } 00254 00255 protected: 00256 //! Called by constructors to do basic setup - first call will read Config::motion_config::kinematics from disk, future initializes reuse static roconfig 00257 void init(); 00258 00259 //! Returns the location of a named point, relative to the link it is attached to 00260 /*! @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. 00261 * @param[out] c on exit, chain index the IP is on 00262 * @param[out] l on exit, link index the IP is on 00263 * @param[out] ip on exit, a homogeneous column vector of the requested point 00264 * 00265 * If @a name is not found, @a c and @a l will be -1 and @a ip will be all 0's. 00266 * This internal version of the function allows us to use @a c and @a l, ourselves, 00267 * but users will probably want to use the getInterestPoint(name,j,ip) version */ 00268 void getInterestPoint(const std::string& name, unsigned int& c, unsigned int& l, NEWMAT::Matrix& ip); 00269 00270 //! Called at the beginning of each function which accesses ROBOOP computations - should make sure the ROBOOP structures are up to date with Tekkotsu structures 00271 /*! This class will pull current values from WorldState, but it is expected 00272 * that subclasses (i.e. PostureEngine) will want to provide their own joint values. 00273 * Updates from link 1 through link @a l. 00274 * @param[in] c the chain to update 00275 * @param[in] l the last link to update (later links in the chain are left untouched) */ 00276 virtual void update(unsigned int c, unsigned int l); 00277 00278 //! converts a Tekkotsu output index to a chain and link 00279 /*! @param[in] tkout the tekkotsu index to lookup 00280 * @param[out] c set to the chain index that @a tkout is in 00281 * @param[out] l set to the link in @a c corresponding to @a tkout */ 00282 bool lookup(unsigned int tkout, unsigned int& c, unsigned int& l) { 00283 if(tkout>=NumReferenceFrames) { 00284 serr->printf("ERROR Kinematics: illegal link %d\n",l); 00285 return false; 00286 } 00287 if(jointMaps[tkout].chain>=chains.size()) { 00288 serr->printf("ERROR Kinematics: no chain available for link %d\n",l); 00289 return false; 00290 } 00291 c=jointMaps[tkout].chain; 00292 l=jointMaps[tkout].link; 00293 return true; 00294 } 00295 00296 //! A separate ROBOOP::Robot instantiation for each chain since ROBOOP package doesn't support branching chains (which would be rather difficult to implement well) 00297 std::vector<ROBOOP::Robot*> chains; 00298 00299 //! holds mapping for each chain's links back to the tekkotsu outputs and reference frames they represent 00300 std::vector< std::vector<unsigned int> > chainMaps; 00301 00302 //! Allows mapping from tekkotsu output index to chain and link indicies 00303 struct JointMap { 00304 JointMap() : chain(-1U), link(-1U) {} //!< constructor 00305 JointMap(unsigned int c, unsigned int l) : chain(c), link(l) {} //!< constructor 00306 unsigned int chain; //!< the chain index 00307 unsigned int link; //!< the link index 00308 }; 00309 //! holds the position and attached link of a given interest point 00310 struct InterestPoint { 00311 InterestPoint() : x(), y(), z(), chain(), link() {} //!< constructor 00312 InterestPoint(float x_, float y_, float z_, unsigned int chain_, unsigned int link_) 00313 : x(x_), y(y_), z(z_), chain(chain_), link(link_) {} //!< constructor 00314 float x; //!< x value 00315 float y; //!< y value 00316 float z; //!< z value 00317 unsigned int chain; //!< chain containing @a link 00318 int link; //!< link in @a chain 00319 }; 00320 00321 //! holds mapping from tekkotsu output index to chain and link indicies 00322 JointMap jointMaps[NumReferenceFrames]; 00323 00324 //! cache of the configuration of the robot for rapid initialization (so we don't have to re-read from disk) 00325 static ROBOOP::Config * roconfig; 00326 //! allows us to use the STL hash_map with strings 00327 struct hashstring { 00328 //! hashes a string by multiplying current total by 5, add new character 00329 /*! not my idea, this is what the STL library does for char*, i've just reimplemented it for strings */ 00330 size_t operator()(const string& s) const { 00331 unsigned long h=0; 00332 for(string::size_type x=s.size(); x!=0; x--) 00333 h=h*5+s[x]; 00334 return (size_t)h; 00335 } 00336 }; 00337 //! we'll be using the hash_map to store named interest points 00338 typedef __gnu_cxx::hash_map<const string,InterestPoint,hashstring> InterestPointMap; 00339 //! these interest points are shared by all Kinematics classes (i.e. all PostureEngines) 00340 /*! this is to reduce initialization time, but does mean one robot can't do 00341 * kinematic calculations regarding a different model robot... */ 00342 static InterestPointMap * ips; 00343 }; 00344 00345 //! a global instance of Kinematics, joint values reference those of WorldState so users can easily query the current spatial locations of joints 00346 extern Kinematics * kine; 00347 00348 /*! @file 00349 * @brief Describes Kinematics, which provides access to the mathematical functionality of the roboop package using Tekkotsu data structures 00350 * @author ejt (Creator) 00351 * 00352 * $Author: ejt $ 00353 * $Name: tekkotsu-2_2_2 $ 00354 * $Revision: 1.30 $ 00355 * $State: Exp $ 00356 * $Date: 2004/12/23 01:47:07 $ 00357 */ 00358 00359 #endif |
Tekkotsu v2.2.2 |
Generated Tue Jan 4 15:43:14 2005 by Doxygen 1.4.0 |