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

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

Revision: 115
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=115&view=rev
Author:   tfoote
Date:     2008-04-16 21:30:14 -0700 (Wed, 16 Apr 2008)

Log Message:
-----------
updating test program to work with new time.  And adding DHparams input ticket:16 and testing basic functionality.

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

Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc	2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/libTF.cc	2008-04-17 04:30:14 UTC (rev 115)
@@ -92,7 +92,19 @@
   getFrame(frameID)->setParamsEulers(a,b,c,d,e,f,time);
 }
 
+void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d, unsigned long long time)
+{
+  if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES || frameID == NO_PARENT || frameID == ROOT_FRAME)
+    throw InvalidFrame;
+  
+  if (frames[frameID] == NULL)
+    frames[frameID] = new RefFrame();
+  
+  getFrame(frameID)->setParent(parentID);
+  getFrame(frameID)->setParamsDH(a,b,c,d,time);
+}
 
+
 NEWMAT::Matrix TransformReference::get(unsigned int target_frame, unsigned int source_frame, unsigned long long time)
 {
   NEWMAT::Matrix myMat(4,4);

Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh	2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/libTF.hh	2008-04-17 04:30:14 UTC (rev 115)
@@ -90,6 +90,7 @@
   /********** Mutators **************/
   /* Set a new frame or update an old one. */
   void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double,unsigned long long time);
+  void set(unsigned int framid, unsigned int parentid, double,double,double,double,unsigned long long time);
   // Possible exceptions TransformReference::LookupException
 
   /*********** Accessors *************/

Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc	2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/main.cc	2008-04-17 04:30:14 UTC (rev 115)
@@ -18,14 +18,16 @@
 
   
   //Fill in some transforms
-  mTR.set(10,2,1,1,1,dyaw,dp,dr,atime);
+  //  mTR.set(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
+  mTR.set(10,2,1,1,1,dyaw,atime);
   mTR.set(2,3,1,1,1,dyaw,dp,dr+1,atime-1000);
   mTR.set(2,3,1,1,1,dyaw,dp,dr-1,atime+1000);
   mTR.set(3,5,dx,dy,dz,dyaw,dp,dr,atime);
   mTR.set(5,1,dx,dy,dz,dyaw,dp,dr,atime);
   mTR.set(6,5,dx,dy,dz,dyaw,dp,dr,atime);
   mTR.set(7,6,1,1,1,dyaw,dp,dr,atime);
-  mTR.set(8,7,1,1,1,dyaw,dp,dr,atime);
+  mTR.set(8,7,1,1,1,dyaw,atime);
+  //  mTR.set(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
   
   
   //Demonstrate InvalidFrame LookupException

Modified: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc	2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/testQuat.cc	2008-04-17 04:30:14 UTC (rev 115)
@@ -37,13 +37,13 @@
 
 int main()
 {
-  Quaternion3D myquat(1,1,1,1,1,1,1);
+  Quaternion3D myquat(1,1,1,1,1,1,1,1000101010);
 
-  Quaternion3D my2ndquat(myquat.asMatrix());
+  Quaternion3D my2ndquat(myquat.asMatrix(1000101010),1000101010);
   std::cout << "Quat1"<<std::endl;
-  myquat.printMatrix();
+  myquat.printMatrix(1000101010);
   std::cout << "Quat2"<<std::endl;
-  my2ndquat.printMatrix();
+  my2ndquat.printMatrix(1000101010);
 
   return 0;
 }


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