Menu
▾
▴
personalrobots-commit
|
From: <tf...@us...> - 2008-03-26 20:48:56
|
Revision: 34
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=34&view=rev
Author: tfoote
Date: 2008-03-26 13:49:00 -0700 (Wed, 26 Mar 2008)
Log Message:
-----------
lots of documentation in headers
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-03-26 02:28:34 UTC (rev 33)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-03-26 20:49:00 UTC (rev 34)
@@ -1,8 +1,7 @@
#include "libTF.hh"
RefFrame::RefFrame() :
- parent(0),
- active(false)
+ parent(0)
{
return;
}
@@ -34,14 +33,12 @@
}
-TransformReference::TransformReference():
- mMat(4,4)
-{
-
+/*TransformReference::TransformReference()
+ *{
+ * return;
+ *}
+ */
- return;
-}
-
void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f)
{
frames[frameID].setParent(parentID);
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-03-26 02:28:34 UTC (rev 33)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-03-26 20:49:00 UTC (rev 34)
@@ -10,43 +10,71 @@
class RefFrame
{
public:
+ /* The type of frame
+ * This determines how many different parameters to expect to be updated, versus fixed */
// static enum FrameType {SIXDOF, DH //how to tell which mode it's in //todo add this process i'm going to start with 6dof only
+
+ /* Constructor */
RefFrame();
+
+ /* Set the parameters for this frame */
void setParams(double, double, double, double, double, double);
+
+ /* Get the parent node */
inline unsigned int getParent(){return parent;};
+
+ /* Return tha parent node */
inline void setParent(unsigned int parentID){parent = parentID;};
+
+ /* Generate and return the transform associated with gettingn into this frame */
NEWMAT::Matrix getMatrix();
+
+ /* Generate and return the transform associated with getting out of this frame. */
NEWMAT::Matrix getInverseMatrix();
private:
+ /* Storage of the parametsrs */
double params[6];
+
+ /* A helper function to build a homogeneous transform based on 6dof parameters */
static bool fill_transformation_matrix(NEWMAT::Matrix& matrix_pointer, double ax,
double ay, double az, double yaw,
double pitch, double roll);
+ /* Storage of the parent */
unsigned int parent;
- bool active;
};
class TransformReference
{
public:
+ /* Set a new frame or update an old one. */
void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double);
+
+ /* Get the transform between two frames. */
NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame);
- TransformReference();
+ /* Constructor */
+ // TransformReference();
+
+ /* Debugging function that will print to std::cout the transformation matrix */
void view(unsigned int target_frame, unsigned int source_frame);
+
private:
- double rD[6];
- NEWMAT::Matrix mMat;
+ /* The frames that the tree can be made of */
RefFrame frames[100];
+
+ /* This struct is how the list of transforms are stored before being generated. */
typedef struct
{
std::vector<unsigned int> inverseTransforms;
std::vector<unsigned int> forwardTransforms;
} TransformLists;
+ /* 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);
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-03-31 20:56:47
|
Revision: 72
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=72&view=rev
Author: tfoote
Date: 2008-03-31 13:56:54 -0700 (Mon, 31 Mar 2008)
Log Message:
-----------
adding dynamic allocation of frames. and exceptions if an uninitialized frame is tried to be accessed. ticket:4
Modified Paths:
--------------
pkg/trunk/libTF/simple/Makefile
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/Makefile
===================================================================
--- pkg/trunk/libTF/simple/Makefile 2008-03-31 17:43:56 UTC (rev 71)
+++ pkg/trunk/libTF/simple/Makefile 2008-03-31 20:56:54 UTC (rev 72)
@@ -6,7 +6,7 @@
test: main.cc libTF.o
- g++ -O2 $^ -o test -lnewmat
+ g++ -g -O2 $^ -o test -lnewmat
clean:
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-03-31 17:43:56 UTC (rev 71)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-03-31 20:56:54 UTC (rev 72)
@@ -33,16 +33,27 @@
}
-/*TransformReference::TransformReference()
- *{
- * return;
- *}
- */
+TransformReference::TransformReference()
+{
+ /* initialize pointers to NULL */
+ for (unsigned int i = 0; i < MAX_NUM_FRAMES; i++)
+ {
+ frames[i] = NULL;
+ }
+ return;
+}
+
void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f)
{
- frames[frameID].setParent(parentID);
- frames[frameID].setParams(a,b,c,d,e,f);
+ if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES)
+ throw InvalidFrame;
+
+ if (frames[frameID] == NULL)
+ frames[frameID] = new RefFrame();
+
+ getFrame(frameID)->setParent(parentID);
+ getFrame(frameID)->setParams(a,b,c,d,e,f);
}
@@ -110,19 +121,19 @@
unsigned int frame = target_frame;
while (frame != 0)
{
- if (frames[frame].getParent() > 100) exit(-1); //todo cleanup
+ if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
mTfLs.inverseTransforms.push_back(frame);
// std::cout <<"Frame: " << frame <<std::endl;
- frame = frames[frame].getParent();
+ frame = getFrame(frame)->getParent();
}
mTfLs.inverseTransforms.push_back(0);
frame = source_frame;
while (frame != 0)
{
- if (frames[frame].getParent() > 100) exit(-1); //todo cleanup
+ if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
mTfLs.forwardTransforms.push_back(frame);
- frame = frames[frame].getParent();
+ frame = getFrame(frame)->getParent();
}
mTfLs.forwardTransforms.push_back(0);
@@ -152,13 +163,13 @@
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
- retMat *= frames[lists.inverseTransforms[i]].getInverseMatrix();
+ retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix();
// 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 *= frames[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(); //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;
}
@@ -175,7 +186,7 @@
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
std::cout << lists.inverseTransforms[i];
- // retMat *= frames[lists.inverseTransforms[i]].getInverseMatrix();
+ // retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix();
}
std::cout << std::endl;
@@ -183,7 +194,7 @@
for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
{
std::cout << lists.forwardTransforms[i];
- // retMat *= frames[lists.inverseTransforms[lists.forwardTransforms.size() -1 - i]].getMatrix(); //Do this list backwards for it was generated traveling the wrong way
+ // retMat *= getFrame(lists.inverseTransforms[lists.forwardTransforms.size() -1 - i])->getMatrix(); //Do this list backwards for it was generated traveling the wrong way
}
std::cout << std::endl;
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-03-31 17:43:56 UTC (rev 71)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-03-31 20:56:54 UTC (rev 72)
@@ -10,6 +10,7 @@
class RefFrame
{
public:
+
/* The type of frame
* This determines how many different parameters to expect to be updated, versus fixed */
// static enum FrameType {SIXDOF, DH //how to tell which mode it's in //todo add this process i'm going to start with 6dof only
@@ -46,7 +47,9 @@
class TransformReference
{
- public:
+public:
+ static const unsigned int MAX_NUM_FRAMES = 100;
+
/* Set a new frame or update an old one. */
void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double);
@@ -54,14 +57,29 @@
NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame);
/* Constructor */
- // TransformReference();
+ TransformReference();
/* Debugging function that will print to std::cout the transformation matrix */
void view(unsigned int target_frame, unsigned int source_frame);
-
- private:
+
+ /* 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;
+private:
+
+ inline RefFrame* getFrame(unsigned int frame_number) { if (frames[frame_number] == NULL) throw InvalidFrame; else return frames[frame_number];};
/* The frames that the tree can be made of */
- RefFrame frames[100];
+ RefFrame* frames[MAX_NUM_FRAMES];
/* This struct is how the list of transforms are stored before being generated. */
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-03-31 17:43:56 UTC (rev 71)
+++ pkg/trunk/libTF/simple/main.cc 2008-03-31 20:56:54 UTC (rev 72)
@@ -36,7 +36,17 @@
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.view(1,8);
+ try
+ {
+ mTR.view(1,9);
+ }
+ catch (TransformReference::LookupException &ex)
+ {
+ std::cout << "Caught " << ex.what()<<std::endl;
+ mTR.view(1,8);
+ }
+
+
std::cout <<"Calling get(1,8)"<<std::endl;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-02 18:06:36
|
Revision: 73
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=73&view=rev
Author: tfoote
Date: 2008-04-02 11:06:39 -0700 (Wed, 02 Apr 2008)
Log Message:
-----------
adding NO_PARENT and ROOT_FRAME semantics and catching a loop in search by using max depth
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-03-31 20:56:54 UTC (rev 72)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-02 18:06:39 UTC (rev 73)
@@ -119,29 +119,37 @@
// std::vector<unsigned int> retVec;
unsigned int frame = target_frame;
- while (frame != 0)
+ unsigned int counter = 0; //A counter to keep track of how deep we've descended
+ while (frame != NO_PARENT && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
{
if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
mTfLs.inverseTransforms.push_back(frame);
// std::cout <<"Frame: " << frame <<std::endl;
frame = getFrame(frame)->getParent();
+ /* Check if we've gone too deep. Probably a loop */
+ if (counter++ > MAX_GRAPH_DEPTH)
+ throw(MaxSearchDepth);
}
mTfLs.inverseTransforms.push_back(0);
frame = source_frame;
- while (frame != 0)
+ counter = 0;
+ while (frame != NO_PARENT && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
{
if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
mTfLs.forwardTransforms.push_back(frame);
frame = getFrame(frame)->getParent();
+ /* Check if we've gone too deep. Probably a loop */
+ if (counter++ > MAX_GRAPH_DEPTH)
+ throw(MaxSearchDepth);
}
mTfLs.forwardTransforms.push_back(0);
+
+ /* Make sure the end of the search shares a parent. */
+ if (mTfLs.inverseTransforms.back() != mTfLs.forwardTransforms.back())
+ throw(NoFrameConnectivity);
- //todo fixme throw something
- // if (tList.size() <= 1) exit(-1);
- // if (sList.size() <= 1) exit(-1); // I think below will catch all these anyway
-
while (mTfLs.inverseTransforms.back() == mTfLs.forwardTransforms.back())
{
// std::cout << "removing " << mTfLs.inverseTransforms.back() << std::endl;
@@ -185,7 +193,7 @@
std::cout << "Inverse Transforms:" <<std::endl;
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
- std::cout << lists.inverseTransforms[i];
+ std::cout << lists.inverseTransforms[i]<<", ";
// retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix();
}
std::cout << std::endl;
@@ -193,7 +201,7 @@
std::cout << "Forward Transforms: "<<std::endl ;
for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
{
- std::cout << lists.forwardTransforms[i];
+ std::cout << lists.forwardTransforms[i]<<", ";
// retMat *= getFrame(lists.inverseTransforms[lists.forwardTransforms.size() -1 - i])->getMatrix(); //Do this list backwards for it was generated traveling the wrong way
}
std::cout << std::endl;
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-03-31 20:56:54 UTC (rev 72)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-02 18:06:39 UTC (rev 73)
@@ -48,7 +48,13 @@
class TransformReference
{
public:
+ static const unsigned int ROOT_FRAME = 1;
+ static const unsigned int NO_PARENT = 0;
+
+ /* 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;
/* Set a new frame or update an old one. */
void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double);
@@ -68,6 +74,7 @@
public:
virtual const char* what() const throw() { return "InvalidFrame"; }
} InvalidFrame;
+
/* An exception class to notify of no connection */
class ConnectivityException : public std::exception
{
@@ -75,14 +82,27 @@
virtual const char* what() const throw() { return "No connection between frames"; }
private:
} NoFrameConnectivity;
+
+ /* An exception class to notify of no connection */
+ 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:
+ /* 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];};
- /* The frames that the tree can be made of */
+
+ /* The pointers to potential frames that the tree can be made of.
+ * The frames will be dynamically created 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 generated. */
+ /* This struct is how the list of transforms are stored before being passed to computeTransformFromList. */
typedef struct
{
std::vector<unsigned int> inverseTransforms;
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-03-31 20:56:54 UTC (rev 72)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-02 18:06:39 UTC (rev 73)
@@ -29,7 +29,7 @@
- mTR.set(1,2,1,1,1,dyaw,dp,dr);
+ 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,0,dx,dy,dz,dyaw,dp,dr);
@@ -38,20 +38,20 @@
mTR.set(8,7,1,1,1,dyaw,dp,dr);
try
{
- mTR.view(1,9);
+ mTR.view(10,9);
}
catch (TransformReference::LookupException &ex)
{
std::cout << "Caught " << ex.what()<<std::endl;
- mTR.view(1,8);
+ mTR.view(10,8);
}
- std::cout <<"Calling get(1,8)"<<std::endl;
+ std::cout <<"Calling get(10,8)"<<std::endl;
// NEWMAT::Matrix mat = mTR.get(1,1);
- NEWMAT::Matrix mat = mTR.get(1,8);
+ NEWMAT::Matrix mat = mTR.get(10,8);
std::cout << "Result" << std::endl << mat<< std::endl;
#ifdef DONOTUSE
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-02 22:10:34
|
Revision: 75
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=75&view=rev
Author: tfoote
Date: 2008-04-02 15:10:38 -0700 (Wed, 02 Apr 2008)
Log Message:
-----------
successfully catching loops and unconnected frames
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-02 20:48:56 UTC (rev 74)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-02 22:10:38 UTC (rev 75)
@@ -46,7 +46,7 @@
void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d,double e,double f)
{
- if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES)
+ if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES || frameID == NO_PARENT || frameID == ROOT_FRAME)
throw InvalidFrame;
if (frames[frameID] == NULL)
@@ -120,36 +120,45 @@
unsigned int frame = target_frame;
unsigned int counter = 0; //A counter to keep track of how deep we've descended
- while (frame != NO_PARENT && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+ while (true)
{
+ mTfLs.inverseTransforms.push_back(frame);
+ if (frame == NO_PARENT || frame == ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+ break;
if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
- mTfLs.inverseTransforms.push_back(frame);
// std::cout <<"Frame: " << frame <<std::endl;
frame = getFrame(frame)->getParent();
/* Check if we've gone too deep. Probably a loop */
if (counter++ > MAX_GRAPH_DEPTH)
throw(MaxSearchDepth);
}
- mTfLs.inverseTransforms.push_back(0);
+ // mTfLs.inverseTransforms.push_back(0);
frame = source_frame;
counter = 0;
- while (frame != NO_PARENT && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+ while (true)//(frame != NO_PARENT && frame != ROOT_FRAME) //Descend until we reach the root node or don't have a parent
{
+ mTfLs.forwardTransforms.push_back(frame);
+ if (frame == NO_PARENT || frame == ROOT_FRAME) //Descend until we reach the root node or don't have a parent
+ break;
if (getFrame(frame)->getParent() > MAX_NUM_FRAMES) throw InvalidFrame;
- mTfLs.forwardTransforms.push_back(frame);
frame = getFrame(frame)->getParent();
/* Check if we've gone too deep. Probably a loop */
if (counter++ > MAX_GRAPH_DEPTH)
throw(MaxSearchDepth);
}
- mTfLs.forwardTransforms.push_back(0);
+ // mTfLs.forwardTransforms.push_back(0);
/* Make sure the end of the search shares a parent. */
if (mTfLs.inverseTransforms.back() != mTfLs.forwardTransforms.back())
throw(NoFrameConnectivity);
+ /* Make sure that we don't have a no parent at the top */
+ std::cout << "Back = " << mTfLs.inverseTransforms.back()<<" " << mTfLs.forwardTransforms.back();
+ // if (mTfLs.inverseTransforms.back() == NO_PARENT || mTfLs.forwardTransforms.back() == NO_PARENT)
+ // throw(NoFrameConnectivity);
+
while (mTfLs.inverseTransforms.back() == mTfLs.forwardTransforms.back())
{
// std::cout << "removing " << mTfLs.inverseTransforms.back() << std::endl;
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-02 20:48:56 UTC (rev 74)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-02 22:10:38 UTC (rev 75)
@@ -32,7 +32,7 @@
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,0,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);
@@ -43,10 +43,11 @@
catch (TransformReference::LookupException &ex)
{
std::cout << "Caught " << ex.what()<<std::endl;
- mTR.view(10,8);
}
-
+ std::cout<<"Viewing (10,8)";
+ mTR.view(10,8);
+
std::cout <<"Calling get(10,8)"<<std::endl;
@@ -54,6 +55,34 @@
NEWMAT::Matrix mat = mTR.get(10,8);
std::cout << "Result" << std::endl << mat<< std::endl;
+
+
+
+
+ mTR.set(6,7,dx,dy,dz,dyaw,dp,dr);
+
+ try {
+ mTR.view(10,8);
+ }
+ catch (TransformReference::MaxDepthException &ex)
+ {
+ std::cout <<"caught loop in graph"<<std::endl;
+ }
+
+ mTR.set(6,0,dx,dy,dz,dyaw,dp,dr);
+
+ try {
+ mTR.view(10,8);
+ }
+ catch (TransformReference::ConnectivityException &ex)
+ {
+ std::cout <<"caught unconnected frame"<<std::endl;
+ }
+
+
+
+
+
#ifdef DONOTUSE
// NEWMAT::Matrix mat(4,4);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-02 23:57:23
|
Revision: 77
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=77&view=rev
Author: tfoote
Date: 2008-04-02 16:56:19 -0700 (Wed, 02 Apr 2008)
Log Message:
-----------
adding a homogeneous transform from dh params function
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-02 22:26:51 UTC (rev 76)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-02 23:56:19 UTC (rev 77)
@@ -107,8 +107,41 @@
return true;
};
+// Math from http://en.wikipedia.org/wiki/Robotics_conventions
+bool RefFrame::fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
+ double length, double distance, double alpha)
+{
+ double ca = cos(alpha);
+ double sa = sin(alpha);
+ double ct = cos(theta);
+ double st = sin(theta);
+ double* matrix_pointer = matrix.Store();
+ if (matrix.Storage() != 16)
+ return false;
+ matrix_pointer[0] = ct;
+ matrix_pointer[1] = -st*ca;
+ matrix_pointer[2] = st*sa;
+ matrix_pointer[3] = distance * ct;
+ matrix_pointer[4] = st;
+ matrix_pointer[5] = ct*ca;
+ matrix_pointer[6] = -ct*sa;
+ matrix_pointer[7] = distance*st;
+ matrix_pointer[8] = 0;
+ matrix_pointer[9] = sa;
+ matrix_pointer[10] = ca;
+ matrix_pointer[11] = length;
+ matrix_pointer[12] = 0.0;
+ matrix_pointer[13] = 0.0;
+ matrix_pointer[14] = 0.0;
+ matrix_pointer[15] = 1.0;
+
+ return true;
+};
+
+
+
TransformReference::TransformLists TransformReference::lookUpList(unsigned int target_frame, unsigned int source_frame)
{
TransformLists mTfLs;
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-02 22:26:51 UTC (rev 76)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-02 23:56:19 UTC (rev 77)
@@ -40,6 +40,11 @@
static bool fill_transformation_matrix(NEWMAT::Matrix& matrix_pointer, double ax,
double ay, double az, double yaw,
double pitch, double roll);
+
+ /* A helper function to build a homogeneous transform based on DH parameters */
+ bool fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
+ double length, double distance, double alpha);
+
/* Storage of the parent */
unsigned int parent;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
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.
|
|
From: <tf...@us...> - 2008-04-17 21:54:12
|
Revision: 119
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=119&view=rev
Author: tfoote
Date: 2008-04-17 14:47:02 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
removing obsolete local data storage
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 21:41:36 UTC (rev 118)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 21:47:02 UTC (rev 119)
@@ -40,7 +40,6 @@
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)
@@ -61,8 +60,7 @@
};
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;
@@ -124,7 +122,6 @@
temp.zr = 0.25 * S;
temp.w = (mat[1] - mat[4] ) / S;
}
- xt = temp.xt; yt = temp.yt; zt = temp.zt; xr = temp.xr; yr = temp.yr; zr = temp.zr; w = temp.w;
add_value(temp);
};
@@ -261,9 +258,9 @@
mat[6] = 2 * ( yz + xw );
mat[10] = 1 - 2 * ( xx + yy );
mat[12] = mat[13] = mat[14] = 0;
- mat[3] = xt;
- mat[7] = yt;
- mat[11] = zt;
+ mat[3] = temp.xt;
+ mat[7] = temp.yt;
+ mat[11] = temp.zt;
mat[15] = 1;
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 21:41:36 UTC (rev 118)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 21:47:02 UTC (rev 119)
@@ -103,9 +103,6 @@
static unsigned long long Qgettime(void);
private:
- //Quaternion Storage
- double xt,yt,zt,xr,yr,zr,w;
-
/**** Linked List stuff ****/
static const long long MAX_STORAGE_TIME = 1000000000; // max of 100 seconds storage
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-17 21:41:36 UTC (rev 118)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-17 21:47:02 UTC (rev 119)
@@ -21,7 +21,7 @@
// mTR.set(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
mTR.set(10,2,1,1,1,dyaw,atime);
//mTR.set(2,3,1-1,1,1,dyaw,dp,dr,atime-1000);
- mTR.set(2,3,2,1,1,dyaw,dp,dr,atime-100);
+ mTR.set(2,3,1,1,1,dyaw,dp,dr,atime-100);
mTR.set(2,3,1,1,1,dyaw,dp,dr,atime-50);
mTR.set(2,3,1,1,1,dyaw,dp,dr,atime-1000);
//mTR.set(2,3,1+1,1,1,dyaw,dp,dr,atime+1000);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-18 01:13:36
|
Revision: 128
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=128&view=rev
Author: tfoote
Date: 2008-04-17 18:13:41 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
returning a string instead of directly displaying to the terminal
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-18 00:50:56 UTC (rev 127)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-18 01:13:41 UTC (rev 128)
@@ -183,24 +183,25 @@
}
-void TransformReference::view(unsigned int target_frame, unsigned int source_frame)
+std::string TransformReference::viewChain(unsigned int target_frame, unsigned int source_frame)
{
+ stringstream mstream;
TransformLists lists = lookUpList(target_frame, source_frame);
- std::cout << "Inverse Transforms:" <<std::endl;
+ mstream << "Inverse Transforms:" <<std::endl;
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
- std::cout << lists.inverseTransforms[i]<<", ";
+ mstream << lists.inverseTransforms[i]<<", ";
// retMat *= getFrame(lists.inverseTransforms[i])->getInverseMatrix();
}
- std::cout << std::endl;
+ mstream << std::endl;
- std::cout << "Forward Transforms: "<<std::endl ;
+ mstream << "Forward Transforms: "<<std::endl ;
for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
{
- std::cout << lists.forwardTransforms[i]<<", ";
+ mstream << lists.forwardTransforms[i]<<", ";
// retMat *= getFrame(lists.inverseTransforms[lists.forwardTransforms.size() -1 - i])->getMatrix(); //Do this list backwards for it was generated traveling the wrong way
}
- std::cout << std::endl;
-
+ mstream << std::endl;
+ return mstream.str();
}
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-18 00:50:56 UTC (rev 127)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-18 01:13:41 UTC (rev 128)
@@ -39,15 +39,23 @@
#include <math.h>
#include <vector>
#include "Quaternion3D.hh"
+#include <sstream>
+/** 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;};
@@ -60,6 +68,15 @@
};
+/*** 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:
@@ -87,7 +104,7 @@
// TransformReference::MaxDepthException
/* Debugging function that will print to std::cout the transformation matrix */
- void view(unsigned int target_frame, unsigned int source_frame);
+ std::string viewChain(unsigned int target_frame, unsigned int source_frame);
// Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
// TransformReference::MaxDepthException
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-18 00:50:56 UTC (rev 127)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-18 01:13:41 UTC (rev 128)
@@ -36,7 +36,7 @@
//Demonstrate InvalidFrame LookupException
try
{
- mTR.view(10,9);
+ std::cout<< mTR.viewChain(10,9);
}
catch (TransformReference::LookupException &ex)
{
@@ -46,7 +46,7 @@
// See the list of transforms to get between the frames
std::cout<<"Viewing (10,8):"<<std::endl;
- mTR.view(10,8);
+ std::cout << mTR.viewChain(10,8);
//See the resultant transform
@@ -62,7 +62,7 @@
mTR.set(6,7,dx,dy,dz,dyaw,dp,dr,atime);
try {
- mTR.view(10,8);
+ std::cout<<mTR.viewChain(10,8);
}
catch (TransformReference::MaxDepthException &ex)
{
@@ -73,7 +73,7 @@
mTR.set(6,0,dx,dy,dz,dyaw,dp,dr,atime);
try {
- mTR.view(10,8);
+ std::cout<<mTR.viewChain(10,8);
}
catch (TransformReference::ConnectivityException &ex)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-03 01:06:49
|
Revision: 78
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=78&view=rev
Author: tfoote
Date: 2008-04-02 18:06:55 -0700 (Wed, 02 Apr 2008)
Log Message:
-----------
adding dhparam into RefFrame methods
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-02 23:56:19 UTC (rev 77)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-03 01:06:55 UTC (rev 78)
@@ -6,8 +6,10 @@
return;
}
-void RefFrame::setParams(double a,double b,double c,double d,double e,double f)
+/* Six DOF version */
+void RefFrame::setParamsXYZYPR(double a,double b,double c,double d,double e,double f)
{
+ paramType = SIXDOF;
params[0]=a;
params[1]=b;
params[2]=c;
@@ -17,19 +19,47 @@
}
+/* DH Params version */
+void RefFrame::setParamsDH(double a,double b,double c,double d)
+{
+ paramType = DH;
+ params[0]=a;
+ params[1]=b;
+ params[2]=c;
+ params[3]=d;
+}
+
+
NEWMAT::Matrix RefFrame::getMatrix()
{
NEWMAT::Matrix mMat(4,4);
- fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
+ switch ( paramType)
+ {
+ case SIXDOF:
+ fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
+ break;
+ case DH:
+ fill_transformation_matrix_from_dh(mMat,params[0],params[1],params[2],params[3]);
+ }
return mMat;
}
NEWMAT::Matrix RefFrame::getInverseMatrix()
{
NEWMAT::Matrix mMat(4,4);
- fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
- //todo create a fill_inverse_transform_matrix call to be more efficient
- return mMat.i();
+ switch(paramType)
+ {
+ case SIXDOF:
+ fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
+ //todo create a fill_inverse_transform_matrix call to be more efficient
+ return mMat.i();
+ break;
+ case DH:
+ fill_transformation_matrix_from_dh(mMat,params[0],params[1],params[2],params[3]);
+ //todo create a fill_inverse_transform_matrix call to be more efficient
+ return mMat.i();
+ break;
+ }
}
@@ -53,7 +83,7 @@
frames[frameID] = new RefFrame();
getFrame(frameID)->setParent(parentID);
- getFrame(frameID)->setParams(a,b,c,d,e,f);
+ getFrame(frameID)->setParamsXYZYPR(a,b,c,d,e,f);
}
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-02 23:56:19 UTC (rev 77)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-03 01:06:55 UTC (rev 78)
@@ -13,13 +13,14 @@
/* The type of frame
* This determines how many different parameters to expect to be updated, versus fixed */
- // static enum FrameType {SIXDOF, DH //how to tell which mode it's in //todo add this process i'm going to start with 6dof only
+ enum ParamTypeEnum {SIXDOF, DH };//how to tell which mode it's in //todo add this process i'm going to start with 6dof only
/* Constructor */
RefFrame();
/* Set the parameters for this frame */
- void setParams(double, double, double, double, double, double);
+ void setParamsXYZYPR(double, double, double, double, double, double);
+ void setParamsDH(double, double, double, double);
/* Get the parent node */
inline unsigned int getParent(){return parent;};
@@ -33,7 +34,8 @@
/* Generate and return the transform associated with getting out of this frame. */
NEWMAT::Matrix getInverseMatrix();
private:
- /* Storage of the parametsrs */
+ /* Storage of the parametsrs
+ * NOTE: Depending on if this is a 6dof or DH parameter the storage will be different. */
double params[6];
/* A helper function to build a homogeneous transform based on 6dof parameters */
@@ -42,12 +44,14 @@
double pitch, double roll);
/* A helper function to build a homogeneous transform based on DH parameters */
- bool fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
+ static bool fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
double length, double distance, double alpha);
/* Storage of the parent */
unsigned int parent;
+ ParamTypeEnum paramType;
+
};
class TransformReference
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-03 19:37:06
|
Revision: 87
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=87&view=rev
Author: tfoote
Date: 2008-04-03 12:37:11 -0700 (Thu, 03 Apr 2008)
Log Message:
-----------
cleaning up and documenting libTF much more
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-03 08:58:53 UTC (rev 86)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-03 19:37:11 UTC (rev 87)
@@ -229,9 +229,9 @@
throw(NoFrameConnectivity);
/* Make sure that we don't have a no parent at the top */
- std::cout << "Back = " << mTfLs.inverseTransforms.back()<<" " << mTfLs.forwardTransforms.back();
- // if (mTfLs.inverseTransforms.back() == NO_PARENT || mTfLs.forwardTransforms.back() == NO_PARENT)
- // throw(NoFrameConnectivity);
+ // std::cout << "Back = " << mTfLs.inverseTransforms.back()<<" " << mTfLs.forwardTransforms.back();
+ if (mTfLs.inverseTransforms.back() == NO_PARENT || mTfLs.forwardTransforms.back() == NO_PARENT)
+ throw(NoFrameConnectivity);
while (mTfLs.inverseTransforms.back() == mTfLs.forwardTransforms.back())
{
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-03 08:58:53 UTC (rev 86)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-03 19:37:11 UTC (rev 87)
@@ -33,10 +33,12 @@
/* Generate and return the transform associated with getting out of this frame. */
NEWMAT::Matrix getInverseMatrix();
+
+
private:
/* Storage of the parametsrs
* NOTE: Depending on if this is a 6dof or DH parameter the storage will be different. */
- double params[6];
+ double params[6];
/* A helper function to build a homogeneous transform based on 6dof parameters */
static bool fill_transformation_matrix(NEWMAT::Matrix& matrix_pointer, double ax,
@@ -57,26 +59,40 @@
class TransformReference
{
public:
- static const unsigned int ROOT_FRAME = 1;
- static const unsigned int NO_PARENT = 0;
+ /************* Constants ***********************/
+ 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;
+ 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. */
void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double);
+ // Possible exceptions TransformReference::LookupException
- /* Get the transform between two frames. */
+ /*********** Accessors *************/
+
+ /* Get the transform between two frames by frame ID. */
NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame);
+ // Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
+ // TransformReference::MaxDepthException
- /* Constructor */
- TransformReference();
-
/* Debugging function that will print to std::cout the transformation matrix */
void view(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
{
@@ -92,7 +108,7 @@
private:
} NoFrameConnectivity;
- /* An exception class to notify of no connection */
+ /* An exception class to notify that the search for connectivity descended too deep. */
class MaxDepthException : public std::exception
{
public:
@@ -100,24 +116,26 @@
private:
} MaxSearchDepth;
-
private:
-
- /* 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];};
+ /******************** Internal Storage ****************/
/* The pointers to potential frames that the tree can be made of.
- * The frames will be dynamically created at run time when set the first time. */
+ * 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. */
+ /* 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);
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-03 08:58:53 UTC (rev 86)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-03 19:37:11 UTC (rev 87)
@@ -9,135 +9,67 @@
TransformReference mTR;
- // while (true)
+
+ dx = dy= dz = 0;
+ dyaw = dp = dr = .1;
+
+
+ //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);
+
+
+ //Demonstrate InvalidFrame LookupException
+ try
{
- /* cout << "delta x:";
- cin >> dx;
- cout << "delta y:";
- cin >> dy;
- cout << "delta z:";
- cin >> dz;
- cout << "delta yaw:";
- cin >> dyaw;
- cout << "delta pitch:";
- cin >> dp;
- cout << "delta roll:";
- cin >> dr;
- */
- dx = dy= dz = 0;
- dyaw = dp = dr = .1;
-
-
-
- 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);
- try
- {
- mTR.view(10,9);
- }
- catch (TransformReference::LookupException &ex)
- {
- std::cout << "Caught " << ex.what()<<std::endl;
- }
-
- std::cout<<"Viewing (10,8)";
- mTR.view(10,8);
-
+ mTR.view(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;
+ mTR.view(10,8);
+
+
+ //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);
+
+ std::cout << "Result of get(10,8):" << std::endl << mat<< std::endl;
-
- std::cout <<"Calling get(10,8)"<<std::endl;
- // NEWMAT::Matrix mat = mTR.get(1,1);
- NEWMAT::Matrix mat = mTR.get(10,8);
-
- std::cout << "Result" << std::endl << mat<< std::endl;
-
-
-
-
- mTR.set(6,7,dx,dy,dz,dyaw,dp,dr);
-
- try {
- mTR.view(10,8);
- }
- catch (TransformReference::MaxDepthException &ex)
- {
- std::cout <<"caught loop in graph"<<std::endl;
- }
-
- mTR.set(6,0,dx,dy,dz,dyaw,dp,dr);
-
- try {
- mTR.view(10,8);
- }
- catch (TransformReference::ConnectivityException &ex)
- {
- std::cout <<"caught unconnected frame"<<std::endl;
- }
-
-
-
-
-
-#ifdef DONOTUSE
-
- // NEWMAT::Matrix mat(4,4);
- NEWMAT::Matrix MatI(4,4);
- NEWMAT::Matrix MatI2(4,4);
-
-
- // double vec[] = {dx,dy,dz,dyaw,dp,dr};
- // mat <<vec;
- double init_time = (double)clock()/(double) CLOCKS_PER_SEC;
- int iterations = 1;
- for (int i = 0; i < iterations; i++)
- {
- MatI = mat.i();
- }
- std::cout <<"Straight Inverse" <<std::endl<< MatI;
- std::cout << "This should be Identity if Inverse is right: "<<std::endl << MatI * mat;
- std::cout << "Time Elapsed for "<<iterations<<"iterations is:"<<(double)clock()/(double) CLOCKS_PER_SEC - init_time <<std::endl;
- std::cout << "Net: "<<((double)clock()/(double) CLOCKS_PER_SEC -init_time)/(double)iterations <<std::endl;
-
-
- NEWMAT::Matrix trans_mask(4,4);
- trans_mask << 0 << 0 << 0 << -1
- << 0 << 0 << 0 << -1
- << 0 << 0 << 0 << -1
- << 0 << 0 << 0 << 1;
-
- NEWMAT::Matrix trans_fix(4,4);
- trans_fix << 1 << 0 << 0 << 0
- << 0 << 1 << 0 << 0
- << 0 << 0 << 1 << 0
- << 0 << 0 << 0 << 0;
-
-
- NEWMAT::Matrix rot_mask(4,4);
- rot_mask << 1 << 1 << 1 << 0
- << 1 << 1 << 1 << 0
- << 1 << 1 << 1 << 0
- << 0 << 0 << 0 << 1;
-
- init_time = (double)clock()/(double) CLOCKS_PER_SEC;
- for (int i = 0; i < iterations; i++)
- {
- MatI2 = SP(mat, rot_mask).t();
- MatI2 *= (SP(mat,trans_mask) + trans_fix);
- }
- std::cout <<"My Inverse:" <<std::endl<< MatI2;
- std::cout << "Product: " <<std::endl<< MatI2 * mat;
- std::cout << "Time Elapsed for method 2 "<<iterations<<" iterations is:"<<(double)clock()/(double) CLOCKS_PER_SEC - init_time <<std::endl;
- std::cout << "Net: "<<((double)clock()/(double) CLOCKS_PER_SEC - init_time) /(double)iterations <<std::endl;
-
-
- std::cout <<"difference: "<<std::endl<<MatI - MatI2;
-#endif //DONOTUSE
-
+
+
+ //Break the graph, making it loop and demonstrate catching MaxDepthException
+ mTR.set(6,7,dx,dy,dz,dyaw,dp,dr);
+
+ try {
+ mTR.view(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.set(6,0,dx,dy,dz,dyaw,dp,dr);
+
+ try {
+ mTR.view(10,8);
+ }
+ catch (TransformReference::ConnectivityException &ex)
+ {
+ std::cout <<"caught unconnected frame"<<std::endl;
+ }
+
return 0;
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-10 18:47:05
|
Revision: 96
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=96&view=rev
Author: tfoote
Date: 2008-04-10 11:46:41 -0700 (Thu, 10 Apr 2008)
Log Message:
-----------
adding license
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-10 17:08:27 UTC (rev 95)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-10 18:46:41 UTC (rev 96)
@@ -1,3 +1,35 @@
+//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 "libTF.hh"
RefFrame::RefFrame() :
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-10 17:08:27 UTC (rev 95)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-10 18:46:41 UTC (rev 96)
@@ -1,3 +1,35 @@
+//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>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-16 22:26:54
|
Revision: 106
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=106&view=rev
Author: tfoote
Date: 2008-04-16 15:16:17 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
first cut at quaternion class
Modified Paths:
--------------
pkg/trunk/libTF/simple/Makefile
Added Paths:
-----------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/testQuat.cc
Modified: pkg/trunk/libTF/simple/Makefile
===================================================================
--- pkg/trunk/libTF/simple/Makefile 2008-04-16 21:16:16 UTC (rev 105)
+++ pkg/trunk/libTF/simple/Makefile 2008-04-16 22:16:17 UTC (rev 106)
@@ -3,8 +3,13 @@
all: test
+#%.o:%.cc
+#%.o:%.hh
+quat: testQuat.cc Quaternion3D.o
+ g++ -g -O2 $^ -o quat -lnewmat
+
test: main.cc libTF.o
g++ -g -O2 $^ -o test -lnewmat
Added: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc (rev 0)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 22:16:17 UTC (rev 106)
@@ -0,0 +1,73 @@
+#include "Quaternion3D.hh"
+
+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)
+{
+ return;
+};
+
+
+Quaternion3D::Quaternion3D(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)
+{
+ Normalize();
+ return;
+};
+
+
+void Quaternion3D::Normalize()
+{
+ double mag = getMagnitude();
+ xr /= mag;
+ yr /= mag;
+ zr /= mag;
+ w /= mag;
+};
+
+double Quaternion3D::getMagnitude()
+{
+ return sqrt(xr*xr + yr*yr + zr*zr + w*w);
+};
+
+
+NEWMAT::Matrix Quaternion3D::asMatrix()
+{
+ NEWMAT::Matrix outMat(4,4);
+
+ double * mat = outMat.Store();
+
+ // math from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+ double xx = xr * xr;
+ double xy = xr * yr;
+ double xz = xr * zr;
+ double xw = xr * w;
+ double yy = yr * yr;
+ double yz = yr * zr;
+ double yw = yr * w;
+ double zz = zr * zr;
+ double zw = zr * w;
+ mat[0] = 1 - 2 * ( yy + zz );
+ mat[4] = 2 * ( xy - zw );
+ mat[8] = 2 * ( xz + yw );
+ mat[1] = 2 * ( xy + zw );
+ mat[5] = 1 - 2 * ( xx + zz );
+ mat[9] = 2 * ( yz - xw );
+ mat[2] = 2 * ( xz - yw );
+ mat[6] = 2 * ( yz + xw );
+ mat[10] = 1 - 2 * ( xx + yy );
+ mat[12] = mat[13] = mat[14] = 0;
+ mat[3] = xt;
+ mat[7] = yt;
+ mat[11] = zt;
+ mat[15] = 1;
+
+
+ return outMat;
+};
+
+
+void Quaternion3D::printMatrix()
+{
+ std::cout << asMatrix();
+
+};
Added: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh (rev 0)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 22:16:17 UTC (rev 106)
@@ -0,0 +1,54 @@
+#ifndef QUATERNION3D_HH
+#define QUATERNION3D_HH
+
+#include <iostream>
+#include <newmat/newmat.h>
+#include <newmat/newmatio.h>
+#include <math.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:
+ //Constructor
+ Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
+
+ 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 Normalize();
+ double getMagnitude();
+
+ NEWMAT::Matrix asMatrix();
+ void printMatrix();
+
+
+private:
+ //Quaternion Storage
+ double xt,yt,zt,xr,yr,zr,w;
+
+
+};
+
+
+
+
+
+
+
+
+
+
+
+
+#endif //QUATERNION3D_HH
Added: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc (rev 0)
+++ pkg/trunk/libTF/simple/testQuat.cc 2008-04-16 22:16:17 UTC (rev 106)
@@ -0,0 +1,13 @@
+
+
+#include "Quaternion3D.hh"
+
+
+int main()
+{
+ Quaternion3D myquat(1,1,1,1,1,1,1);
+
+ myquat.printMatrix();
+
+ return 0;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-16 22:39:40
|
Revision: 107
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=107&view=rev
Author: tfoote
Date: 2008-04-16 15:39:37 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
adding an import from homogeneous method
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/testQuat.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 22:16:17 UTC (rev 106)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 22:39:37 UTC (rev 107)
@@ -14,7 +14,60 @@
return;
};
+Quaternion3D::Quaternion3D(NEWMAT::Matrix matIn)
+{
+ // math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+ double * mat = matIn.Store();
+ //Get the translations
+ xt = mat[3];
+ yt = mat[7];
+ zt = mat[11];
+
+ //TODO ASSERT others are zero and one as they should be
+
+
+ double T = 1 + mat[0] + mat[5] + mat[10];
+
+
+ // If the trace of the matrix is greater than zero, then
+ // perform an "instant" calculation.
+ // Important note wrt. rouning errors:
+
+ 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;
+ }
+ //If the trace of the matrix is equal to zero then identify
+ // which major diagonal element has the greatest value.
+ // Depending on this, calculate the following:
+
+ 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;
+ } 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;
+ } 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;
+ }
+};
+
+
void Quaternion3D::Normalize()
{
double mag = getMagnitude();
@@ -36,7 +89,7 @@
double * mat = outMat.Store();
- // math from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+ // math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
double xx = xr * xr;
double xy = xr * yr;
double xz = xr * zr;
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 22:16:17 UTC (rev 106)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 22:39:37 UTC (rev 107)
@@ -22,6 +22,7 @@
public:
//Constructor
Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
+ Quaternion3D(NEWMAT::Matrix matIn);
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;} ;
Modified: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc 2008-04-16 22:16:17 UTC (rev 106)
+++ pkg/trunk/libTF/simple/testQuat.cc 2008-04-16 22:39:37 UTC (rev 107)
@@ -7,7 +7,11 @@
{
Quaternion3D myquat(1,1,1,1,1,1,1);
+ Quaternion3D my2ndquat(myquat.asMatrix());
+ std::cout << "Quat1"<<std::endl;
myquat.printMatrix();
+ std::cout << "Quat2"<<std::endl;
+ my2ndquat.printMatrix();
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-16 23:53:33
|
Revision: 108
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=108&view=rev
Author: tfoote
Date: 2008-04-16 16:53:39 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
adding fromDH Params for Quaternion, and starting to switch to the new class
Modified Paths:
--------------
pkg/trunk/libTF/simple/Makefile
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/Makefile
===================================================================
--- pkg/trunk/libTF/simple/Makefile 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/Makefile 2008-04-16 23:53:39 UTC (rev 108)
@@ -10,7 +10,7 @@
quat: testQuat.cc Quaternion3D.o
g++ -g -O2 $^ -o quat -lnewmat
-test: main.cc libTF.o
+test: main.cc libTF.o Quaternion3D.o
g++ -g -O2 $^ -o test -lnewmat
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 23:53:39 UTC (rev 108)
@@ -14,8 +14,13 @@
return;
};
-Quaternion3D::Quaternion3D(NEWMAT::Matrix matIn)
+Quaternion3D::Quaternion3D(NEWMAT::Matrix matrixIn)
{
+ fromMatrix(matrixIn);
+};
+
+void Quaternion3D::fromMatrix(NEWMAT::Matrix matIn)
+{
// math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
double * mat = matIn.Store();
@@ -67,7 +72,90 @@
}
};
+void Quaternion3D::fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
+{
+ fromMatrix(matrixFromEuler(_x,_y,_z,_yaw,_pitch,_roll));
+};
+void Quaternion3D::fromDH(double theta,
+ double length, double distance, double alpha)
+{
+ fromMatrix(matrixFromDH(theta, length, distance, alpha));
+};
+
+
+NEWMAT::Matrix Quaternion3D::matrixFromEuler(double ax,
+ double ay, double az, double yaw,
+ double pitch, double roll)
+{
+ NEWMAT::Matrix matrix(4,4);
+ double ca = cos(yaw);
+ double sa = sin(yaw);
+ double cb = cos(pitch);
+ double sb = sin(pitch);
+ double cg = cos(roll);
+ double sg = sin(roll);
+ double sbsg = sb*sg;
+ double sbcg = sb*cg;
+
+
+ double* matrix_pointer = matrix.Store();
+
+ matrix_pointer[0] = ca*cb;
+ matrix_pointer[1] = (ca*sbsg)-(sa*cg);
+ matrix_pointer[2] = (ca*sbcg)+(sa*sg);
+ matrix_pointer[3] = ax;
+ matrix_pointer[4] = sa*cb;
+ matrix_pointer[5] = (sa*sbsg)+(ca*cg);
+ matrix_pointer[6] = (sa*sbcg)-(ca*sg);
+ matrix_pointer[7] = ay;
+ matrix_pointer[8] = -sb;
+ matrix_pointer[9] = cb*sg;
+ matrix_pointer[10] = cb*cg;
+ matrix_pointer[11] = az;
+ matrix_pointer[12] = 0.0;
+ matrix_pointer[13] = 0.0;
+ matrix_pointer[14] = 0.0;
+ matrix_pointer[15] = 1.0;
+
+ return matrix;
+};
+
+
+// Math from http://en.wikipedia.org/wiki/Robotics_conventions
+NEWMAT::Matrix Quaternion3D::matrixFromDH(double theta,
+ double length, double distance, double alpha)
+{
+ NEWMAT::Matrix matrix(4,4);
+
+ double ca = cos(alpha);
+ double sa = sin(alpha);
+ double ct = cos(theta);
+ double st = sin(theta);
+
+ double* matrix_pointer = matrix.Store();
+
+ matrix_pointer[0] = ct;
+ matrix_pointer[1] = -st*ca;
+ matrix_pointer[2] = st*sa;
+ matrix_pointer[3] = distance * ct;
+ matrix_pointer[4] = st;
+ matrix_pointer[5] = ct*ca;
+ matrix_pointer[6] = -ct*sa;
+ matrix_pointer[7] = distance*st;
+ matrix_pointer[8] = 0;
+ matrix_pointer[9] = sa;
+ matrix_pointer[10] = ca;
+ matrix_pointer[11] = length;
+ matrix_pointer[12] = 0.0;
+ matrix_pointer[13] = 0.0;
+ matrix_pointer[14] = 0.0;
+ matrix_pointer[15] = 1.0;
+
+ return matrix;
+};
+
+
void Quaternion3D::Normalize()
{
double mag = getMagnitude();
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 23:53:39 UTC (rev 108)
@@ -20,17 +20,38 @@
class Quaternion3D {
public:
- //Constructor
+ /** Constructors **/
+ // Standard constructor which takes in 7 doubles
Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
- Quaternion3D(NEWMAT::Matrix matIn);
-
+ // Constructor from Matrix
+ Quaternion3D(NEWMAT::Matrix matrixIn);
+
+ /** 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;} ;
+ //Set the values from a matrix
+ void fromMatrix(NEWMAT::Matrix matIn);
+ void fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
+ void fromDH(double theta, double length, double distance, double alpha);
+
+ /**** Utility Functions ****/
+ static NEWMAT::Matrix matrixFromDH(double theta,
+ double length, double distance, double alpha);
+ static NEWMAT::Matrix matrixFromEuler(double ax,
+ double ay, double az, double yaw,
+ double pitch, double roll);
+
+ // Utility functions to normalize and get magnitude.
void Normalize();
double getMagnitude();
+ /** Accessors **/
+ // Return a Matrix
NEWMAT::Matrix asMatrix();
+
+ //Print as a matrix
void printMatrix();
@@ -39,6 +60,7 @@
double xt,yt,zt,xr,yr,zr,w;
+
};
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-16 23:53:39 UTC (rev 108)
@@ -33,7 +33,8 @@
#include "libTF.hh"
RefFrame::RefFrame() :
- parent(0)
+ parent(0),
+ myQuat(0,0,0,0,0,0,0)
{
return;
}
@@ -48,7 +49,8 @@
params[3]=d;
params[4]=e;
params[5]=f;
-
+
+ myQuat.fromEuler(a,b,c,d,e,f);
}
/* DH Params version */
@@ -68,7 +70,9 @@
switch ( paramType)
{
case SIXDOF:
- fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
+ //temp insert
+ return myQuat.asMatrix();
+ // fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
break;
case DH:
fill_transformation_matrix_from_dh(mMat,params[0],params[1],params[2],params[3]);
@@ -82,6 +86,7 @@
switch(paramType)
{
case SIXDOF:
+ return myQuat.asMatrix().i();
fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
//todo create a fill_inverse_transform_matrix call to be more efficient
return mMat.i();
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-16 23:53:39 UTC (rev 108)
@@ -38,6 +38,7 @@
#include <newmat/newmatio.h>
#include <math.h>
#include <vector>
+#include "Quaternion3D.hh"
class RefFrame
{
@@ -72,6 +73,8 @@
* NOTE: Depending on if this is a 6dof or DH parameter the storage will be different. */
double params[6];
+ Quaternion3D myQuat;
+
/* A helper function to build a homogeneous transform based on 6dof parameters */
static bool fill_transformation_matrix(NEWMAT::Matrix& matrix_pointer, double ax,
double ay, double az, double yaw,
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-16 22:39:37 UTC (rev 107)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-16 23:53:39 UTC (rev 108)
@@ -1,3 +1,4 @@
+#include "Quaternion3D.hh"
#include "libTF.hh"
#include <time.h>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-16 23:55:03
|
Revision: 109
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=109&view=rev
Author: tfoote
Date: 2008-04-16 16:55:11 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
adding licensing
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/testQuat.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 23:53:39 UTC (rev 108)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-16 23:55:11 UTC (rev 109)
@@ -1,3 +1,35 @@
+// 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"
Euler3D::Euler3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll) :
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 23:53:39 UTC (rev 108)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-16 23:55:11 UTC (rev 109)
@@ -1,3 +1,35 @@
+// 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
Modified: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc 2008-04-16 23:53:39 UTC (rev 108)
+++ pkg/trunk/libTF/simple/testQuat.cc 2008-04-16 23:55:11 UTC (rev 109)
@@ -1,3 +1,35 @@
+// 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"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 00:03:02
|
Revision: 110
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=110&view=rev
Author: tfoote
Date: 2008-04-16 17:03:10 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
stripping out stuff that has been replaced
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-16 23:55:11 UTC (rev 109)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-17 00:03:10 UTC (rev 110)
@@ -34,70 +34,33 @@
RefFrame::RefFrame() :
parent(0),
- myQuat(0,0,0,0,0,0,0)
+ myQuat(0,0,0,0,0,0,1)
{
return;
}
/* Six DOF version */
-void RefFrame::setParamsXYZYPR(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)
{
- paramType = SIXDOF;
- params[0]=a;
- params[1]=b;
- params[2]=c;
- params[3]=d;
- params[4]=e;
- params[5]=f;
-
myQuat.fromEuler(a,b,c,d,e,f);
}
/* DH Params version */
void RefFrame::setParamsDH(double a,double b,double c,double d)
{
- paramType = DH;
- params[0]=a;
- params[1]=b;
- params[2]=c;
- params[3]=d;
+ myQuat.fromDH(a,b,c,d);
}
NEWMAT::Matrix RefFrame::getMatrix()
{
- NEWMAT::Matrix mMat(4,4);
- switch ( paramType)
- {
- case SIXDOF:
- //temp insert
- return myQuat.asMatrix();
- // fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
- break;
- case DH:
- fill_transformation_matrix_from_dh(mMat,params[0],params[1],params[2],params[3]);
- }
- return mMat;
+ return myQuat.asMatrix();
}
NEWMAT::Matrix RefFrame::getInverseMatrix()
{
- NEWMAT::Matrix mMat(4,4);
- switch(paramType)
- {
- case SIXDOF:
- return myQuat.asMatrix().i();
- fill_transformation_matrix(mMat,params[0],params[1],params[2],params[3],params[4],params[5]);
- //todo create a fill_inverse_transform_matrix call to be more efficient
- return mMat.i();
- break;
- case DH:
- fill_transformation_matrix_from_dh(mMat,params[0],params[1],params[2],params[3]);
- //todo create a fill_inverse_transform_matrix call to be more efficient
- return mMat.i();
- break;
- }
-}
+ return myQuat.asMatrix().i();
+};
TransformReference::TransformReference()
@@ -120,7 +83,7 @@
frames[frameID] = new RefFrame();
getFrame(frameID)->setParent(parentID);
- getFrame(frameID)->setParamsXYZYPR(a,b,c,d,e,f);
+ getFrame(frameID)->setParamsEulers(a,b,c,d,e,f);
}
@@ -134,81 +97,6 @@
}
-
-
-bool RefFrame::fill_transformation_matrix(NEWMAT::Matrix & matrix, double ax,
- double ay, double az, double yaw,
- double pitch, double roll)
-{
- double ca = cos(yaw);
- double sa = sin(yaw);
- double cb = cos(pitch);
- double sb = sin(pitch);
- double cg = cos(roll);
- double sg = sin(roll);
- double sbsg = sb*sg;
- double sbcg = sb*cg;
-
-
- double* matrix_pointer = matrix.Store();
- if (matrix.Storage() != 16)
- return false;
-
- matrix_pointer[0] = ca*cb;
- matrix_pointer[1] = (ca*sbsg)-(sa*cg);
- matrix_pointer[2] = (ca*sbcg)+(sa*sg);
- matrix_pointer[3] = ax;
- matrix_pointer[4] = sa*cb;
- matrix_pointer[5] = (sa*sbsg)+(ca*cg);
- matrix_pointer[6] = (sa*sbcg)-(ca*sg);
- matrix_pointer[7] = ay;
- matrix_pointer[8] = -sb;
- matrix_pointer[9] = cb*sg;
- matrix_pointer[10] = cb*cg;
- matrix_pointer[11] = az;
- matrix_pointer[12] = 0.0;
- matrix_pointer[13] = 0.0;
- matrix_pointer[14] = 0.0;
- matrix_pointer[15] = 1.0;
-
- return true;
-};
-
-// Math from http://en.wikipedia.org/wiki/Robotics_conventions
-bool RefFrame::fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
- double length, double distance, double alpha)
-{
- double ca = cos(alpha);
- double sa = sin(alpha);
- double ct = cos(theta);
- double st = sin(theta);
-
- double* matrix_pointer = matrix.Store();
- if (matrix.Storage() != 16)
- return false;
-
- matrix_pointer[0] = ct;
- matrix_pointer[1] = -st*ca;
- matrix_pointer[2] = st*sa;
- matrix_pointer[3] = distance * ct;
- matrix_pointer[4] = st;
- matrix_pointer[5] = ct*ca;
- matrix_pointer[6] = -ct*sa;
- matrix_pointer[7] = distance*st;
- matrix_pointer[8] = 0;
- matrix_pointer[9] = sa;
- matrix_pointer[10] = ca;
- matrix_pointer[11] = length;
- matrix_pointer[12] = 0.0;
- matrix_pointer[13] = 0.0;
- matrix_pointer[14] = 0.0;
- matrix_pointer[15] = 1.0;
-
- return true;
-};
-
-
-
TransformReference::TransformLists TransformReference::lookUpList(unsigned int target_frame, unsigned int source_frame)
{
TransformLists mTfLs;
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-16 23:55:11 UTC (rev 109)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-17 00:03:10 UTC (rev 110)
@@ -44,15 +44,11 @@
{
public:
- /* The type of frame
- * This determines how many different parameters to expect to be updated, versus fixed */
- enum ParamTypeEnum {SIXDOF, DH };//how to tell which mode it's in //todo add this process i'm going to start with 6dof only
-
/* Constructor */
RefFrame();
/* Set the parameters for this frame */
- void setParamsXYZYPR(double, double, double, double, double, double);
+ void setParamsEulers(double, double, double, double, double, double);
void setParamsDH(double, double, double, double);
/* Get the parent node */
@@ -69,26 +65,12 @@
private:
- /* Storage of the parametsrs
- * NOTE: Depending on if this is a 6dof or DH parameter the storage will be different. */
- double params[6];
-
+ /* Storage of the parametsrs */
Quaternion3D myQuat;
- /* A helper function to build a homogeneous transform based on 6dof parameters */
- static bool fill_transformation_matrix(NEWMAT::Matrix& matrix_pointer, double ax,
- double ay, double az, double yaw,
- double pitch, double roll);
-
- /* A helper function to build a homogeneous transform based on DH parameters */
- static bool fill_transformation_matrix_from_dh(NEWMAT::Matrix& matrix, double theta,
- double length, double distance, double alpha);
-
/* Storage of the parent */
unsigned int parent;
- ParamTypeEnum paramType;
-
};
class TransformReference
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 01:46:17
|
Revision: 111
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=111&view=rev
Author: tfoote
Date: 2008-04-16 18:46:21 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
adding a set quaternion method
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-17 00:03:10 UTC (rev 110)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-17 01:46:21 UTC (rev 111)
@@ -39,6 +39,12 @@
return;
}
+/* Quaternion 3D version */
+void RefFrame::setParamsQuaternion3D(double a,double b,double c,double d,double e,double f, double g)
+{
+ myQuat.Set(a,b,c,d,e,f,g);
+};
+
/* Six DOF version */
void RefFrame::setParamsEulers(double a,double b,double c,double d,double e,double f)
{
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-17 00:03:10 UTC (rev 110)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-17 01:46:21 UTC (rev 111)
@@ -48,6 +48,7 @@
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);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 01:46:29
|
Revision: 112
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=112&view=rev
Author: tfoote
Date: 2008-04-16 18:46:36 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
adding interpolation linked list and methods
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 01:46:21 UTC (rev 111)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 01:46:36 UTC (rev 112)
@@ -40,14 +40,22 @@
Quaternion3D::Quaternion3D(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)
+ 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):
+ max_storage_time(MAX_STORAGE_TIME),
+ first(NULL),
+ last(NULL)
{
+ pthread_mutex_init( &linked_list_mutex, NULL);
fromMatrix(matrixIn);
};
@@ -244,3 +252,227 @@
std::cout << asMatrix();
};
+
+
+unsigned long long Quaternion3D::Qgettime()
+{
+ timeval temp_time_struct;
+ gettimeofday(&temp_time_struct,NULL);
+ return temp_time_struct.tv_sec * 1000000ULL + (unsigned long long)temp_time_struct.tv_usec;
+}
+
+
+bool Quaternion3D::getValue(Quaternion3DStorage& buff, unsigned long long time, long long &time_diff)
+{
+ Quaternion3DStorage p_temp_1;
+ Quaternion3DStorage p_temp_2;
+ // long long temp_time;
+ int num_nodes;
+
+ bool retval = false;
+
+ pthread_mutex_lock(&linked_list_mutex);
+ num_nodes = findClosest(p_temp_1,p_temp_2, time, time_diff);
+
+ if (num_nodes == 0)
+ retval= false;
+ else if (num_nodes == 1)
+ {
+ memcpy(&buff, &p_temp_1, sizeof(Quaternion3DStorage));
+ retval = true;
+ }
+ else
+ {
+ interpolate(p_temp_1, p_temp_2, time, buff);
+ retval = true;
+ }
+
+ pthread_mutex_unlock(&linked_list_mutex);
+
+
+ return retval;
+
+};
+
+void Quaternion3D::add_value(Quaternion3DStorage dataIn)
+{
+ Quaternion3DStorage temp;
+ //cout << "started thread" << endl;
+
+ pthread_mutex_lock(&linked_list_mutex);
+ insertNode(dataIn);
+ pruneList();
+ pthread_mutex_unlock(&linked_list_mutex);
+
+
+};
+
+
+void Quaternion3D::insertNode(Quaternion3DStorage new_val)
+{
+ data_LL* p_current;
+ data_LL* p_old;
+
+ // cout << "Inserting Node" << endl;
+
+ //Base case empty list
+ if (first == NULL)
+ {
+ cout << "Base case" << endl;
+ first = new data_LL;
+ first->data = new_val;
+ first->next = NULL;
+ first->previous = NULL;
+ last = first;
+ }
+ else
+ {
+ //Increment through until at the end of the list or in the right spot
+ p_current = first;
+ while (p_current != NULL && first->data.time > new_val.time)
+ {
+ //cout << "passed beyond " << p_current->data.time << endl;
+ p_current = p_current->next;
+ }
+
+ //THis means we hit the end of the list so just append the node
+ if (p_current == NULL)
+ {
+ //cout << "Appending node to the end" << endl;
+ p_current = new data_LL;
+ p_current->data = new_val;
+ p_current->previous = last;
+ p_current->next = NULL;
+
+ last = p_current;
+ }
+ else
+ {
+
+ // cout << "Found a place to put data into the list" << endl;
+
+ // Insert the new node
+ // Record where the old first node was
+ p_old = p_current;
+ //Fill in the new node
+ p_current = new data_LL;
+ p_current->data = new_val;
+ p_current->next = p_old;
+ p_current->previous = p_old->previous;
+
+ //point the old to the new
+ p_old->previous = p_current;
+
+ //If at the top of the list make sure we're not
+ if (p_current->previous == NULL)
+ first = p_current;
+ }
+
+
+
+ }
+};
+
+void Quaternion3D::pruneList()
+{
+ unsigned long long current_time = Qgettime();
+ data_LL* p_current = last;
+
+ // cout << "Pruning List" << endl;
+
+ //Empty Set
+ if (last == NULL) return;
+
+ //While time stamps too old
+ while (p_current->data.time + max_storage_time < current_time)
+ {
+ // cout << "Age of node " << (double)(-p_current->data.time + current_time)/1000000.0 << endl;
+ // Make sure that there's at least two elements in the list
+ if (p_current->previous != NULL)
+ {
+ if (p_current->previous->previous != NULL)
+ {
+ // Remove the last node
+ p_current->previous->next = NULL;
+ last = p_current->previous;
+ delete p_current;
+ p_current = last;
+ // cout << " Pruning Node" << endl;
+ }
+ else
+ break;
+ }
+ else
+ break;
+
+ }
+
+};
+
+
+
+int Quaternion3D::findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, unsigned long long target_time, long long &time_diff)
+{
+
+ unsigned long long current_time = Qgettime();
+ data_LL* p_current = first;
+
+
+ // Base case no list
+ if (first == NULL)
+ {
+ return 0;
+ }
+
+ //Case one element list
+ else if (first->next == NULL)
+ {
+ one = first->data;
+ time_diff = current_time - first->data.time;
+ return 1;
+ }
+
+ else
+ {
+ //Two or more elements
+ //Find the one that just exceeds the time or hits the end
+ //and then take the previous one
+ p_current = first->next; //Start on the 2nd element so if we fail we fall back to the first one
+ while (p_current->next != NULL && p_current->data.time > target_time)
+ {
+ p_current = p_current->next;
+ }
+
+ one = p_current->data;
+ two = p_current->previous->data;
+
+ //FIXME this should be the min distance not just one random one.
+ time_diff = target_time - two.time;
+ return 2;
+ }
+};
+
+
+void Quaternion3D::interpolate(Quaternion3DStorage &one, Quaternion3DStorage &two,unsigned long long target_time, Quaternion3DStorage& output)
+{
+ //fixme do a proper interpolatioln here!!!
+ output.time = target_time;
+
+ output.xt = interpolateDouble(one.xt, one.time, two.xt, two.time, target_time);
+ output.yt = interpolateDouble(one.yt, one.time, two.yt, two.time, target_time);
+ output.zt = interpolateDouble(one.zt, one.time, two.zt, two.time, target_time);
+ output.xr = interpolateDouble(one.xr, one.time, two.xr, two.time, target_time);
+ output.yr = interpolateDouble(one.yr, one.time, two.yr, two.time, target_time);
+ output.zr = interpolateDouble(one.zr, one.time, two.zr, two.time, target_time);
+ output.w = interpolateDouble(one.w, one.time, two.w, two.time, target_time);
+
+};
+
+double Quaternion3D::interpolateDouble(double first, unsigned long long first_time, double second, unsigned long long second_time, unsigned long long target_time)
+{
+ if ( first_time == second_time ) {
+ return first;
+ } else {
+ return first + (second-first)* (double)((target_time - first_time)/(second_time - first_time));
+ }
+};
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 01:46:21 UTC (rev 111)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 01:46:36 UTC (rev 112)
@@ -37,6 +37,8 @@
#include <newmat/newmat.h>
#include <newmat/newmatio.h>
#include <math.h>
+#include <pthread.h>
+#include <sys/time.h>
class Euler3D {
public:
@@ -45,13 +47,19 @@
//Storage
double x,y,z,yaw,pitch,roll;
+};
-
-};
+class Quaternion3D {
-class Quaternion3D {
public:
+ // Storage
+ struct Quaternion3DStorage
+ {
+ double xt, yt, zt, xr, yr, zr, w;
+ unsigned long long time;
+ };
+
/** Constructors **/
// Standard constructor which takes in 7 doubles
Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
@@ -62,9 +70,12 @@
// 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;} ;
+
//Set the values from a matrix
void fromMatrix(NEWMAT::Matrix matIn);
+ // Set the values using Euler angles
void fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
+ // Set the values using DH Parameters
void fromDH(double theta, double length, double distance, double alpha);
@@ -82,7 +93,7 @@
/** Accessors **/
// Return a Matrix
NEWMAT::Matrix asMatrix();
-
+
//Print as a matrix
void printMatrix();
@@ -91,8 +102,51 @@
//Quaternion Storage
double xt,yt,zt,xr,yr,zr,w;
+ /**** Linked List stuff ****/
+ static const unsigned int MAX_STORAGE_TIME = 100000000; // 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(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
+ void insertNode(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, 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(Quaternion3DStorage &one, Quaternion3DStorage &two, unsigned long long target_time, Quaternion3DStorage& output);
+
+ //Used by interpolate to interpolate between double values
+ double interpolateDouble(double, unsigned long long, double, unsigned long long, 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;
+
+
+
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 02:27:06
|
Revision: 114
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=114&view=rev
Author: tfoote
Date: 2008-04-16 19:27:14 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
removing debugging output, and demonstrating interpolation working
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 02:09:56 UTC (rev 113)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 02:27:14 UTC (rev 114)
@@ -334,7 +334,7 @@
//Base case empty list
if (first == NULL)
{
- cout << "Base case" << endl;
+ //cout << "Base case" << endl;
first = new data_LL;
first->data = new_val;
first->next = NULL;
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-17 02:09:56 UTC (rev 113)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-17 02:27:14 UTC (rev 114)
@@ -19,7 +19,8 @@
//Fill in some transforms
mTR.set(10,2,1,1,1,dyaw,dp,dr,atime);
- mTR.set(2,3,1,1,1,dyaw,dp,dr,atime);
+ mTR.set(2,3,1,1,1,dyaw,dp,dr+1,atime-1000);
+ mTR.set(2,3,1,1,1,dyaw,dp,dr-1,atime+1000);
mTR.set(3,5,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(5,1,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(6,5,dx,dy,dz,dyaw,dp,dr,atime);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 04:30:08
|
Revision: 115
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=115&view=rev
Author: tfoote
Date: 2008-04-16 21:30:14 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
updating test program to work with new time. And adding DHparams input ticket:16 and testing basic functionality.
Modified Paths:
--------------
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/libTF.hh
pkg/trunk/libTF/simple/main.cc
pkg/trunk/libTF/simple/testQuat.cc
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-17 04:30:14 UTC (rev 115)
@@ -92,7 +92,19 @@
getFrame(frameID)->setParamsEulers(a,b,c,d,e,f,time);
}
+void TransformReference::set(unsigned int frameID, unsigned int parentID, double a,double b,double c,double d, unsigned long long time)
+{
+ if (frameID > MAX_NUM_FRAMES || parentID > MAX_NUM_FRAMES || frameID == NO_PARENT || frameID == ROOT_FRAME)
+ throw InvalidFrame;
+
+ if (frames[frameID] == NULL)
+ frames[frameID] = new RefFrame();
+
+ getFrame(frameID)->setParent(parentID);
+ getFrame(frameID)->setParamsDH(a,b,c,d,time);
+}
+
NEWMAT::Matrix TransformReference::get(unsigned int target_frame, unsigned int source_frame, unsigned long long time)
{
NEWMAT::Matrix myMat(4,4);
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-17 04:30:14 UTC (rev 115)
@@ -90,6 +90,7 @@
/********** Mutators **************/
/* Set a new frame or update an old one. */
void set(unsigned int framid, unsigned int parentid, double,double,double,double,double,double,unsigned long long time);
+ void set(unsigned int framid, unsigned int parentid, double,double,double,double,unsigned long long time);
// Possible exceptions TransformReference::LookupException
/*********** Accessors *************/
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-17 04:30:14 UTC (rev 115)
@@ -18,14 +18,16 @@
//Fill in some transforms
- mTR.set(10,2,1,1,1,dyaw,dp,dr,atime);
+ // mTR.set(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
+ mTR.set(10,2,1,1,1,dyaw,atime);
mTR.set(2,3,1,1,1,dyaw,dp,dr+1,atime-1000);
mTR.set(2,3,1,1,1,dyaw,dp,dr-1,atime+1000);
mTR.set(3,5,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(5,1,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(6,5,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(7,6,1,1,1,dyaw,dp,dr,atime);
- mTR.set(8,7,1,1,1,dyaw,dp,dr,atime);
+ mTR.set(8,7,1,1,1,dyaw,atime);
+ // mTR.set(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
//Demonstrate InvalidFrame LookupException
Modified: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc 2008-04-17 02:27:14 UTC (rev 114)
+++ pkg/trunk/libTF/simple/testQuat.cc 2008-04-17 04:30:14 UTC (rev 115)
@@ -37,13 +37,13 @@
int main()
{
- Quaternion3D myquat(1,1,1,1,1,1,1);
+ Quaternion3D myquat(1,1,1,1,1,1,1,1000101010);
- Quaternion3D my2ndquat(myquat.asMatrix());
+ Quaternion3D my2ndquat(myquat.asMatrix(1000101010),1000101010);
std::cout << "Quat1"<<std::endl;
- myquat.printMatrix();
+ myquat.printMatrix(1000101010);
std::cout << "Quat2"<<std::endl;
- my2ndquat.printMatrix();
+ my2ndquat.printMatrix(1000101010);
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 21:41:45
|
Revision: 118
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=118&view=rev
Author: tfoote
Date: 2008-04-17 14:41:36 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
confirmed that time interpolation is working. and fixed small bug
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/main.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 19:51:25 UTC (rev 117)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 21:41:36 UTC (rev 118)
@@ -47,7 +47,7 @@
{
pthread_mutex_init( &linked_list_mutex, NULL);
- Normalize();
+ //fixme Normalize();
return;
};
@@ -124,7 +124,7 @@
temp.zr = 0.25 * S;
temp.w = (mat[1] - mat[4] ) / S;
}
-
+ xt = temp.xt; yt = temp.yt; zt = temp.zt; xr = temp.xr; yr = temp.yr; zr = temp.zr; w = temp.w;
add_value(temp);
};
@@ -212,7 +212,7 @@
};
-void Quaternion3D::Normalize()
+void Quaternion3D::Quaternion3DStorage::Normalize()
{
double mag = getMagnitude();
xr /= mag;
@@ -221,7 +221,7 @@
w /= mag;
};
-double Quaternion3D::getMagnitude()
+double Quaternion3D::Quaternion3DStorage::getMagnitude()
{
return sqrt(xr*xr + yr*yr + zr*zr + w*w);
};
@@ -229,20 +229,28 @@
NEWMAT::Matrix Quaternion3D::asMatrix(unsigned long long time)
{
+ Quaternion3DStorage temp;
+ long long diff_time;
+ getValue(temp, time, diff_time);
+
+ // printStorage(temp);
+ // std::cout <<"Locally: "<< xt <<", "<< yt <<", "<< zt <<", "<< xr <<", "<<yr <<", "<<zr <<", "<<w<<std::endl;
+ //std::cout << "time Difference: "<< diff_time<<std::endl;
+
NEWMAT::Matrix outMat(4,4);
double * mat = outMat.Store();
// math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
- double xx = xr * xr;
- double xy = xr * yr;
- double xz = xr * zr;
- double xw = xr * w;
- double yy = yr * yr;
- double yz = yr * zr;
- double yw = yr * w;
- double zz = zr * zr;
- double zw = zr * w;
+ double xx = temp.xr * temp.xr;
+ double xy = temp.xr * temp.yr;
+ double xz = temp.xr * temp.zr;
+ double xw = temp.xr * temp.w;
+ double yy = temp.yr * temp.yr;
+ double yz = temp.yr * temp.zr;
+ double yw = temp.yr * temp.w;
+ double zz = temp.zr * temp.zr;
+ double zw = temp.zr * temp.w;
mat[0] = 1 - 2 * ( yy + zz );
mat[4] = 2 * ( xy - zw );
mat[8] = 2 * ( xz + yw );
@@ -266,7 +274,11 @@
void Quaternion3D::printMatrix(unsigned long long time)
{
std::cout << asMatrix(time);
+};
+void Quaternion3D::printStorage(const Quaternion3DStorage& storage)
+{
+ std::cout << "Storage: " << storage.xt <<", " << storage.yt <<", " << storage.zt<<", " << storage.xr<<", " << storage.yr <<", " << storage.zr <<", " << storage.w<<std::endl;
};
@@ -430,7 +442,6 @@
int Quaternion3D::findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, unsigned long long target_time, long long &time_diff)
{
- unsigned long long current_time = Qgettime();
data_LL* p_current = first;
@@ -444,7 +455,7 @@
else if (first->next == NULL)
{
one = first->data;
- time_diff = current_time - first->data.time;
+ time_diff = target_time - first->data.time;
return 1;
}
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 19:51:25 UTC (rev 117)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 21:41:36 UTC (rev 118)
@@ -54,8 +54,13 @@
public:
// Storage
- struct Quaternion3DStorage
+ class Quaternion3DStorage
{
+ public:
+ // Utility functions to normalize and get magnitude.
+ void Normalize();
+ double getMagnitude();
+
double xt, yt, zt, xr, yr, zr, w;
unsigned long long time;
};
@@ -85,9 +90,6 @@
double ay, double az, double yaw,
double pitch, double roll);
- // Utility functions to normalize and get magnitude.
- void Normalize();
- double getMagnitude();
/** Accessors **/
// Return a Matrix
@@ -95,6 +97,7 @@
//Print as a matrix
void printMatrix(unsigned long long time);
+ void printStorage(const Quaternion3DStorage &storage);
// this is a function to return the current time in microseconds from the beginning of 1970
static unsigned long long Qgettime(void);
@@ -104,7 +107,7 @@
double xt,yt,zt,xr,yr,zr,w;
/**** Linked List stuff ****/
- static const unsigned int MAX_STORAGE_TIME = 100000000; // max of 100 seconds storage
+ static const long long MAX_STORAGE_TIME = 1000000000; // max of 100 seconds storage
struct data_LL{
Quaternion3DStorage data;
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-17 19:51:25 UTC (rev 117)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-17 21:41:36 UTC (rev 118)
@@ -12,7 +12,7 @@
dx = dy= dz = 0;
- dyaw = dp = dr = .1;
+ dyaw = dp = dr = 0.1;
unsigned long long atime = Quaternion3D::Qgettime();
@@ -20,14 +20,17 @@
//Fill in some transforms
// mTR.set(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
mTR.set(10,2,1,1,1,dyaw,atime);
- mTR.set(2,3,1,1,1,dyaw,dp,dr+1,atime-1000);
- mTR.set(2,3,1,1,1,dyaw,dp,dr-1,atime+1000);
+ //mTR.set(2,3,1-1,1,1,dyaw,dp,dr,atime-1000);
+ mTR.set(2,3,2,1,1,dyaw,dp,dr,atime-100);
+ mTR.set(2,3,1,1,1,dyaw,dp,dr,atime-50);
+ mTR.set(2,3,1,1,1,dyaw,dp,dr,atime-1000);
+ //mTR.set(2,3,1+1,1,1,dyaw,dp,dr,atime+1000);
mTR.set(3,5,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(5,1,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(6,5,dx,dy,dz,dyaw,dp,dr,atime);
mTR.set(7,6,1,1,1,dyaw,dp,dr,atime);
mTR.set(8,7,1,1,1,dyaw,atime);
- // mTR.set(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
+ //mTR.set(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
//Demonstrate InvalidFrame LookupException
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 22:18:57
|
Revision: 120
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=120&view=rev
Author: tfoote
Date: 2008-04-17 15:19:01 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
removing extraneous constructors
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/libTF.cc
pkg/trunk/libTF/simple/testQuat.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 21:47:02 UTC (rev 119)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 22:19:01 UTC (rev 120)
@@ -39,7 +39,7 @@
};
-Quaternion3D::Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time):
+Quaternion3D::Quaternion3D():
max_storage_time(MAX_STORAGE_TIME),
first(NULL),
last(NULL)
@@ -50,15 +50,6 @@
return;
};
-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, time);
-};
-
void Quaternion3D::Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time)
{
Quaternion3DStorage temp;
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 21:47:02 UTC (rev 119)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 22:19:01 UTC (rev 120)
@@ -66,10 +66,8 @@
};
/** Constructors **/
- // Standard constructor which takes in 7 doubles
- 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, unsigned long long time);
+ // Standard constructor
+ Quaternion3D();
/** Mutators **/
// Set the values manually
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-17 21:47:02 UTC (rev 119)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-17 22:19:01 UTC (rev 120)
@@ -34,7 +34,7 @@
RefFrame::RefFrame() :
parent(0),
- myQuat(0,0,0,0,0,0,1,111110000)
+ myQuat()
{
return;
}
Modified: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc 2008-04-17 21:47:02 UTC (rev 119)
+++ pkg/trunk/libTF/simple/testQuat.cc 2008-04-17 22:19:01 UTC (rev 120)
@@ -37,9 +37,11 @@
int main()
{
- Quaternion3D myquat(1,1,1,1,1,1,1,1000101010);
+ Quaternion3D myquat;
- Quaternion3D my2ndquat(myquat.asMatrix(1000101010),1000101010);
+ 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;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 23:37:39
|
Revision: 121
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=121&view=rev
Author: tfoote
Date: 2008-04-17 16:37:45 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
overloading << operator and adding const correctness checking
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 22:19:01 UTC (rev 120)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 23:37:45 UTC (rev 121)
@@ -60,13 +60,13 @@
} ;
-void Quaternion3D::fromMatrix(NEWMAT::Matrix matIn, unsigned long long time)
+void Quaternion3D::fromMatrix(const 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();
+ const double * mat = matIn.Store();
//Get the translations
temp.xt = mat[3];
temp.yt = mat[7];
@@ -200,6 +200,22 @@
};
+Quaternion3D::Quaternion3DStorage& Quaternion3D::Quaternion3DStorage::operator=(const Quaternion3DStorage & input)
+{
+ xt = input.xt;
+ yt = input.yt;
+ zt = input.zt;
+ xr = input.xr;
+ yr = input.yr;
+ zr = input.zr;
+ w = input.w ;
+ time = input.time;
+
+ return *this;
+};
+
+
+
void Quaternion3D::Quaternion3DStorage::Normalize()
{
double mag = getMagnitude();
@@ -214,13 +230,22 @@
return sqrt(xr*xr + yr*yr + zr*zr + w*w);
};
+//Note global scope
+std::ostream & operator<<(std::ostream& mystream, const Quaternion3D::Quaternion3DStorage& storage)
+{
+ mystream << "Storage: " << storage.xt <<", " << storage.yt <<", " << storage.zt<<", " << storage.xr<<", " << storage.yr <<", " << storage.zr <<", " << storage.w<<std::endl;
+ return mystream;
+};
+
+
NEWMAT::Matrix Quaternion3D::asMatrix(unsigned long long time)
{
Quaternion3DStorage temp;
long long diff_time;
getValue(temp, time, diff_time);
+ std::cout << temp;
// printStorage(temp);
// std::cout <<"Locally: "<< xt <<", "<< yt <<", "<< zt <<", "<< xr <<", "<<yr <<", "<<zr <<", "<<w<<std::endl;
//std::cout << "time Difference: "<< diff_time<<std::endl;
@@ -266,7 +291,7 @@
void Quaternion3D::printStorage(const Quaternion3DStorage& storage)
{
- std::cout << "Storage: " << storage.xt <<", " << storage.yt <<", " << storage.zt<<", " << storage.xr<<", " << storage.yr <<", " << storage.zr <<", " << storage.w<<std::endl;
+ std::cout << storage;
};
@@ -310,21 +335,16 @@
};
-void Quaternion3D::add_value(Quaternion3DStorage dataIn)
+void Quaternion3D::add_value(const Quaternion3DStorage &dataIn)
{
- Quaternion3DStorage temp;
- //cout << "started thread" << endl;
-
pthread_mutex_lock(&linked_list_mutex);
insertNode(dataIn);
pruneList();
pthread_mutex_unlock(&linked_list_mutex);
-
-
};
-void Quaternion3D::insertNode(Quaternion3DStorage new_val)
+void Quaternion3D::insertNode(const Quaternion3DStorage & new_val)
{
data_LL* p_current;
data_LL* p_old;
@@ -427,7 +447,7 @@
-int Quaternion3D::findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, unsigned long long target_time, long long &time_diff)
+int Quaternion3D::findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, const unsigned long long target_time, long long &time_diff)
{
data_LL* p_current = first;
@@ -468,7 +488,7 @@
};
-void Quaternion3D::interpolate(Quaternion3DStorage &one, Quaternion3DStorage &two,unsigned long long target_time, Quaternion3DStorage& output)
+void Quaternion3D::interpolate(const Quaternion3DStorage &one, const Quaternion3DStorage &two,unsigned long long target_time, Quaternion3DStorage& output)
{
//fixme do a proper interpolatioln here!!!
output.time = target_time;
@@ -483,7 +503,7 @@
};
-double Quaternion3D::interpolateDouble(double first, unsigned long long first_time, double second, unsigned long long second_time, unsigned long long target_time)
+double Quaternion3D::interpolateDouble(const double first, const unsigned long long first_time, const double second, const unsigned long long second_time, const unsigned long long target_time)
{
if ( first_time == second_time ) {
return first;
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 22:19:01 UTC (rev 120)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 23:37:45 UTC (rev 121)
@@ -60,6 +60,7 @@
// Utility functions to normalize and get magnitude.
void Normalize();
double getMagnitude();
+ Quaternion3DStorage & operator=(const Quaternion3DStorage & input);
double xt, yt, zt, xr, yr, zr, w;
unsigned long long time;
@@ -74,7 +75,7 @@
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, unsigned long long time);
+ 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
@@ -111,26 +112,26 @@
};
bool getValue(Quaternion3DStorage& buff, unsigned long long time, long long &time_diff);
- void add_value(Quaternion3DStorage);//todo fixme finish implementing this
+ void add_value(const Quaternion3DStorage&);//todo fixme finish implementing this
// insert a node into the sorted linked list
- void insertNode(Quaternion3DStorage);
+ 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, unsigned long long target_time, long long &time_diff);
+ 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(Quaternion3DStorage &one, Quaternion3DStorage &two, unsigned long long target_time, Quaternion3DStorage& output);
+ 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(double, unsigned long long, double, unsigned long long, unsigned long long);
+ 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;
@@ -148,6 +149,8 @@
+//A global ostream overload for displaying storage
+std::ostream & operator<<(std::ostream& mystream,const Quaternion3D::Quaternion3DStorage & storage);
@@ -156,5 +159,4 @@
-
#endif //QUATERNION3D_HH
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-17 23:52:02
|
Revision: 122
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=122&view=rev
Author: tfoote
Date: 2008-04-17 16:52:04 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
overloading << operator and adding const correctness checking
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 23:37:45 UTC (rev 121)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 23:52:04 UTC (rev 122)
@@ -238,14 +238,22 @@
return mystream;
};
+Quaternion3D::Quaternion3DStorage Quaternion3D::asQuaternion(unsigned long long time)
+{
+ Quaternion3DStorage temp;
+ long long diff_time; //todo Find a way to use this offset. pass storage by reference and return diff_time??
+ getValue(temp, time, diff_time);
+ return temp;
+};
+
NEWMAT::Matrix Quaternion3D::asMatrix(unsigned long long time)
{
Quaternion3DStorage temp;
long long diff_time;
getValue(temp, time, diff_time);
- std::cout << temp;
+ // std::cout << temp;
// printStorage(temp);
// std::cout <<"Locally: "<< xt <<", "<< yt <<", "<< zt <<", "<< xr <<", "<<yr <<", "<<zr <<", "<<w<<std::endl;
//std::cout << "time Difference: "<< diff_time<<std::endl;
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 23:37:45 UTC (rev 121)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 23:52:04 UTC (rev 122)
@@ -57,11 +57,17 @@
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);
-
+
+
+ /* Internal Data */
double xt, yt, zt, xr, yr, zr, w;
unsigned long long time;
};
@@ -90,13 +96,14 @@
double pitch, double roll);
- /** Accessors **/
+ /** Interpolated Accessors **/
// Return a Matrix
+ Quaternion3D::Quaternion3DStorage asQuaternion(unsigned long long time);
NEWMAT::Matrix asMatrix(unsigned long long time);
//Print as a matrix
- void printMatrix(unsigned long long time);
- void printStorage(const Quaternion3DStorage &storage);
+ 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??
// this is a function to return the current time in microseconds from the beginning of 1970
static unsigned long long Qgettime(void);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-04-18 00:01:57
|
Revision: 123
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=123&view=rev
Author: tfoote
Date: 2008-04-17 17:02:02 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
cleaning up API
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
pkg/trunk/libTF/simple/libTF.cc
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 23:52:04 UTC (rev 122)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-18 00:02:02 UTC (rev 123)
@@ -50,7 +50,7 @@
return;
};
-void Quaternion3D::Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time)
+void Quaternion3D::fromQuaternion(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time)
{
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;
@@ -238,7 +238,7 @@
return mystream;
};
-Quaternion3D::Quaternion3DStorage Quaternion3D::asQuaternion(unsigned long long time)
+Quaternion3D::Quaternion3DStorage Quaternion3D::getQuaternion(unsigned long long time)
{
Quaternion3DStorage temp;
long long diff_time; //todo Find a way to use this offset. pass storage by reference and return diff_time??
@@ -247,31 +247,38 @@
};
-NEWMAT::Matrix Quaternion3D::asMatrix(unsigned long long time)
+NEWMAT::Matrix Quaternion3D::getMatrix(unsigned long long time)
{
Quaternion3DStorage temp;
long long diff_time;
getValue(temp, time, diff_time);
- // std::cout << temp;
+ std::cout << temp;
// printStorage(temp);
// std::cout <<"Locally: "<< xt <<", "<< yt <<", "<< zt <<", "<< xr <<", "<<yr <<", "<<zr <<", "<<w<<std::endl;
//std::cout << "time Difference: "<< diff_time<<std::endl;
+
+ return temp.asMatrix();
+}
+
+
+NEWMAT::Matrix Quaternion3D::Quaternion3DStorage::asMatrix()
+{
NEWMAT::Matrix outMat(4,4);
double * mat = outMat.Store();
// math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
- double xx = temp.xr * temp.xr;
- double xy = temp.xr * temp.yr;
- double xz = temp.xr * temp.zr;
- double xw = temp.xr * temp.w;
- double yy = temp.yr * temp.yr;
- double yz = temp.yr * temp.zr;
- double yw = temp.yr * temp.w;
- double zz = temp.zr * temp.zr;
- double zw = temp.zr * temp.w;
+ double xx = xr * xr;
+ double xy = xr * yr;
+ double xz = xr * zr;
+ double xw = xr * w;
+ double yy = yr * yr;
+ double yz = yr * zr;
+ double yw = yr * w;
+ double zz = zr * zr;
+ double zw = zr * w;
mat[0] = 1 - 2 * ( yy + zz );
mat[4] = 2 * ( xy - zw );
mat[8] = 2 * ( xz + yw );
@@ -282,9 +289,9 @@
mat[6] = 2 * ( yz + xw );
mat[10] = 1 - 2 * ( xx + yy );
mat[12] = mat[13] = mat[14] = 0;
- mat[3] = temp.xt;
- mat[7] = temp.yt;
- mat[11] = temp.zt;
+ mat[3] = xt;
+ mat[7] = yt;
+ mat[11] = zt;
mat[15] = 1;
@@ -294,7 +301,7 @@
void Quaternion3D::printMatrix(unsigned long long time)
{
- std::cout << asMatrix(time);
+ std::cout << getMatrix(time);
};
void Quaternion3D::printStorage(const Quaternion3DStorage& storage)
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 23:52:04 UTC (rev 122)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-18 00:02:02 UTC (rev 123)
@@ -67,6 +67,9 @@
Quaternion3DStorage & operator=(const Quaternion3DStorage & input);
+ /* accessors */
+ NEWMAT::Matrix asMatrix();
+
/* Internal Data */
double xt, yt, zt, xr, yr, zr, w;
unsigned long long time;
@@ -78,7 +81,7 @@
/** Mutators **/
// Set the values manually
- void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
+ 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);
@@ -98,8 +101,8 @@
/** Interpolated Accessors **/
// Return a Matrix
- Quaternion3D::Quaternion3DStorage asQuaternion(unsigned long long time);
- NEWMAT::Matrix asMatrix(unsigned long long time);
+ Quaternion3D::Quaternion3DStorage getQuaternion(unsigned long long time);
+ NEWMAT::Matrix getMatrix(unsigned long long time);
//Print as a matrix
void printMatrix(unsigned long long time); //Not a critical part either
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-17 23:52:04 UTC (rev 122)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-18 00:02:02 UTC (rev 123)
@@ -42,7 +42,7 @@
/* Quaternion 3D version */
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,time);
+ myQuat.fromQuaternion(a,b,c,d,e,f,g,time);
};
/* Six DOF version */
@@ -60,12 +60,12 @@
NEWMAT::Matrix RefFrame::getMatrix(unsigned long long time)
{
- return myQuat.asMatrix(time);
+ return myQuat.getMatrix(time);
}
NEWMAT::Matrix RefFrame::getInverseMatrix(unsigned long long time)
{
- return myQuat.asMatrix(time).i();
+ return myQuat.getMatrix(time).i();
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X