Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [113] pkg/trunk/libTF/simple
|
From: <tf...@us...> - 2008-04-17 02:09:50
|
Revision: 113
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=113&view=rev
Author: tfoote
Date: 2008-04-16 19:09:56 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
still working using timestamps everywhere
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 01:46:36 UTC (rev 112)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 02:09:56 UTC (rev 113)
@@ -39,35 +39,49 @@
};
-Quaternion3D::Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w):
+Quaternion3D::Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time):
xt(_xt),yt(_yt),zt(_zt),xr(_xr),yr(_yr),zr(_zr),w(_w),
max_storage_time(MAX_STORAGE_TIME),
first(NULL),
last(NULL)
{
+
pthread_mutex_init( &linked_list_mutex, NULL);
Normalize();
return;
};
-Quaternion3D::Quaternion3D(NEWMAT::Matrix matrixIn):
+Quaternion3D::Quaternion3D(NEWMAT::Matrix matrixIn, unsigned long long time):
max_storage_time(MAX_STORAGE_TIME),
first(NULL),
last(NULL)
{
pthread_mutex_init( &linked_list_mutex, NULL);
- fromMatrix(matrixIn);
+ fromMatrix(matrixIn, time);
};
-void Quaternion3D::fromMatrix(NEWMAT::Matrix matIn)
+void Quaternion3D::Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time)
+{xt = _xt; yt = _yt; zt = _zt; xr = _xr; yr = _yr; zr = _zr; w = _w;
+
+ Quaternion3DStorage temp;
+ temp.xt = _xt; temp.yt = _yt; temp.zt = _zt; temp.xr = _xr; temp.yr = _yr; temp.zr = _zr; temp.w = _w; temp.time = time;
+
+ add_value(temp);
+
+} ;
+
+
+void Quaternion3D::fromMatrix(NEWMAT::Matrix matIn, unsigned long long time)
{
// math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+ Quaternion3DStorage temp;
+ temp.time = time;
double * mat = matIn.Store();
//Get the translations
- xt = mat[3];
- yt = mat[7];
- zt = mat[11];
+ temp.xt = mat[3];
+ temp.yt = mat[7];
+ temp.zt = mat[11];
//TODO ASSERT others are zero and one as they should be
@@ -82,10 +96,10 @@
if ( T > 0.00000001 ) //to avoid large distortions!
{
double S = sqrt(T) * 2;
- xr = ( mat[9] - mat[6] ) / S;
- yr = ( mat[2] - mat[8] ) / S;
- zr = ( mat[4] - mat[1] ) / S;
- w = 0.25 * S;
+ temp.xr = ( mat[9] - mat[6] ) / S;
+ temp.yr = ( mat[2] - mat[8] ) / S;
+ temp.zr = ( mat[4] - mat[1] ) / S;
+ temp.w = 0.25 * S;
}
//If the trace of the matrix is equal to zero then identify
// which major diagonal element has the greatest value.
@@ -93,34 +107,36 @@
if ( mat[0] > mat[5] && mat[0] > mat[10] ) {// Column 0:
double S = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
- xr = 0.25 * S;
- yr = (mat[1] + mat[4] ) / S;
- zr = (mat[8] + mat[2] ) / S;
- w = (mat[6] - mat[9] ) / S;
+ temp.xr = 0.25 * S;
+ temp.yr = (mat[1] + mat[4] ) / S;
+ temp.zr = (mat[8] + mat[2] ) / S;
+ temp.w = (mat[6] - mat[9] ) / S;
} else if ( mat[5] > mat[10] ) {// Column 1:
double S = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
- xr = (mat[1] + mat[4] ) / S;
- yr = 0.25 * S;
- zr = (mat[6] + mat[9] ) / S;
- w = (mat[8] - mat[2] ) / S;
+ temp.xr = (mat[1] + mat[4] ) / S;
+ temp.yr = 0.25 * S;
+ temp.zr = (mat[6] + mat[9] ) / S;
+ temp.w = (mat[8] - mat[2] ) / S;
} else {// Column 2:
double S = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
- xr = (mat[8] + mat[2] ) / S;
- yr = (mat[6] + mat[9] ) / S;
- zr = 0.25 * S;
- w = (mat[1] - mat[4] ) / S;
+ temp.xr = (mat[8] + mat[2] ) / S;
+ temp.yr = (mat[6] + mat[9] ) / S;
+ temp.zr = 0.25 * S;
+ temp.w = (mat[1] - mat[4] ) / S;
}
+
+ add_value(temp);
};
-void Quaternion3D::fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
+void Quaternion3D::fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll, unsigned long long time)
{
- fromMatrix(matrixFromEuler(_x,_y,_z,_yaw,_pitch,_roll));
+ fromMatrix(matrixFromEuler(_x,_y,_z,_yaw,_pitch,_roll),time);
};
void Quaternion3D::fromDH(double theta,
- double length, double distance, double alpha)
+ double length, double distance, double alpha, unsigned long long time)
{
- fromMatrix(matrixFromDH(theta, length, distance, alpha));
+ fromMatrix(matrixFromDH(theta, length, distance, alpha),time);
};
@@ -211,7 +227,7 @@
};
-NEWMAT::Matrix Quaternion3D::asMatrix()
+NEWMAT::Matrix Quaternion3D::asMatrix(unsigned long long time)
{
NEWMAT::Matrix outMat(4,4);
@@ -247,9 +263,9 @@
};
-void Quaternion3D::printMatrix()
+void Quaternion3D::printMatrix(unsigned long long time)
{
- std::cout << asMatrix();
+ std::cout << asMatrix(time);
};
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 01:46:36 UTC (rev 112)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 02:09:56 UTC (rev 113)
@@ -62,21 +62,20 @@
/** Constructors **/
// Standard constructor which takes in 7 doubles
- Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
+ Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
// Constructor from Matrix
- Quaternion3D(NEWMAT::Matrix matrixIn);
+ Quaternion3D(NEWMAT::Matrix matrixIn, unsigned long long time);
/** Mutators **/
// Set the values manually
- inline void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w)
- {xt = _xt; yt = _yt; zt = _zt; xr = _xr; yr = _yr; zr = _zr; w = _w;} ;
+ void Set(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(NEWMAT::Matrix matIn);
+ void fromMatrix(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);
+ 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 theta, double length, double distance, double alpha);
+ void fromDH(double theta, double length, double distance, double alpha, unsigned long long time);
/**** Utility Functions ****/
@@ -92,11 +91,13 @@
/** Accessors **/
// Return a Matrix
- NEWMAT::Matrix asMatrix();
+ NEWMAT::Matrix asMatrix(unsigned long long time);
//Print as a matrix
- void printMatrix();
+ void printMatrix(unsigned long long time);
+ // this is a function to return the current time in microseconds from the beginning of 1970
+ static unsigned long long Qgettime(void);
private:
//Quaternion Storage
@@ -114,8 +115,6 @@
bool getValue(Quaternion3DStorage& buff, unsigned long long time, long long &time_diff);
void add_value(Quaternion3DStorage);//todo fixme finish implementing this
- // this is a function to return the current time in microseconds from the beginning of 1970
- unsigned long long Qgettime(void);
// insert a node into the sorted linked list
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-17 01:46:36 UTC (rev 112)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-17 02:09:56 UTC (rev 113)
@@ -34,38 +34,38 @@
RefFrame::RefFrame() :
parent(0),
- myQuat(0,0,0,0,0,0,1)
+ myQuat(0,0,0,0,0,0,1,111110000)
{
return;
}
/* Quaternion 3D version */
-void RefFrame::setParamsQuaternion3D(double a,double b,double c,double d,double e,double f, double g)
+void RefFrame::setParamsQuaternion3D(double a,double b,double c,double d,double e,double f, double g, unsigned long long time)
{
- myQuat.Set(a,b,c,d,e,f,g);
+ myQuat.Set(a,b,c,d,e,f,g,time);
};
/* Six DOF version */
-void RefFrame::setParamsEulers(double a,double b,double c,double d,double e,double f)
+void RefFrame::setParamsEulers(double a,double b,double c,double d,double e,double f, unsigned long long time)
{
- myQuat.fromEuler(a,b,c,d,e,f);
+ myQuat.fromEuler(a,b,c,d,e,f,time) ;
}
/* DH Params version */
-void RefFrame::setParamsDH(double a,double b,double c,double d)
+void RefFrame::setParamsDH(double a,double b,double c,double d, unsigned long long time)
{
- myQuat.fromDH(a,b,c,d);
+ myQuat.fromDH(a,b,c,d,time);
}
-NEWMAT::Matrix RefFrame::getMatrix()
+NEWMAT::Matrix RefFrame::getMatrix(unsigned long long time)
{
- return myQuat.asMatrix();
+ return myQuat.asMatrix(time);
}
-NEWMAT::Matrix RefFrame::getInverseMatrix()
+NEWMAT::Matrix RefFrame::getInverseMatrix(unsigned long long time)
{
- return myQuat.asMatrix().i();
+ return myQuat.asMatrix(time).i();
};
@@ -80,7 +80,7 @@
}
-void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f)
+void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f, unsigned long long time)
{
if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES || frameID == NO_PARENT || frameID == ROOT_FRAME)
throw InvalidFrame;
@@ -89,15 +89,15 @@
frames[frameID] = new RefFrame();
getFrame(frameID)->setParent(parentID);
- getFrame(frameID)->setParamsEulers(a,b,c,d,e,f);
+ getFrame(frameID)->setParamsEulers(a,b,c,d,e,f,time);
}
-NEWMAT::Matrix TransformReference::get(unsigned int target_frame, unsigned int source_frame)
+NEWMAT::Matrix TransformReference::get(unsigned int target_frame, unsigned int source_frame, unsigned long long time)
{
NEWMAT::Matrix myMat(4,4);
TransformLists lists = lookUpList(target_frame, source_frame);
- myMat = computeTransformFromList(lists);
+ myMat = computeTransformFromList(lists,time);
// std::cout << myMat;
return myMat;
}
@@ -175,7 +175,7 @@
}
-NEWMAT::Matrix TransformReference::computeTransformFromList(TransformLists lists)
+NEWMAT::Matrix TransformReference::computeTransformFromList(TransformLists lists, unsigned long long time)
{
NEWMAT::Matrix retMat(4,4);
retMat << 1 << 0 << 0 << 0
@@ -185,13 +185,13 @@
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
- retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix();
+ retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix(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])->getMatrix(); //Do this list backwards for it was generated traveling the wrong way
+ retMat *= getFrame(lists.forwardTransforms[lists.forwardTransforms.size() -1 - i])->getMatrix(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;
}
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-17 01:46:36 UTC (rev 112)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-17 02:09:56 UTC (rev 113)
@@ -48,9 +48,9 @@
RefFrame();
/* Set the parameters for this frame */
- void setParamsQuaternion3D(double, double, double, double, double, double, double);
- void setParamsEulers(double, double, double, double, double, double);
- void setParamsDH(double, double, double, double);
+ void setParamsQuaternion3D(double, double, double, double, double, double, double, unsigned long long time);
+ void setParamsEulers(double, double, double, double, double, double, unsigned long long time);
+ void setParamsDH(double, double, double, double, unsigned long long time);
/* Get the parent node */
inline unsigned int getParent(){return parent;};
@@ -59,10 +59,10 @@
inline void setParent(unsigned int parentID){parent = parentID;};
/* Generate and return the transform associated with gettingn into this frame */
- NEWMAT::Matrix getMatrix();
+ NEWMAT::Matrix getMatrix(unsigned long long time);
/* Generate and return the transform associated with getting out of this frame. */
- NEWMAT::Matrix getInverseMatrix();
+ NEWMAT::Matrix getInverseMatrix(unsigned long long time);
private:
@@ -89,13 +89,13 @@
/********** Mutators **************/
/* Set a new frame or update an old one. */
- void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double);
+ void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double,unsigned long long time);
// Possible exceptions TransformReference::LookupException
/*********** Accessors *************/
/* Get the transform between two frames by frame ID. */
- NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame);
+ NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame, unsigned long long time);
// Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
// TransformReference::MaxDepthException
@@ -158,7 +158,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);
+ NEWMAT::Matrix computeTransformFromList(TransformLists list, unsigned long long time);
};
#endif //LIBTF_HH
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-17 01:46:36 UTC (rev 112)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-17 02:09:56 UTC (rev 113)
@@ -14,15 +14,17 @@
dx = dy= dz = 0;
dyaw = dp = dr = .1;
+ unsigned long long atime = Quaternion3D::Qgettime();
+
//Fill in some transforms
- mTR.set(10,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,1,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.set(10,2,1,1,1,dyaw,dp,dr,atime);
+ mTR.set(2,3,1,1,1,dyaw,dp,dr,atime);
+ 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);
//Demonstrate InvalidFrame LookupException
@@ -44,14 +46,14 @@
//See the resultant transform
std::cout <<"Calling get(10,8)"<<std::endl;
// NEWMAT::Matrix mat = mTR.get(1,1);
- NEWMAT::Matrix mat = mTR.get(10,8);
+ NEWMAT::Matrix mat = mTR.get(10,8,atime);
- std::cout << "Result of get(10,8):" << std::endl << mat<< std::endl;
+ std::cout << "Result of get(10,8,atime):" << std::endl << mat<< std::endl;
//Break the graph, making it loop and demonstrate catching MaxDepthException
- mTR.set(6,7,dx,dy,dz,dyaw,dp,dr);
+ mTR.set(6,7,dx,dy,dz,dyaw,dp,dr,atime);
try {
mTR.view(10,8);
@@ -62,7 +64,7 @@
}
//Break the graph, making it disconnected, and demonstrate catching ConnectivityException
- mTR.set(6,0,dx,dy,dz,dyaw,dp,dr);
+ mTR.set(6,0,dx,dy,dz,dyaw,dp,dr,atime);
try {
mTR.view(10,8);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
