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

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

Revision: 77
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=77&view=rev
Author:   tfoote
Date:     2008-04-02 16:56:19 -0700 (Wed, 02 Apr 2008)

Log Message:
-----------
adding a homogeneous transform from dh params function

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 22:26:51 UTC (rev 76)
+++ pkg/trunk/libTF/simple/libTF.cc	2008-04-02 23:56:19 UTC (rev 77)
@@ -107,8 +107,41 @@
   return true;
 };
 
+// Math from http://en.wikipedia.org/wiki/Robotics_conventions
+bool RefFrame::fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
+						 double length, double distance, double alpha)
+{
+  double ca = cos(alpha);
+  double sa = sin(alpha);
+  double ct = cos(theta);
+  double st = sin(theta);
 
+  double* matrix_pointer = matrix.Store();
+  if (matrix.Storage() != 16)
+    return false;
 
+  matrix_pointer[0] =  ct;
+  matrix_pointer[1] = -st*ca;
+  matrix_pointer[2] = st*sa;
+  matrix_pointer[3] = distance * ct;
+  matrix_pointer[4] = st;
+  matrix_pointer[5] = ct*ca;
+  matrix_pointer[6] = -ct*sa;
+  matrix_pointer[7] = distance*st;
+  matrix_pointer[8] = 0;
+  matrix_pointer[9] = sa;
+  matrix_pointer[10] = ca;
+  matrix_pointer[11] = length;
+  matrix_pointer[12] = 0.0;
+  matrix_pointer[13] = 0.0;
+  matrix_pointer[14] = 0.0;
+  matrix_pointer[15] = 1.0;
+
+  return true;
+};
+
+
+
 TransformReference::TransformLists TransformReference::lookUpList(unsigned int target_frame, unsigned int source_frame)
 {
   TransformLists mTfLs;

Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh	2008-04-02 22:26:51 UTC (rev 76)
+++ pkg/trunk/libTF/simple/libTF.hh	2008-04-02 23:56:19 UTC (rev 77)
@@ -40,6 +40,11 @@
   static  bool fill_transformation_matrix(NEWMAT::Matrix& matrix_pointer, double ax,
 					  double ay, double az, double yaw,
 					  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,
+					  double length, double distance, double alpha);
+
   /* Storage of the parent */
   unsigned int parent;
 


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