Tekkotsu 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()
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