Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Kinematics.h

Go 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() : 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_4_1 $
00354  * $Revision: 1.31 $
00355  * $State: Exp $
00356  * $Date: 2005/01/14 21:20:05 $
00357  */
00358 
00359 #endif

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:47 2005 by Doxygen 1.4.4