Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [154] pkg/trunk/libTF
|
From: <jl...@us...> - 2008-04-22 20:31:03
|
Revision: 154
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=154&view=rev
Author: jleibs
Date: 2008-04-22 13:31:08 -0700 (Tue, 22 Apr 2008)
Log Message:
-----------
All done shuffling files to new locations.
Added Paths:
-----------
pkg/trunk/libTF/include/libTF/Quaternion3D.hh
pkg/trunk/libTF/include/libTF/libTF.hh
pkg/trunk/libTF/src/test/main.cc
pkg/trunk/libTF/src/test/testQuat.cc
Removed Paths:
-------------
pkg/trunk/libTF/src/simple/Quaternion3D.hh
pkg/trunk/libTF/src/simple/libTF.hh
pkg/trunk/libTF/src/simple/main.cc
pkg/trunk/libTF/src/simple/testQuat.cc
Copied: pkg/trunk/libTF/include/libTF/Quaternion3D.hh (from rev 153, pkg/trunk/libTF/src/simple/Quaternion3D.hh)
===================================================================
--- pkg/trunk/libTF/include/libTF/Quaternion3D.hh (rev 0)
+++ pkg/trunk/libTF/include/libTF/Quaternion3D.hh 2008-04-22 20:31:08 UTC (rev 154)
@@ -0,0 +1,174 @@
+// Software License Agreement (BSD License)
+//
+// Copyright (c) 2008, Willow Garage, Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following
+// disclaimer in the documentation and/or other materials provided
+// with the distribution.
+// * Neither the name of the Willow Garage nor the names of its
+// contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+#ifndef QUATERNION3D_HH
+#define QUATERNION3D_HH
+
+#include <iostream>
+#include <newmat/newmat.h>
+#include <newmat/newmatio.h>
+#include <math.h>
+#include <pthread.h>
+#include <sys/time.h>
+class Euler3D {
+public:
+ //Constructor
+ Euler3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
+
+ //Storage
+ double x,y,z,yaw,pitch,roll;
+};
+
+
+class Quaternion3D {
+
+public:
+ // Storage class
+ class Quaternion3DStorage
+ {
+ public:
+
+ /** Unary operators preserve timestamps
+ * Binary operators destroy timestamps */
+
+ // Utility functions to normalize and get magnitude.
+ void Normalize();
+ double getMagnitude();
+ Quaternion3DStorage & operator=(const Quaternion3DStorage & input);
+
+
+ /* accessors */
+ NEWMAT::Matrix asMatrix();
+
+ /* Internal Data */
+ double xt, yt, zt, xr, yr, zr, w;
+ unsigned long long time;
+ };
+
+ /** Constructors **/
+ // Standard constructor
+ Quaternion3D();
+
+ /** Mutators **/
+ // Set the values manually
+ void fromQuaternion(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
+ //Set the values from a matrix
+ void fromMatrix(const NEWMAT::Matrix& matIn, unsigned long long time);
+ // Set the values using Euler angles
+ void fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll, unsigned long long time);
+ // Set the values using DH Parameters
+ void fromDH(double length, double alpha, double offset, double theta, unsigned long long time);
+
+
+ /** Interpolated Accessors **/
+ // Return a Quaternion
+ Quaternion3D::Quaternion3DStorage getQuaternion(unsigned long long time);
+ // Return a Matrix
+ NEWMAT::Matrix getMatrix(unsigned long long time);
+ // Return the inverse matrix
+ NEWMAT::Matrix getInverseMatrix(unsigned long long time);
+
+ /**** Utility Functions ****/
+ // this is a function to return the current time in microseconds from the beginning of 1970
+ static unsigned long long Qgettime(void);
+
+ // Convert DH Parameters to a Homogeneous Transformation Matrix
+ static NEWMAT::Matrix matrixFromDH(double length, double alpha, double offset, double theta);
+ // Convert Euler Angles to a Homogeneous Transformation Matrix
+ static NEWMAT::Matrix matrixFromEuler(double ax,
+ double ay, double az, double yaw,
+ double pitch, double roll);
+
+ //Print as a matrix
+ void printMatrix(unsigned long long time); //Not a critical part either
+ void printStorage(const Quaternion3DStorage &storage); //Do i need this now that i've got the ostream method??
+
+
+private:
+ /**** Linked List stuff ****/
+ static const long long MAX_STORAGE_TIME = 1000000000; // max of 100 seconds storage
+
+ struct data_LL{
+ Quaternion3DStorage data;
+ data_LL * next;
+ data_LL * previous;
+ };
+
+ bool getValue(Quaternion3DStorage& buff, unsigned long long time, long long &time_diff);
+ void add_value(const Quaternion3DStorage&);//todo fixme finish implementing this
+
+
+
+ // insert a node into the sorted linked list
+ void insertNode(const Quaternion3DStorage & );
+ // prune data older than max_storage_time from the list
+ void pruneList();
+
+ //Find the closest two points in the list
+ //Return the distance to the closest one
+ int findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, const unsigned long long target_time, long long &time_diff);
+
+ //Interpolate between two nodes and return the interpolated value
+ // This must always take two valid points!!!!!!
+ // Only Cpose version implemented
+ void interpolate(const Quaternion3DStorage &one, const Quaternion3DStorage &two, const unsigned long long target_time, Quaternion3DStorage& output);
+
+ //Used by interpolate to interpolate between double values
+ double interpolateDouble(const double, const unsigned long long, const double, const unsigned long long, const unsigned long long);
+
+ //How long to cache incoming values
+ unsigned long long max_storage_time;
+
+ //A mutex to prevent linked list collisions
+ pthread_mutex_t linked_list_mutex;
+
+ //Pointers for the start and end of a sorted linked list.
+ data_LL* first;
+ data_LL* last;
+
+
+
+};
+
+
+
+//A global ostream overload for displaying storage
+std::ostream & operator<<(std::ostream& mystream,const Quaternion3D::Quaternion3DStorage & storage);
+
+
+
+
+
+
+
+
+#endif //QUATERNION3D_HH
Copied: pkg/trunk/libTF/include/libTF/libTF.hh (from rev 153, pkg/trunk/libTF/src/simple/libTF.hh)
===================================================================
--- pkg/trunk/libTF/include/libTF/libTF.hh (rev 0)
+++ pkg/trunk/libTF/include/libTF/libTF.hh 2008-04-22 20:31:08 UTC (rev 154)
@@ -0,0 +1,172 @@
+//Software License Agreement (BSD License)
+
+//Copyright (c) 2008, Willow Garage, Inc.
+//All rights reserved.
+
+//Redistribution and use in source and binary forms, with or without
+//modification, are permitted provided that the following conditions
+//are met:
+
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following
+// disclaimer in the documentation and/or other materials provided
+// with the distribution.
+// * Neither the name of the Willow Garage nor the names of its
+// contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+
+//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+//COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+//INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+//BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+//CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+//LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef LIBTF_HH
+#define LIBTF_HH
+#include <iostream>
+#include <iomanip>
+#include <newmat/newmat.h>
+#include <newmat/newmatio.h>
+#include <math.h>
+#include <vector>
+#include <sstream>
+
+#include "Quaternion3D.hh"
+
+/** RefFrame *******
+ * An instance of this class is created for each frame in the system.
+ * This class natively handles the relationship between frames.
+ *
+ * The derived class Quaternion3D provides a buffered history of positions
+ * with interpolation.
+ */
+
+class RefFrame: public Quaternion3D
+{
+public:
+
+ /* Constructor */
+ RefFrame();
+
+ /* Get the parent node */
+ inline unsigned int getParent(){return parent;};
+
+ /* Return tha parent node */
+ inline void setParent(unsigned int parentID){parent = parentID;};
+private:
+
+ /* Storage of the parent */
+ unsigned int parent;
+
+};
+
+/*** Transform Reference ***********
+ * This class provides a simple interface to allow recording and lookup of
+ * relationships between arbitrary frames of the system.
+ *
+ * The positions of frames over time must be pushed in.
+ *
+ * It will provide interpolated positions out given a time argument.
+ */
+
+class TransformReference
+{
+public:
+ /************* Constants ***********************/
+ static const unsigned int ROOT_FRAME = 1; //Hard Value for ROOT_FRAME
+ static const unsigned int NO_PARENT = 0; //Value for NO_PARENT
+
+ static const unsigned int MAX_NUM_FRAMES = 100; /* The maximum number of frames possible */
+ static const unsigned int MAX_GRAPH_DEPTH = 100; /* The maximum number of times to descent before determining that graph has a loop. */
+
+ /* Constructor */
+ TransformReference();
+
+ /********** Mutators **************/
+ /* Set a new frame or update an old one. */
+ /* Use Euler Angles. X forward, Y to the left, Z up, Yaw about Z, pitch about new Y, Roll about new X */
+ void setWithEulers(unsigned int framid, unsigned int parentid, double x, double y, double z, double yaw, double pitch, double roll, unsigned long long time);
+ /* Using DH Parameters */
+ // Conventions from http://en.wikipedia.org/wiki/Robotics_conventions
+ void setWithDH(unsigned int framid, unsigned int parentid, double length, double alpha, double offset, double theta, unsigned long long time);
+ // Possible exceptions TransformReference::LookupException
+
+ /*********** Accessors *************/
+
+ /* Get the transform between two frames by frame ID. */
+ NEWMAT::Matrix getMatrix(unsigned int target_frame, unsigned int source_frame, unsigned long long time);
+ // Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
+ // TransformReference::MaxDepthException
+
+ /* Debugging function that will print to std::cout the transformation matrix */
+ std::string viewChain(unsigned int target_frame, unsigned int source_frame);
+ // Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
+ // TransformReference::MaxDepthException
+
+
+
+
+
+
+ /************ Possible Exceptions ****************************/
+
+ /* An exception class to notify of bad frame number */
+ class LookupException : public std::exception
+ {
+ public:
+ virtual const char* what() const throw() { return "InvalidFrame"; }
+ } InvalidFrame;
+
+ /* An exception class to notify of no connection */
+ class ConnectivityException : public std::exception
+ {
+ public:
+ virtual const char* what() const throw() { return "No connection between frames"; }
+ private:
+ } NoFrameConnectivity;
+
+ /* An exception class to notify that the search for connectivity descended too deep. */
+ class MaxDepthException : public std::exception
+ {
+ public:
+ virtual const char* what() const throw() { return "Search exceeded max depth. Probably a loop in the tree."; }
+ private:
+ } MaxSearchDepth;
+
+private:
+ /******************** Internal Storage ****************/
+
+ /* The pointers to potential frames that the tree can be made of.
+ * The frames will be dynamically allocated at run time when set the first time. */
+ RefFrame* frames[MAX_NUM_FRAMES];
+
+
+ /* This struct is how the list of transforms are stored before being passed to computeTransformFromList. */
+ typedef struct
+ {
+ std::vector<unsigned int> inverseTransforms;
+ std::vector<unsigned int> forwardTransforms;
+ } TransformLists;
+
+ /************************* Internal Functions ****************************/
+
+ /* An accessor to get a frame, which will throw an exception if the frame is no there. */
+ inline RefFrame* getFrame(unsigned int frame_number) { if (frames[frame_number] == NULL) throw InvalidFrame; else return frames[frame_number];};
+
+ /* Find the list of connected frames necessary to connect two different frames */
+ TransformLists lookUpList(unsigned int target_frame, unsigned int source_frame);
+
+ /* Compute the transform based on the list of frames */
+ NEWMAT::Matrix computeTransformFromList(TransformLists list, unsigned long long time);
+
+};
+#endif //LIBTF_HH
Deleted: pkg/trunk/libTF/src/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/src/simple/Quaternion3D.hh 2008-04-22 20:29:25 UTC (rev 153)
+++ pkg/trunk/libTF/src/simple/Quaternion3D.hh 2008-04-22 20:31:08 UTC (rev 154)
@@ -1,174 +0,0 @@
-// Software License Agreement (BSD License)
-//
-// Copyright (c) 2008, Willow Garage, Inc.
-// All rights reserved.
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions
-// are met:
-//
-// * Redistributions of source code must retain the above copyright
-// notice, this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above
-// copyright notice, this list of conditions and the following
-// disclaimer in the documentation and/or other materials provided
-// with the distribution.
-// * Neither the name of the Willow Garage nor the names of its
-// contributors may be used to endorse or promote products derived
-// from this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-#ifndef QUATERNION3D_HH
-#define QUATERNION3D_HH
-
-#include <iostream>
-#include <newmat/newmat.h>
-#include <newmat/newmatio.h>
-#include <math.h>
-#include <pthread.h>
-#include <sys/time.h>
-class Euler3D {
-public:
- //Constructor
- Euler3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
-
- //Storage
- double x,y,z,yaw,pitch,roll;
-};
-
-
-class Quaternion3D {
-
-public:
- // Storage class
- class Quaternion3DStorage
- {
- public:
-
- /** Unary operators preserve timestamps
- * Binary operators destroy timestamps */
-
- // Utility functions to normalize and get magnitude.
- void Normalize();
- double getMagnitude();
- Quaternion3DStorage & operator=(const Quaternion3DStorage & input);
-
-
- /* accessors */
- NEWMAT::Matrix asMatrix();
-
- /* Internal Data */
- double xt, yt, zt, xr, yr, zr, w;
- unsigned long long time;
- };
-
- /** Constructors **/
- // Standard constructor
- Quaternion3D();
-
- /** Mutators **/
- // Set the values manually
- void fromQuaternion(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
- //Set the values from a matrix
- void fromMatrix(const NEWMAT::Matrix& matIn, unsigned long long time);
- // Set the values using Euler angles
- void fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll, unsigned long long time);
- // Set the values using DH Parameters
- void fromDH(double length, double alpha, double offset, double theta, unsigned long long time);
-
-
- /** Interpolated Accessors **/
- // Return a Quaternion
- Quaternion3D::Quaternion3DStorage getQuaternion(unsigned long long time);
- // Return a Matrix
- NEWMAT::Matrix getMatrix(unsigned long long time);
- // Return the inverse matrix
- NEWMAT::Matrix getInverseMatrix(unsigned long long time);
-
- /**** Utility Functions ****/
- // this is a function to return the current time in microseconds from the beginning of 1970
- static unsigned long long Qgettime(void);
-
- // Convert DH Parameters to a Homogeneous Transformation Matrix
- static NEWMAT::Matrix matrixFromDH(double length, double alpha, double offset, double theta);
- // Convert Euler Angles to a Homogeneous Transformation Matrix
- static NEWMAT::Matrix matrixFromEuler(double ax,
- double ay, double az, double yaw,
- double pitch, double roll);
-
- //Print as a matrix
- void printMatrix(unsigned long long time); //Not a critical part either
- void printStorage(const Quaternion3DStorage &storage); //Do i need this now that i've got the ostream method??
-
-
-private:
- /**** Linked List stuff ****/
- static const long long MAX_STORAGE_TIME = 1000000000; // max of 100 seconds storage
-
- struct data_LL{
- Quaternion3DStorage data;
- data_LL * next;
- data_LL * previous;
- };
-
- bool getValue(Quaternion3DStorage& buff, unsigned long long time, long long &time_diff);
- void add_value(const Quaternion3DStorage&);//todo fixme finish implementing this
-
-
-
- // insert a node into the sorted linked list
- void insertNode(const Quaternion3DStorage & );
- // prune data older than max_storage_time from the list
- void pruneList();
-
- //Find the closest two points in the list
- //Return the distance to the closest one
- int findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, const unsigned long long target_time, long long &time_diff);
-
- //Interpolate between two nodes and return the interpolated value
- // This must always take two valid points!!!!!!
- // Only Cpose version implemented
- void interpolate(const Quaternion3DStorage &one, const Quaternion3DStorage &two, const unsigned long long target_time, Quaternion3DStorage& output);
-
- //Used by interpolate to interpolate between double values
- double interpolateDouble(const double, const unsigned long long, const double, const unsigned long long, const unsigned long long);
-
- //How long to cache incoming values
- unsigned long long max_storage_time;
-
- //A mutex to prevent linked list collisions
- pthread_mutex_t linked_list_mutex;
-
- //Pointers for the start and end of a sorted linked list.
- data_LL* first;
- data_LL* last;
-
-
-
-};
-
-
-
-//A global ostream overload for displaying storage
-std::ostream & operator<<(std::ostream& mystream,const Quaternion3D::Quaternion3DStorage & storage);
-
-
-
-
-
-
-
-
-#endif //QUATERNION3D_HH
Deleted: pkg/trunk/libTF/src/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/src/simple/libTF.hh 2008-04-22 20:29:25 UTC (rev 153)
+++ pkg/trunk/libTF/src/simple/libTF.hh 2008-04-22 20:31:08 UTC (rev 154)
@@ -1,172 +0,0 @@
-//Software License Agreement (BSD License)
-
-//Copyright (c) 2008, Willow Garage, Inc.
-//All rights reserved.
-
-//Redistribution and use in source and binary forms, with or without
-//modification, are permitted provided that the following conditions
-//are met:
-
-// * Redistributions of source code must retain the above copyright
-// notice, this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above
-// copyright notice, this list of conditions and the following
-// disclaimer in the documentation and/or other materials provided
-// with the distribution.
-// * Neither the name of the Willow Garage nor the names of its
-// contributors may be used to endorse or promote products derived
-// from this software without specific prior written permission.
-
-//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-//COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-//INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-//BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-//CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-//LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-//POSSIBILITY OF SUCH DAMAGE.
-
-#ifndef LIBTF_HH
-#define LIBTF_HH
-#include <iostream>
-#include <iomanip>
-#include <newmat/newmat.h>
-#include <newmat/newmatio.h>
-#include <math.h>
-#include <vector>
-#include <sstream>
-
-#include "Quaternion3D.hh"
-
-/** RefFrame *******
- * An instance of this class is created for each frame in the system.
- * This class natively handles the relationship between frames.
- *
- * The derived class Quaternion3D provides a buffered history of positions
- * with interpolation.
- */
-
-class RefFrame: public Quaternion3D
-{
-public:
-
- /* Constructor */
- RefFrame();
-
- /* Get the parent node */
- inline unsigned int getParent(){return parent;};
-
- /* Return tha parent node */
- inline void setParent(unsigned int parentID){parent = parentID;};
-private:
-
- /* Storage of the parent */
- unsigned int parent;
-
-};
-
-/*** Transform Reference ***********
- * This class provides a simple interface to allow recording and lookup of
- * relationships between arbitrary frames of the system.
- *
- * The positions of frames over time must be pushed in.
- *
- * It will provide interpolated positions out given a time argument.
- */
-
-class TransformReference
-{
-public:
- /************* Constants ***********************/
- static const unsigned int ROOT_FRAME = 1; //Hard Value for ROOT_FRAME
- static const unsigned int NO_PARENT = 0; //Value for NO_PARENT
-
- static const unsigned int MAX_NUM_FRAMES = 100; /* The maximum number of frames possible */
- static const unsigned int MAX_GRAPH_DEPTH = 100; /* The maximum number of times to descent before determining that graph has a loop. */
-
- /* Constructor */
- TransformReference();
-
- /********** Mutators **************/
- /* Set a new frame or update an old one. */
- /* Use Euler Angles. X forward, Y to the left, Z up, Yaw about Z, pitch about new Y, Roll about new X */
- void setWithEulers(unsigned int framid, unsigned int parentid, double x, double y, double z, double yaw, double pitch, double roll, unsigned long long time);
- /* Using DH Parameters */
- // Conventions from http://en.wikipedia.org/wiki/Robotics_conventions
- void setWithDH(unsigned int framid, unsigned int parentid, double length, double alpha, double offset, double theta, unsigned long long time);
- // Possible exceptions TransformReference::LookupException
-
- /*********** Accessors *************/
-
- /* Get the transform between two frames by frame ID. */
- NEWMAT::Matrix getMatrix(unsigned int target_frame, unsigned int source_frame, unsigned long long time);
- // Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
- // TransformReference::MaxDepthException
-
- /* Debugging function that will print to std::cout the transformation matrix */
- std::string viewChain(unsigned int target_frame, unsigned int source_frame);
- // Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
- // TransformReference::MaxDepthException
-
-
-
-
-
-
- /************ Possible Exceptions ****************************/
-
- /* An exception class to notify of bad frame number */
- class LookupException : public std::exception
- {
- public:
- virtual const char* what() const throw() { return "InvalidFrame"; }
- } InvalidFrame;
-
- /* An exception class to notify of no connection */
- class ConnectivityException : public std::exception
- {
- public:
- virtual const char* what() const throw() { return "No connection between frames"; }
- private:
- } NoFrameConnectivity;
-
- /* An exception class to notify that the search for connectivity descended too deep. */
- class MaxDepthException : public std::exception
- {
- public:
- virtual const char* what() const throw() { return "Search exceeded max depth. Probably a loop in the tree."; }
- private:
- } MaxSearchDepth;
-
-private:
- /******************** Internal Storage ****************/
-
- /* The pointers to potential frames that the tree can be made of.
- * The frames will be dynamically allocated at run time when set the first time. */
- RefFrame* frames[MAX_NUM_FRAMES];
-
-
- /* This struct is how the list of transforms are stored before being passed to computeTransformFromList. */
- typedef struct
- {
- std::vector<unsigned int> inverseTransforms;
- std::vector<unsigned int> forwardTransforms;
- } TransformLists;
-
- /************************* Internal Functions ****************************/
-
- /* An accessor to get a frame, which will throw an exception if the frame is no there. */
- inline RefFrame* getFrame(unsigned int frame_number) { if (frames[frame_number] == NULL) throw InvalidFrame; else return frames[frame_number];};
-
- /* Find the list of connected frames necessary to connect two different frames */
- TransformLists lookUpList(unsigned int target_frame, unsigned int source_frame);
-
- /* Compute the transform based on the list of frames */
- NEWMAT::Matrix computeTransformFromList(TransformLists list, unsigned long long time);
-
-};
-#endif //LIBTF_HH
Deleted: pkg/trunk/libTF/src/simple/main.cc
===================================================================
--- pkg/trunk/libTF/src/simple/main.cc 2008-04-22 20:29:25 UTC (rev 153)
+++ pkg/trunk/libTF/src/simple/main.cc 2008-04-22 20:31:08 UTC (rev 154)
@@ -1,84 +0,0 @@
-#include "Quaternion3D.hh"
-#include "libTF.hh"
-#include <time.h>
-
-using namespace std;
-
-int main(void)
-{
- double dx,dy,dz,dyaw,dp,dr;
- TransformReference mTR;
-
-
-
- dx = dy= dz = 0;
- dyaw = dp = dr = 0.1;
-
- unsigned long long atime = Quaternion3D::Qgettime();
-
-
- //Fill in some transforms
- // mTR.setWithEulers(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
- mTR.setWithDH(10,2,1.0,1.0,1.0,dyaw,atime);
- //mTR.setWithEulers(2,3,1-1,1,1,dyaw,dp,dr,atime-1000);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-100);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-50);
- mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-1000);
- //mTR.setWithEulers(2,3,1+1,1,1,dyaw,dp,dr,atime+1000);
- mTR.setWithEulers(3,5,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(5,1,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
- mTR.setWithEulers(7,6,1,1,1,dyaw,dp,dr,atime);
- mTR.setWithDH(8,7,1.0,1.0,1.0,dyaw,atime);
- //mTR.setWithEulers(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
-
-
- //Demonstrate InvalidFrame LookupException
- try
- {
- std::cout<< mTR.viewChain(10,9);
- }
- catch (TransformReference::LookupException &ex)
- {
- std::cout << "Caught " << ex.what()<<std::endl;
- }
-
-
- // See the list of transforms to get between the frames
- std::cout<<"Viewing (10,8):"<<std::endl;
- std::cout << mTR.viewChain(10,8);
-
-
- //See the resultant transform
- std::cout <<"Calling getMatrix(10,8)"<<std::endl;
- // NEWMAT::Matrix mat = mTR.getMatrix(1,1);
- NEWMAT::Matrix mat = mTR.getMatrix(10,8,atime);
-
- std::cout << "Result of getMatrix(10,8,atime):" << std::endl << mat<< std::endl;
-
-
-
- //Break the graph, making it loop and demonstrate catching MaxDepthException
- mTR.setWithEulers(6,7,dx,dy,dz,dyaw,dp,dr,atime);
-
- try {
- std::cout<<mTR.viewChain(10,8);
- }
- catch (TransformReference::MaxDepthException &ex)
- {
- std::cout <<"caught loop in graph"<<std::endl;
- }
-
- //Break the graph, making it disconnected, and demonstrate catching ConnectivityException
- mTR.setWithEulers(6,0,dx,dy,dz,dyaw,dp,dr,atime);
-
- try {
- std::cout<<mTR.viewChain(10,8);
- }
- catch (TransformReference::ConnectivityException &ex)
- {
- std::cout <<"caught unconnected frame"<<std::endl;
- }
-
- return 0;
-};
Deleted: pkg/trunk/libTF/src/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/src/simple/testQuat.cc 2008-04-22 20:29:25 UTC (rev 153)
+++ pkg/trunk/libTF/src/simple/testQuat.cc 2008-04-22 20:31:08 UTC (rev 154)
@@ -1,51 +0,0 @@
-// Software License Agreement (BSD License)
-//
-// Copyright (c) 2008, Willow Garage, Inc.
-// All rights reserved.
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions
-// are met:
-//
-// * Redistributions of source code must retain the above copyright
-// notice, this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above
-// copyright notice, this list of conditions and the following
-// disclaimer in the documentation and/or other materials provided
-// with the distribution.
-// * Neither the name of the Willow Garage nor the names of its
-// contributors may be used to endorse or promote products derived
-// from this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-
-
-#include "Quaternion3D.hh"
-
-
-int main()
-{
- Quaternion3D myquat;
-
- Quaternion3D my2ndquat;
- myquat.Set(1,1,1,0,0,0,1,1000101010);
- my2ndquat.Set(1,1,1,0,0,0,1,1000101010);
- std::cout << "Quat1"<<std::endl;
- myquat.printMatrix(1000101010);
- std::cout << "Quat2"<<std::endl;
- my2ndquat.printMatrix(1000101010);
-
- return 0;
-}
Copied: pkg/trunk/libTF/src/test/main.cc (from rev 153, pkg/trunk/libTF/src/simple/main.cc)
===================================================================
--- pkg/trunk/libTF/src/test/main.cc (rev 0)
+++ pkg/trunk/libTF/src/test/main.cc 2008-04-22 20:31:08 UTC (rev 154)
@@ -0,0 +1,84 @@
+#include "Quaternion3D.hh"
+#include "libTF.hh"
+#include <time.h>
+
+using namespace std;
+
+int main(void)
+{
+ double dx,dy,dz,dyaw,dp,dr;
+ TransformReference mTR;
+
+
+
+ dx = dy= dz = 0;
+ dyaw = dp = dr = 0.1;
+
+ unsigned long long atime = Quaternion3D::Qgettime();
+
+
+ //Fill in some transforms
+ // mTR.setWithEulers(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
+ mTR.setWithDH(10,2,1.0,1.0,1.0,dyaw,atime);
+ //mTR.setWithEulers(2,3,1-1,1,1,dyaw,dp,dr,atime-1000);
+ mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-100);
+ mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-50);
+ mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-1000);
+ //mTR.setWithEulers(2,3,1+1,1,1,dyaw,dp,dr,atime+1000);
+ mTR.setWithEulers(3,5,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(5,1,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(7,6,1,1,1,dyaw,dp,dr,atime);
+ mTR.setWithDH(8,7,1.0,1.0,1.0,dyaw,atime);
+ //mTR.setWithEulers(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
+
+
+ //Demonstrate InvalidFrame LookupException
+ try
+ {
+ std::cout<< mTR.viewChain(10,9);
+ }
+ catch (TransformReference::LookupException &ex)
+ {
+ std::cout << "Caught " << ex.what()<<std::endl;
+ }
+
+
+ // See the list of transforms to get between the frames
+ std::cout<<"Viewing (10,8):"<<std::endl;
+ std::cout << mTR.viewChain(10,8);
+
+
+ //See the resultant transform
+ std::cout <<"Calling getMatrix(10,8)"<<std::endl;
+ // NEWMAT::Matrix mat = mTR.getMatrix(1,1);
+ NEWMAT::Matrix mat = mTR.getMatrix(10,8,atime);
+
+ std::cout << "Result of getMatrix(10,8,atime):" << std::endl << mat<< std::endl;
+
+
+
+ //Break the graph, making it loop and demonstrate catching MaxDepthException
+ mTR.setWithEulers(6,7,dx,dy,dz,dyaw,dp,dr,atime);
+
+ try {
+ std::cout<<mTR.viewChain(10,8);
+ }
+ catch (TransformReference::MaxDepthException &ex)
+ {
+ std::cout <<"caught loop in graph"<<std::endl;
+ }
+
+ //Break the graph, making it disconnected, and demonstrate catching ConnectivityException
+ mTR.setWithEulers(6,0,dx,dy,dz,dyaw,dp,dr,atime);
+
+ try {
+ std::cout<<mTR.viewChain(10,8);
+ }
+ catch (TransformReference::ConnectivityException &ex)
+ {
+ std::cout <<"caught unconnected frame"<<std::endl;
+ }
+
+ return 0;
+};
Copied: pkg/trunk/libTF/src/test/testQuat.cc (from rev 153, pkg/trunk/libTF/src/simple/testQuat.cc)
===================================================================
--- pkg/trunk/libTF/src/test/testQuat.cc (rev 0)
+++ pkg/trunk/libTF/src/test/testQuat.cc 2008-04-22 20:31:08 UTC (rev 154)
@@ -0,0 +1,51 @@
+// Software License Agreement (BSD License)
+//
+// Copyright (c) 2008, Willow Garage, Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following
+// disclaimer in the documentation and/or other materials provided
+// with the distribution.
+// * Neither the name of the Willow Garage nor the names of its
+// contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+
+
+#include "Quaternion3D.hh"
+
+
+int main()
+{
+ Quaternion3D myquat;
+
+ Quaternion3D my2ndquat;
+ myquat.Set(1,1,1,0,0,0,1,1000101010);
+ my2ndquat.Set(1,1,1,0,0,0,1,1000101010);
+ std::cout << "Quat1"<<std::endl;
+ myquat.printMatrix(1000101010);
+ std::cout << "Quat2"<<std::endl;
+ my2ndquat.printMatrix(1000101010);
+
+ return 0;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
