Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [239] pkg/trunk/libTF
|
From: <tf...@us...> - 2008-04-29 23:41:39
|
Revision: 239
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=239&view=rev
Author: tfoote
Date: 2008-04-29 16:41:43 -0700 (Tue, 29 Apr 2008)
Log Message:
-----------
adding a point method ticket:22, and fixing zero distance bug
Modified Paths:
--------------
pkg/trunk/libTF/doc/libTF_Manual.tex
pkg/trunk/libTF/include/libTF/libTF.h
pkg/trunk/libTF/src/simple/libTF.cpp
pkg/trunk/libTF/src/test/main.cpp
Modified: pkg/trunk/libTF/doc/libTF_Manual.tex
===================================================================
--- pkg/trunk/libTF/doc/libTF_Manual.tex 2008-04-29 23:24:20 UTC (rev 238)
+++ pkg/trunk/libTF/doc/libTF_Manual.tex 2008-04-29 23:41:43 UTC (rev 239)
@@ -17,7 +17,9 @@
\newfloat{struct}{thp}{lop}
\floatname{struct}{Data Structure}
+\setcounter{tocdepth}{4}
+
\begin{document}
\title{libTF Manual}
\author{Tully Foote\\
@@ -119,6 +121,16 @@
\texttt{ULLtime} is the number of nanoseconds since the epoch(1970) expressed as
an unsigned long long, which should be equivilant to a \texttt{uint64\_t}.
+\begin{verbatim}
+struct TFPoint
+{
+ double x,y,z;
+ unsigned long long time;
+ unsigned int frame;
+};
+\end{verbatim}
+
+
\subsubsection{Constructor}
\index{libTF API!Constructor}
\begin{verbatim}
@@ -172,6 +184,14 @@
unsigned long long time);
\end{verbatim}
+\paragraph{transformPoint}
+\begin{verbatim}
+/* Transform a point to a different frame */
+TFPoint transformPoint(unsigned int target_frame, const TFPoint & point_in);
+\end{verbatim}
+Point $P_0 = (x,y,z,1)^T$ will be transformed into the target frame
+using the time and source frame embedded in the point struct.
+
\paragraph{viewChain}
\index{libTF API!viewChain}
\begin{verbatim}
@@ -179,6 +199,22 @@
unsigned int source_frame);
\end{verbatim}
+\subsubsection{Constants}
+\begin{verbatim}
+static const unsigned int ROOT_FRAME = 1; //Hard Value for ROOT_FRAME
+static const unsigned int NO_PARENT = 0; //Value for NO_PARENT
+
+/* The maximum number of frames possible */
+static const unsigned int MAX_NUM_FRAMES = 100;
+/* The maximum number of times to descent before
+ * determining that graph has a loop. */
+static const unsigned int MAX_GRAPH_DEPTH = 100;
+
+//10 seconds in nanoseconds
+static const ULLtime DEFAULT_CACHE_TIME = 10 * 1000000000ULL;
+
+\end{verbatim}
+
\section{ROS Integration}
unimplemented
Modified: pkg/trunk/libTF/include/libTF/libTF.h
===================================================================
--- pkg/trunk/libTF/include/libTF/libTF.h 2008-04-29 23:24:20 UTC (rev 238)
+++ pkg/trunk/libTF/include/libTF/libTF.h 2008-04-29 23:41:43 UTC (rev 239)
@@ -42,6 +42,20 @@
#include "Quaternion3D.h"
+
+
+
+
+/**** Point ****
+ */
+struct TFPoint
+{
+ double x,y,z;
+ unsigned long long time;
+ unsigned int frame;
+};
+
+
/** RefFrame *******
* An instance of this class is created for each frame in the system.
* This class natively handles the relationship between frames.
@@ -106,6 +120,10 @@
/* 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, ULLtime time);
+ /* Set the transform using a matrix */
+ void setWithMatrix(unsigned int framid, unsigned int parentid, const NEWMAT::Matrix & matrix_in, ULLtime time);
+ /* Set the transform using quaternions natively */
+ void setWithQuaternion(unsigned int framid, unsigned int parentid, double xt, double yt, double zt, double xr, double yr, double zr, double w, ULLtime time);
// Possible exceptions TransformReference::LookupException
/*********** Accessors *************/
@@ -115,6 +133,10 @@
// Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
// TransformReference::MaxDepthException
+
+ /* Transform a point to a different frame */
+ TFPoint transformPoint(unsigned int target_frame, const TFPoint & point_in);
+
/* 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,
@@ -164,6 +186,7 @@
// How long to cache transform history
ULLtime cache_time;
+ public:
/* This struct is how the list of transforms are stored before being passed to computeTransformFromList. */
typedef struct
{
@@ -171,6 +194,7 @@
std::vector<unsigned int> forwardTransforms;
} TransformLists;
+ // private:
/************************* Internal Functions ****************************/
/* An accessor to get a frame, which will throw an exception if the frame is no there. */
@@ -180,7 +204,7 @@
TransformLists lookUpList(unsigned int target_frame, unsigned int source_frame);
/* Compute the transform based on the list of frames */
- NEWMAT::Matrix computeTransformFromList(TransformLists list, ULLtime time);
+ NEWMAT::Matrix computeTransformFromList(const TransformLists & list, ULLtime time);
};
#endif //LIBTF_HH
Modified: pkg/trunk/libTF/src/simple/libTF.cpp
===================================================================
--- pkg/trunk/libTF/src/simple/libTF.cpp 2008-04-29 23:24:20 UTC (rev 238)
+++ pkg/trunk/libTF/src/simple/libTF.cpp 2008-04-29 23:41:43 UTC (rev 239)
@@ -77,6 +77,35 @@
}
+void TransformReference::setWithMatrix(unsigned int frameID, unsigned int parentID, const NEWMAT::Matrix & matrix_in, ULLtime time)
+{
+ if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES || frameID == NO_PARENT || frameID == ROOT_FRAME)
+ throw InvalidFrame;
+
+ //TODO check and throw exception if matrix wrong size
+ if (frames[frameID] == NULL)
+ frames[frameID] = new RefFrame();
+
+ getFrame(frameID)->setParent(parentID);
+ getFrame(frameID)->fromMatrix(matrix_in,time);
+}
+
+
+void TransformReference::setWithQuaternion(unsigned int frameID, unsigned int parentID, double xt, double yt, double zt, double xr, double yr, double zr, double w, ULLtime 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)->fromQuaternion(xt, yt, zt, xr, yr, zr, w,time);
+}
+
+
+
+
NEWMAT::Matrix TransformReference::getMatrix(unsigned int target_frame, unsigned int source_frame, ULLtime time)
{
NEWMAT::Matrix myMat(4,4);
@@ -86,6 +115,28 @@
}
+
+
+TFPoint TransformReference::transformPoint(unsigned int target_frame, const TFPoint & point_in)
+{
+ //Create a vector
+ NEWMAT::Matrix pointMat(4,1);
+ pointMat << point_in.x << point_in.y << point_in.z << 1;
+
+ NEWMAT::Matrix myMat = getMatrix(target_frame, point_in.frame, point_in.time);
+
+
+ pointMat = myMat * pointMat;
+ TFPoint retPoint;
+ retPoint.x = pointMat(1,1);
+ retPoint.y = pointMat(2,1);
+ retPoint.z = pointMat(3,1);
+ retPoint.frame = target_frame;
+ retPoint.time = point_in.time;
+ return retPoint;
+}
+
+
TransformReference::TransformLists TransformReference::lookUpList(unsigned int target_frame, unsigned int source_frame)
{
TransformLists mTfLs;
@@ -140,17 +191,24 @@
if (mTfLs.inverseTransforms.back() == NO_PARENT || mTfLs.forwardTransforms.back() == NO_PARENT)
throw(NoFrameConnectivity);
+ bool imdone = false;
while (mTfLs.inverseTransforms.back() == mTfLs.forwardTransforms.back())
{
mTfLs.inverseTransforms.pop_back();
mTfLs.forwardTransforms.pop_back();
+
+ // Make sure we don't go beyond the beginning of the list.
+ // (The while statement above doesn't fail if you hit the beginning of the list,
+ // which happens in the zero distance case.)
+ if (mTfLs.inverseTransforms.size() == 0 || mTfLs.forwardTransforms.size() == 0)
+ break;
}
return mTfLs;
}
-NEWMAT::Matrix TransformReference::computeTransformFromList(TransformLists lists, ULLtime time)
+NEWMAT::Matrix TransformReference::computeTransformFromList(const TransformLists & lists, ULLtime time)
{
NEWMAT::Matrix retMat(4,4);
retMat << 1 << 0 << 0 << 0
@@ -158,17 +216,14 @@
<< 0 << 0 << 1 << 0
<< 0 << 0 << 0 << 1;
+
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
retMat *= getFrame(lists.inverseTransforms[i])->getMatrix(time);
- // 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 *= getFrame(lists.forwardTransforms[lists.forwardTransforms.size() -1 - i])->getInverseMatrix(time); //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;
Modified: pkg/trunk/libTF/src/test/main.cpp
===================================================================
--- pkg/trunk/libTF/src/test/main.cpp 2008-04-29 23:24:20 UTC (rev 238)
+++ pkg/trunk/libTF/src/test/main.cpp 2008-04-29 23:41:43 UTC (rev 239)
@@ -56,8 +56,31 @@
std::cout << "Result of getMatrix(10,8,atime):" << std::endl << mat<< std::endl;
-
-
+ TFPoint mPoint;
+ mPoint.x = 1;
+ mPoint.y = 1;
+ mPoint.z = 1;
+ mPoint.frame = 10;
+
+ TFPoint nPoint = mPoint;
+
+ std::cout <<"Point 1,1,1 goes like this:" <<std::endl;
+ std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
+ mPoint = mTR.transformPoint(2, mPoint);
+ std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
+ mPoint = mTR.transformPoint(3, mPoint);
+ std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
+ mPoint = mTR.transformPoint(5, mPoint);
+ std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
+ mPoint = mTR.transformPoint(6, mPoint);
+ std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
+ mPoint = mTR.transformPoint(7, mPoint);
+ std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
+ mPoint = mTR.transformPoint(8, mPoint);
+ std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
+ mPoint = mTR.transformPoint(10, mPoint);
+ std::cout << "(" << mPoint.x <<","<< mPoint.y << "," << mPoint.z << ") in frame " << mPoint.frame << std::endl;
+
//Break the graph, making it loop and demonstrate catching MaxDepthException
mTR.setWithEulers(6,7,dx,dy,dz,dyaw,dp,dr,atime);
@@ -82,6 +105,7 @@
//Testing clearing the history with parent change
mTR.setWithEulers(7,5,1,1,1,dyaw,dp,dr,atime);
+ //todo display this somehow
return 0;
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
