Thread: [Personalrobots-commit] SF.net SVN: personalrobots: [32] pkg/trunk/libTF

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 32
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=32&view=rev
Author:   tfoote
Date:     2008-03-25 18:32:43 -0700 (Tue, 25 Mar 2008)

Log Message:
-----------
An initial operational demo of the transform library transforming through a tree

Added Paths:
-----------
    pkg/trunk/libTF/simple/
    pkg/trunk/libTF/simple/Makefile
    pkg/trunk/libTF/simple/libTF.cc
    pkg/trunk/libTF/simple/libTF.hh
    pkg/trunk/libTF/simple/main.cc

Property Changed:
----------------
    pkg/trunk/libTF/


Property changes on: pkg/trunk/libTF
___________________________________________________________________
Name: svk:merge
   + 5d0af4bb-b190-461e-89ab-f239232fb17e:/zug/libtf:633

Added: pkg/trunk/libTF/simple/Makefile
===================================================================

--- pkg/trunk/libTF/simple/Makefile	                        (rev 0)
+++ pkg/trunk/libTF/simple/Makefile	2008-03-26 01:32:43 UTC (rev 32)
@@ -0,0 +1,13 @@
+.PHONY=all clean test
+
+
+all: test
+
+
+
+test: main.cc libTF.o
+	g++ -O2 $^ -o test -lnewmat
+
+
+clean:
+	rm -rf *~ test *.o
\ No newline at end of file

Added: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc	                        (rev 0)
+++ pkg/trunk/libTF/simple/libTF.cc	2008-03-26 01:32:43 UTC (rev 32)
@@ -0,0 +1,193 @@
+#include "libTF.hh"
+
+RefFrame::RefFrame() :
+  parent(0),
+  active(false)
+{
+  return;
+}
+
+void RefFrame::setParams(double a,double b,double c,double d,double e,double f)
+{
+  params[0]=a;
+  params[1]=b;
+  params[2]=c;
+  params[3]=d;
+  params[4]=e;
+  params[5]=f;
+
+}
+
+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]);
+  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();
+}
+
+
+TransformReference::TransformReference():
+  mMat(4,4)
+{
+  
+
+  return;
+}
+
+void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f)
+{
+  frames[frameID].setParent(parentID);
+  frames[frameID].setParams(a,b,c,d,e,f);
+}
+
+
+NEWMAT::Matrix TransformReference::get(unsigned int target_frame, unsigned int source_frame)
+{
+  NEWMAT::Matrix myMat(4,4);
+  TransformLists lists = lookUpList(target_frame, source_frame);
+  myMat = computeTransformFromList(lists);
+  //  std::cout << myMat;
+  return myMat;
+}
+
+
+
+
+bool RefFrame::fill_transformation_matrix(NEWMAT::Matrix & matrix, double ax,
+                                                   double ay, double az, double yaw,
+                                                   double pitch, double roll)
+{
+  double ca = cos(yaw);
+  double sa = sin(yaw);
+  double cb = cos(pitch);
+  double sb = sin(pitch);
+  double cg = cos(roll);
+  double sg = sin(roll);
+  double sbsg = sb*sg;
+  double sbcg = sb*cg;
+
+
+  double* matrix_pointer = matrix.Store();
+  if (matrix.Storage() != 16)
+    return false;
+
+  matrix_pointer[0] =  ca*cb;
+  matrix_pointer[1] = (ca*sbsg)-(sa*cg);
+  matrix_pointer[2] = (ca*sbcg)+(sa*sg);
+  matrix_pointer[3] = ax;
+  matrix_pointer[4] = sa*cb;
+  matrix_pointer[5] = (sa*sbsg)+(ca*cg);
+  matrix_pointer[6] = (sa*sbcg)-(ca*sg);
+  matrix_pointer[7] = ay;
+  matrix_pointer[8] = -sb;
+  matrix_pointer[9] = cb*sg;
+  matrix_pointer[10] = cb*cg;
+  matrix_pointer[11] = az;
+  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;
+
+  //  std::vector<unsigned int> tList;
+  //  std::vector<unsigned int> sList;
+
+  //  std::vector<unsigned int> retVec;
+
+  unsigned int frame = target_frame;
+  while (frame != 0)
+    {
+      if (frames[frame].getParent() > 100) exit(-1); //todo cleanup
+      mTfLs.inverseTransforms.push_back(frame);
+      //   std::cout <<"Frame: " << frame <<std::endl;
+      frame = frames[frame].getParent();
+    }
+  mTfLs.inverseTransforms.push_back(0);
+  
+  frame = source_frame;
+  while (frame != 0)
+    {
+      if (frames[frame].getParent() > 100) exit(-1); //todo cleanup
+      mTfLs.forwardTransforms.push_back(frame);
+      frame = frames[frame].getParent();
+    }
+  mTfLs.forwardTransforms.push_back(0);
+  
+
+  //todo fixme throw something
+  //  if (tList.size() <= 1) exit(-1);
+  // if (sList.size() <= 1) exit(-1);   // I think below will catch all these anyway
+  
+  while (mTfLs.inverseTransforms.back() == mTfLs.forwardTransforms.back())
+    {
+      //      std::cout << "removing " << mTfLs.inverseTransforms.back() << std::endl;
+      mTfLs.inverseTransforms.pop_back();
+      mTfLs.forwardTransforms.pop_back();
+    }
+  
+  return mTfLs;
+
+}
+
+NEWMAT::Matrix TransformReference::computeTransformFromList(TransformLists lists)
+{
+  NEWMAT::Matrix retMat(4,4);
+  retMat << 1 << 0 << 0 << 0
+	 << 0 << 1 << 0 << 0
+	 << 0 << 0 << 1 << 0
+	 << 0 << 0 << 0 << 1;
+  
+  for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
+    {
+      retMat *= frames[lists.inverseTransforms[i]].getInverseMatrix();
+      //      std::cout <<"Multiplying by " << std::endl << frames[lists.inverseTransforms[i]].getInverseMatrix() << std::endl; 
+      //std::cout <<"Result "<<std::endl << retMat << std::endl;
+   }
+  for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++) 
+    {
+      retMat *= frames[lists.forwardTransforms[lists.forwardTransforms.size() -1 - i]].getMatrix(); //Do this list backwards for it was generated traveling the wrong way
+      //      std::cout <<"Multiplying by "<<std::endl << frames[lists.forwardTransforms[i]].getMatrix() << std::endl;
+      //std::cout <<"Result "<<std::endl << retMat << std::endl;
+    }
+
+  return retMat;
+}
+
+
+void TransformReference::view(unsigned int target_frame, unsigned int source_frame)
+{
+  TransformLists lists = lookUpList(target_frame, source_frame);
+
+  std::cout << "Inverse Transforms:" <<std::endl;
+  for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
+    {
+      std::cout << lists.inverseTransforms[i];
+      //      retMat *= frames[lists.inverseTransforms[i]].getInverseMatrix();
+    }
+  std::cout << std::endl;
+
+  std::cout << "Forward Transforms: "<<std::endl ;
+  for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++) 
+    {
+      std::cout << lists.forwardTransforms[i];
+      //      retMat *= frames[lists.inverseTransforms[lists.forwardTransforms.size() -1 - i]].getMatrix(); //Do this list backwards for it was generated traveling the wrong way
+    }
+  std::cout << std::endl;
+  
+}

Added: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh	                        (rev 0)
+++ pkg/trunk/libTF/simple/libTF.hh	2008-03-26 01:32:43 UTC (rev 32)
@@ -0,0 +1,53 @@
+#ifndef LIBTF_HH
+#define LIBTF_HH
+#include <iostream>
+#include <iomanip>
+#include <newmat/newmat.h>
+#include <newmat/newmatio.h>
+#include <math.h>
+#include <vector>
+
+class RefFrame
+{
+public:
+  //  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
+  RefFrame();
+  void setParams(double, double, double, double, double, double);
+  inline unsigned int getParent(){return parent;};
+  inline void setParent(unsigned int parentID){parent = parentID;};
+  NEWMAT::Matrix getMatrix();
+  NEWMAT::Matrix getInverseMatrix();
+private:
+  double params[6];
+  static  bool fill_transformation_matrix(NEWMAT::Matrix& matrix_pointer, double ax,
+					  double ay, double az, double yaw,
+					  double pitch, double roll);
+  unsigned int parent;
+  bool active;
+
+};
+
+class TransformReference
+{
+ public:
+  void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double);
+  NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame);
+
+  TransformReference();
+  void view(unsigned int target_frame, unsigned int source_frame);
+ private:
+  double rD[6];
+  NEWMAT::Matrix mMat;
+  RefFrame frames[100];
+
+  typedef struct 
+  {
+    std::vector<unsigned int> inverseTransforms;
+    std::vector<unsigned int> forwardTransforms;
+  } TransformLists;
+
+  TransformLists  lookUpList(unsigned int target_frame, unsigned int source_frame);
+  NEWMAT::Matrix computeTransformFromList(TransformLists list);
+
+};
+#endif //LIBTF_HH

Added: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc	                        (rev 0)
+++ pkg/trunk/libTF/simple/main.cc	2008-03-26 01:32:43 UTC (rev 32)
@@ -0,0 +1,104 @@
+#include "libTF.hh"
+#include <time.h>
+
+using namespace std;
+
+int main(void)
+{
+  double dx,dy,dz,dyaw,dp,dr;
+  TransformReference mTR;
+  
+      
+  //  while (true)
+    {
+      /*      cout << "delta x:";
+      cin >> dx;
+      cout << "delta y:";
+      cin >> dy;
+      cout << "delta z:";
+      cin >> dz;
+      cout << "delta yaw:";
+      cin >> dyaw;
+      cout << "delta pitch:";
+      cin >> dp;
+      cout << "delta roll:";
+      cin >> dr;
+      */
+      dx = dy= dz = 0;
+      dyaw = dp = dr = .1;
+           
+      
+      
+      mTR.set(1,2,1,1,1,dyaw,dp,dr);
+      mTR.set(2,3,1,1,1,dyaw,dp,dr);
+      mTR.set(3,5,dx,dy,dz,dyaw,dp,dr);
+      mTR.set(5,0,dx,dy,dz,dyaw,dp,dr);
+      mTR.set(6,5,dx,dy,dz,dyaw,dp,dr);
+      mTR.set(7,6,1,1,1,dyaw,dp,dr);
+      mTR.set(8,7,1,1,1,dyaw,dp,dr);
+      mTR.view(1,8);
+
+
+      std::cout <<"Calling get(1,8)"<<std::endl;
+      //      NEWMAT::Matrix mat = mTR.get(1,1);
+      NEWMAT::Matrix mat = mTR.get(1,8);
+
+      std::cout << "Result" << std::endl << mat<< std::endl;
+#ifdef DONOTUSE
+
+      //      NEWMAT::Matrix mat(4,4);
+      NEWMAT::Matrix MatI(4,4);
+      NEWMAT::Matrix MatI2(4,4);
+      
+
+      //      double vec[] = {dx,dy,dz,dyaw,dp,dr};
+      // mat <<vec;
+      double init_time = (double)clock()/(double) CLOCKS_PER_SEC;
+      int iterations = 1;
+      for (int i = 0; i < iterations; i++)
+	{
+	  MatI = mat.i();
+	}
+      std::cout <<"Straight Inverse" <<std::endl<< MatI;
+      std::cout << "This should be Identity if Inverse is right: "<<std::endl << MatI * mat;
+      std::cout << "Time Elapsed for "<<iterations<<"iterations is:"<<(double)clock()/(double) CLOCKS_PER_SEC - init_time <<std::endl;
+      std::cout << "Net: "<<((double)clock()/(double) CLOCKS_PER_SEC -init_time)/(double)iterations <<std::endl;
+
+
+      NEWMAT::Matrix trans_mask(4,4);
+      trans_mask << 0 << 0 << 0 << -1
+		 << 0 << 0 << 0 << -1
+		 << 0 << 0 << 0 << -1
+		 << 0 << 0 << 0 << 1;
+
+      NEWMAT::Matrix trans_fix(4,4);
+      trans_fix << 1 << 0 << 0 << 0
+		<< 0 << 1 << 0 << 0
+		<< 0 << 0 << 1 << 0
+		<< 0 << 0 << 0 << 0;
+      
+
+      NEWMAT::Matrix rot_mask(4,4);
+      rot_mask << 1 << 1 << 1 << 0
+	       << 1 << 1 << 1 << 0
+	       << 1 << 1 << 1 << 0
+	       << 0 << 0 << 0 << 1;
+
+      init_time = (double)clock()/(double) CLOCKS_PER_SEC;
+      for (int i = 0; i < iterations; i++)
+	{
+	  MatI2 =   SP(mat, rot_mask).t();
+	  MatI2 *= (SP(mat,trans_mask) + trans_fix);
+	}
+      std::cout <<"My Inverse:" <<std::endl<< MatI2;
+      std::cout << "Product: " <<std::endl<< MatI2 * mat;
+      std::cout << "Time Elapsed for method 2 "<<iterations<<" iterations is:"<<(double)clock()/(double) CLOCKS_PER_SEC - init_time <<std::endl;
+      std::cout << "Net: "<<((double)clock()/(double) CLOCKS_PER_SEC - init_time) /(double)iterations <<std::endl;
+      
+
+      std::cout <<"difference: "<<std::endl<<MatI - MatI2;
+#endif //DONOTUSE
+
+    }
+  return 0;
+};


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




Thread: [Personalrobots-commit] SF.net SVN: personalrobots: [32] pkg/trunk/libTF

personalrobots-commit