Homepage Demos Overview Downloads Tutorials 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  *  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