[Personalrobots-commit] SF.net SVN: personalrobots: [78] pkg/trunk/libTF/simple

[<tf...@us...>]

Revision: 78
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=78&view=rev
Author:   tfoote
Date:     2008-04-02 18:06:55 -0700 (Wed, 02 Apr 2008)

Log Message:
-----------
adding dhparam into RefFrame methods

Modified Paths:
--------------
    pkg/trunk/libTF/simple/libTF.cc
    pkg/trunk/libTF/simple/libTF.hh

Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc	2008-04-02 23:56:19 UTC (rev 77)
+++ pkg/trunk/libTF/simple/libTF.cc	2008-04-03 01:06:55 UTC (rev 78)
@@ -6,8 +6,10 @@
   return;
 }
 
-void RefFrame::setParams(double a,double b,double c,double d,double e,double f)
+/* Six DOF version */
+void RefFrame::setParamsXYZYPR(double a,double b,double c,double d,double e,double f)
 {
+  paramType = SIXDOF;
   params[0]=a;
   params[1]=b;
   params[2]=c;
@@ -17,19 +19,47 @@
 
 }
 
+/* DH Params version */
+void RefFrame::setParamsDH(double a,double b,double c,double d)
+{
+  paramType = DH;
+  params[0]=a;
+  params[1]=b;
+  params[2]=c;
+  params[3]=d;
+}
+
+
 NEWMAT::Matrix RefFrame::getMatrix()
 {
   NEWMAT::Matrix mMat(4,4);
-  fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
+  switch ( paramType)
+    {
+    case SIXDOF:
+      fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
+      break;
+    case DH:
+      fill_transformation_matrix_from_dh(mMat,params[0],params[1],params[2],params[3]);
+    }
   return mMat;
 }
 
 NEWMAT::Matrix RefFrame::getInverseMatrix()
 {
   NEWMAT::Matrix mMat(4,4);
-  fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
-  //todo create a fill_inverse_transform_matrix call to be more efficient
-  return mMat.i();
+  switch(paramType)
+    {
+    case SIXDOF:
+      fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
+      //todo create a fill_inverse_transform_matrix call to be more efficient
+      return mMat.i();
+      break;
+    case DH:
+      fill_transformation_matrix_from_dh(mMat,params[0],params[1],params[2],params[3]);
+      //todo create a fill_inverse_transform_matrix call to be more efficient
+      return mMat.i();
+      break;
+    }
 }
 
 
@@ -53,7 +83,7 @@
     frames[frameID] = new RefFrame();
   
   getFrame(frameID)->setParent(parentID);
-  getFrame(frameID)->setParams(a,b,c,d,e,f);
+  getFrame(frameID)->setParamsXYZYPR(a,b,c,d,e,f);
 }
 
 

Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh	2008-04-02 23:56:19 UTC (rev 77)
+++ pkg/trunk/libTF/simple/libTF.hh	2008-04-03 01:06:55 UTC (rev 78)
@@ -13,13 +13,14 @@
 
   /* The type of frame
    * This determines how many different parameters to expect to be updated, versus fixed */
-  //  static enum FrameType {SIXDOF, DH //how to tell which mode it's in  //todo add this process i'm going to start with 6dof only
+  enum ParamTypeEnum {SIXDOF, DH };//how to tell which mode it's in  //todo add this process i'm going to start with 6dof only
 
   /* Constructor */
   RefFrame();
 
   /* Set the parameters for this frame */
-  void setParams(double, double, double, double, double, double);
+  void setParamsXYZYPR(double, double, double, double, double, double);
+  void setParamsDH(double, double, double, double);
   
   /* Get the parent node */
   inline unsigned int getParent(){return parent;};
@@ -33,7 +34,8 @@
   /* Generate and return the transform associated with getting out of this frame.  */
   NEWMAT::Matrix getInverseMatrix();
 private:
-  /* Storage of the parametsrs */
+  /* Storage of the parametsrs 
+   * NOTE: Depending on if this is a 6dof or DH parameter the storage will be different. */
   double params[6];
 
   /* A helper function to build a homogeneous transform based on 6dof parameters */
@@ -42,12 +44,14 @@
 					  double pitch, double roll);
 
   /* A helper function to build a homogeneous transform based on DH parameters */
-  bool fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
+  static bool fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
 					  double length, double distance, double alpha);
 
   /* Storage of the parent */
   unsigned int parent;
 
+  ParamTypeEnum paramType;
+
 };
 
 class TransformReference


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.