Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [156] pkg/trunk/libTF
|
From: <jl...@us...> - 2008-04-22 21:42:45
|
Revision: 156
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=156&view=rev
Author: jleibs
Date: 2008-04-22 14:42:52 -0700 (Tue, 22 Apr 2008)
Log Message:
-----------
Change from .hh to .h. Unclear if this is good convention, but we're going with it for now.
Modified Paths:
--------------
pkg/trunk/libTF/src/simple/Quaternion3D.cpp
pkg/trunk/libTF/src/simple/libTF.cpp
pkg/trunk/libTF/src/test/main.cpp
Added Paths:
-----------
pkg/trunk/libTF/include/libTF/Quaternion3D.h
pkg/trunk/libTF/include/libTF/libTF.h
Removed Paths:
-------------
pkg/trunk/libTF/include/libTF/Quaternion3D.hh
pkg/trunk/libTF/include/libTF/libTF.hh
Copied: pkg/trunk/libTF/include/libTF/Quaternion3D.h (from rev 154, pkg/trunk/libTF/include/libTF/Quaternion3D.hh)
===================================================================
--- pkg/trunk/libTF/include/libTF/Quaternion3D.h (rev 0)
+++ pkg/trunk/libTF/include/libTF/Quaternion3D.h 2008-04-22 21:42:52 UTC (rev 156)
@@ -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
Deleted: pkg/trunk/libTF/include/libTF/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/include/libTF/Quaternion3D.hh 2008-04-22 20:43:16 UTC (rev 155)
+++ pkg/trunk/libTF/include/libTF/Quaternion3D.hh 2008-04-22 21:42:52 UTC (rev 156)
@@ -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
Copied: pkg/trunk/libTF/include/libTF/libTF.h (from rev 154, pkg/trunk/libTF/include/libTF/libTF.hh)
===================================================================
--- pkg/trunk/libTF/include/libTF/libTF.h (rev 0)
+++ pkg/trunk/libTF/include/libTF/libTF.h 2008-04-22 21:42:52 UTC (rev 156)
@@ -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.h"
+
+/** 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/include/libTF/libTF.hh
===================================================================
--- pkg/trunk/libTF/include/libTF/libTF.hh 2008-04-22 20:43:16 UTC (rev 155)
+++ pkg/trunk/libTF/include/libTF/libTF.hh 2008-04-22 21:42:52 UTC (rev 156)
@@ -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
Modified: pkg/trunk/libTF/src/simple/Quaternion3D.cpp
===================================================================
--- pkg/trunk/libTF/src/simple/Quaternion3D.cpp 2008-04-22 20:43:16 UTC (rev 155)
+++ pkg/trunk/libTF/src/simple/Quaternion3D.cpp 2008-04-22 21:42:52 UTC (rev 156)
@@ -30,7 +30,7 @@
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
-#include "libTF/Quaternion3D.hh"
+#include "libTF/Quaternion3D.h"
Euler3D::Euler3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll) :
x(_x),y(_y),z(_z),yaw(_yaw),pitch(_pitch),roll(_roll)
Modified: pkg/trunk/libTF/src/simple/libTF.cpp
===================================================================
--- pkg/trunk/libTF/src/simple/libTF.cpp 2008-04-22 20:43:16 UTC (rev 155)
+++ pkg/trunk/libTF/src/simple/libTF.cpp 2008-04-22 21:42:52 UTC (rev 156)
@@ -30,7 +30,7 @@
//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.
-#include "libTF/libTF.hh"
+#include "libTF/libTF.h"
RefFrame::RefFrame() :
Quaternion3D(),
Modified: pkg/trunk/libTF/src/test/main.cpp
===================================================================
--- pkg/trunk/libTF/src/test/main.cpp 2008-04-22 20:43:16 UTC (rev 155)
+++ pkg/trunk/libTF/src/test/main.cpp 2008-04-22 21:42:52 UTC (rev 156)
@@ -1,5 +1,4 @@
-#include "libTF/Quaternion3D.hh"
-#include "libTF/libTF.hh"
+#include "libTF/libTF.h"
#include <time.h>
using namespace std;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
