Menu
▾
▴
personalrobots-commit
|
From: <tf...@us...> - 2008-04-18 00:32:02
|
Revision: 125
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=125&view=rev
Author: tfoote
Date: 2008-04-17 17:32:02 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
cleaning up header and adding Inverse method
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-18 00:20:12 UTC (rev 124)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-18 00:32:02 UTC (rev 125)
@@ -261,7 +261,12 @@
return temp.asMatrix();
}
+NEWMAT::Matrix Quaternion3D::getInverseMatrix(unsigned long long time)
+{
+ return getMatrix(time).i();
+};
+
NEWMAT::Matrix Quaternion3D::Quaternion3DStorage::asMatrix()
{
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-18 00:20:12 UTC (rev 124)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-18 00:32:02 UTC (rev 125)
@@ -53,7 +53,7 @@
class Quaternion3D {
public:
- // Storage
+ // Storage class
class Quaternion3DStorage
{
public:
@@ -82,7 +82,6 @@
/** Mutators **/
// Set the values manually
void fromQuaternion(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
-
//Set the values from a matrix
void fromMatrix(const NEWMAT::Matrix& matIn, unsigned long long time);
// Set the values using Euler angles
@@ -91,26 +90,31 @@
void fromDH(double theta, double length, double distance, double alpha, unsigned long long time);
+ /** Interpolated Accessors **/
+ // Return a Quaternion
+ Quaternion3D::Quaternion3DStorage getQuaternion(unsigned long long time);
+ // Return a Matrix
+ NEWMAT::Matrix getMatrix(unsigned long long time);
+ // Return the inverse matrix
+ NEWMAT::Matrix getInverseMatrix(unsigned long long time);
+
/**** Utility Functions ****/
+ // this is a function to return the current time in microseconds from the beginning of 1970
+ static unsigned long long Qgettime(void);
+
+ // Convert DH Parameters to a Homogeneous Transformation Matrix
static NEWMAT::Matrix matrixFromDH(double theta,
double length, double distance, double alpha);
+ // Convert Euler Angles to a Homogeneous Transformation Matrix
static NEWMAT::Matrix matrixFromEuler(double ax,
double ay, double az, double yaw,
double pitch, double roll);
-
- /** Interpolated Accessors **/
- // Return a Matrix
- 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
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);
-
+
+
private:
/**** Linked List stuff ****/
static const long long MAX_STORAGE_TIME = 1000000000; // max of 100 seconds storage
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:35:35
|
Revision: 126
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=126&view=rev
Author: tfoote
Date: 2008-04-17 17:35:41 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
cleaning up header and adding Inverse 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-18 00:32:02 UTC (rev 125)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-18 00:35:41 UTC (rev 126)
@@ -33,42 +33,13 @@
#include "libTF.hh"
RefFrame::RefFrame() :
- parent(0),
- myQuat()
+ Quaternion3D(),
+ parent(0)
{
return;
}
-/* 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.fromQuaternion(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, unsigned long long time)
-{
- myQuat.fromEuler(a,b,c,d,e,f,time) ;
-}
-
-/* DH Params version */
-void RefFrame::setParamsDH(double a,double b,double c,double d, unsigned long long time)
-{
- myQuat.fromDH(a,b,c,d,time);
-}
-
-
-NEWMAT::Matrix RefFrame::getMatrix(unsigned long long time)
-{
- return myQuat.getMatrix(time);
-}
-
-NEWMAT::Matrix RefFrame::getInverseMatrix(unsigned long long time)
-{
- return myQuat.getMatrix(time).i();
-};
-
-
TransformReference::TransformReference()
{
/* initialize pointers to NULL */
@@ -89,7 +60,7 @@
frames[frameID] = new RefFrame();
getFrame(frameID)->setParent(parentID);
- getFrame(frameID)->setParamsEulers(a,b,c,d,e,f,time);
+ getFrame(frameID)->fromEuler(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)
@@ -101,7 +72,7 @@
frames[frameID] = new RefFrame();
getFrame(frameID)->setParent(parentID);
- getFrame(frameID)->setParamsDH(a,b,c,d,time);
+ getFrame(frameID)->fromDH(a,b,c,d,time);
}
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-18 00:32:02 UTC (rev 125)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-18 00:35:41 UTC (rev 126)
@@ -40,34 +40,20 @@
#include <vector>
#include "Quaternion3D.hh"
-class RefFrame
+class RefFrame: public Quaternion3D
{
public:
/* Constructor */
RefFrame();
- /* Set the parameters for this frame */
- 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;};
/* 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(unsigned long long time);
-
- /* Generate and return the transform associated with getting out of this frame. */
- NEWMAT::Matrix getInverseMatrix(unsigned long long time);
-
-
private:
- /* Storage of the parametsrs */
- Quaternion3D myQuat;
/* 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-18 01:29:59
|
Revision: 130
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=130&view=rev
Author: tfoote
Date: 2008-04-17 18:30:07 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
a few more api cleanups
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 01:22:06 UTC (rev 129)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-18 01:30:07 UTC (rev 130)
@@ -51,7 +51,7 @@
}
-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)
+void TransformReference::setWithEulers(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;
@@ -63,7 +63,7 @@
getFrame(frameID)->fromEuler(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)
+void TransformReference::setWithDH(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;
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-18 01:22:06 UTC (rev 129)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-18 01:30:07 UTC (rev 130)
@@ -92,8 +92,8 @@
/********** 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);
+ void setWithEulers(unsigned int framid, unsigned int parentid, double,double,double,double,double,double,unsigned long long time);
+ void setWithDH(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-18 01:22:06 UTC (rev 129)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-18 01:30:07 UTC (rev 130)
@@ -18,19 +18,19 @@
//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,1,dyaw,dp,dr,atime-1000);
- 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);
- 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.setWithEulers(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
+ mTR.setWithDH(10,2,1,1,1,dyaw,atime);
+ //mTR.setWithEulers(2,3,1-1,1,1,dyaw,dp,dr,atime-1000);
+ mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-100);
+ mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-50);
+ mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-1000);
+ //mTR.setWithEulers(2,3,1+1,1,1,dyaw,dp,dr,atime+1000);
+ mTR.setWithEulers(3,5,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(5,1,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(7,6,1,1,1,dyaw,dp,dr,atime);
+ mTR.setWithDH(8,7,1,1,1,dyaw,atime);
+ //mTR.setWithEulers(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
//Demonstrate InvalidFrame LookupException
@@ -59,7 +59,7 @@
//Break the graph, making it loop and demonstrate catching MaxDepthException
- mTR.set(6,7,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(6,7,dx,dy,dz,dyaw,dp,dr,atime);
try {
std::cout<<mTR.viewChain(10,8);
@@ -70,7 +70,7 @@
}
//Break the graph, making it disconnected, and demonstrate catching ConnectivityException
- mTR.set(6,0,dx,dy,dz,dyaw,dp,dr,atime);
+ mTR.setWithEulers(6,0,dx,dy,dz,dyaw,dp,dr,atime);
try {
std::cout<<mTR.viewChain(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-19 00:33:16
|
Revision: 143
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=143&view=rev
Author: tfoote
Date: 2008-04-18 17:33:20 -0700 (Fri, 18 Apr 2008)
Log Message:
-----------
cleaning up API, standardized DH param order, and getMatrix for get
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-18 23:38:49 UTC (rev 142)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-19 00:33:20 UTC (rev 143)
@@ -121,10 +121,9 @@
fromMatrix(matrixFromEuler(_x,_y,_z,_yaw,_pitch,_roll),time);
};
-void Quaternion3D::fromDH(double theta,
- double length, double distance, double alpha, unsigned long long time)
+void Quaternion3D::fromDH(double length, double alpha, double offset, double theta, unsigned long long time)
{
- fromMatrix(matrixFromDH(theta, length, distance, alpha),time);
+ fromMatrix(matrixFromDH(length, alpha, offset, theta),time);
};
@@ -167,8 +166,8 @@
// Math from http://en.wikipedia.org/wiki/Robotics_conventions
-NEWMAT::Matrix Quaternion3D::matrixFromDH(double theta,
- double length, double distance, double alpha)
+NEWMAT::Matrix Quaternion3D::matrixFromDH(double length,
+ double alpha, double offset, double theta)
{
NEWMAT::Matrix matrix(4,4);
@@ -182,15 +181,15 @@
matrix_pointer[0] = ct;
matrix_pointer[1] = -st*ca;
matrix_pointer[2] = st*sa;
- matrix_pointer[3] = distance * ct;
+ matrix_pointer[3] = length * ct;
matrix_pointer[4] = st;
matrix_pointer[5] = ct*ca;
matrix_pointer[6] = -ct*sa;
- matrix_pointer[7] = distance*st;
+ matrix_pointer[7] = length*st;
matrix_pointer[8] = 0;
matrix_pointer[9] = sa;
matrix_pointer[10] = ca;
- matrix_pointer[11] = length;
+ matrix_pointer[11] = offset;
matrix_pointer[12] = 0.0;
matrix_pointer[13] = 0.0;
matrix_pointer[14] = 0.0;
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-18 23:38:49 UTC (rev 142)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-19 00:33:20 UTC (rev 143)
@@ -39,7 +39,6 @@
#include <math.h>
#include <pthread.h>
#include <sys/time.h>
-
class Euler3D {
public:
//Constructor
@@ -87,7 +86,7 @@
// Set the values using Euler angles
void fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll, unsigned long long time);
// Set the values using DH Parameters
- void fromDH(double theta, double length, double distance, double alpha, unsigned long long time);
+ void fromDH(double length, double alpha, double offset, double theta, unsigned long long time);
/** Interpolated Accessors **/
@@ -103,8 +102,7 @@
static unsigned long long Qgettime(void);
// Convert DH Parameters to a Homogeneous Transformation Matrix
- static NEWMAT::Matrix matrixFromDH(double theta,
- double length, double distance, double alpha);
+ static NEWMAT::Matrix matrixFromDH(double length, double alpha, double offset, double theta);
// Convert Euler Angles to a Homogeneous Transformation Matrix
static NEWMAT::Matrix matrixFromEuler(double ax,
double ay, double az, double yaw,
Modified: pkg/trunk/libTF/simple/libTF.cc
===================================================================
--- pkg/trunk/libTF/simple/libTF.cc 2008-04-18 23:38:49 UTC (rev 142)
+++ pkg/trunk/libTF/simple/libTF.cc 2008-04-19 00:33:20 UTC (rev 143)
@@ -76,7 +76,7 @@
}
-NEWMAT::Matrix TransformReference::get(unsigned int target_frame, unsigned int source_frame, unsigned long long time)
+NEWMAT::Matrix TransformReference::getMatrix(unsigned int target_frame, unsigned int source_frame, unsigned long long time)
{
NEWMAT::Matrix myMat(4,4);
TransformLists lists = lookUpList(target_frame, source_frame);
Modified: pkg/trunk/libTF/simple/libTF.hh
===================================================================
--- pkg/trunk/libTF/simple/libTF.hh 2008-04-18 23:38:49 UTC (rev 142)
+++ pkg/trunk/libTF/simple/libTF.hh 2008-04-19 00:33:20 UTC (rev 143)
@@ -38,9 +38,10 @@
#include <newmat/newmatio.h>
#include <math.h>
#include <vector>
-#include "Quaternion3D.hh"
#include <sstream>
+#include "Quaternion3D.hh"
+
/** RefFrame *******
* An instance of this class is created for each frame in the system.
* This class natively handles the relationship between frames.
@@ -92,14 +93,17 @@
/********** Mutators **************/
/* Set a new frame or update an old one. */
- void setWithEulers(unsigned int framid, unsigned int parentid, double,double,double,double,double,double,unsigned long long time);
- void setWithDH(unsigned int framid, unsigned int parentid, double,double,double,double,unsigned long long time);
+ /* Use Euler Angles. X forward, Y to the left, Z up, Yaw about Z, pitch about new Y, Roll about new X */
+ void setWithEulers(unsigned int framid, unsigned int parentid, double x, double y, double z, double yaw, double pitch, double roll, unsigned long long time);
+ /* Using DH Parameters */
+ // Conventions from http://en.wikipedia.org/wiki/Robotics_conventions
+ void setWithDH(unsigned int framid, unsigned int parentid, double length, double alpha, double offset, double theta, unsigned long long time);
// Possible exceptions TransformReference::LookupException
/*********** Accessors *************/
/* Get the transform between two frames by frame ID. */
- NEWMAT::Matrix get(unsigned int target_frame, unsigned int source_frame, unsigned long long time);
+ NEWMAT::Matrix getMatrix(unsigned int target_frame, unsigned int source_frame, unsigned long long time);
// Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
// TransformReference::MaxDepthException
Modified: pkg/trunk/libTF/simple/main.cc
===================================================================
--- pkg/trunk/libTF/simple/main.cc 2008-04-18 23:38:49 UTC (rev 142)
+++ pkg/trunk/libTF/simple/main.cc 2008-04-19 00:33:20 UTC (rev 143)
@@ -19,7 +19,7 @@
//Fill in some transforms
// mTR.setWithEulers(10,2,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params below
- mTR.setWithDH(10,2,1,1,1,dyaw,atime);
+ mTR.setWithDH(10,2,1.0,1.0,1.0,dyaw,atime);
//mTR.setWithEulers(2,3,1-1,1,1,dyaw,dp,dr,atime-1000);
mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-100);
mTR.setWithEulers(2,3,1,1,1,dyaw,dp,dr,atime-50);
@@ -29,7 +29,7 @@
mTR.setWithEulers(5,1,dx,dy,dz,dyaw,dp,dr,atime);
mTR.setWithEulers(6,5,dx,dy,dz,dyaw,dp,dr,atime);
mTR.setWithEulers(7,6,1,1,1,dyaw,dp,dr,atime);
- mTR.setWithDH(8,7,1,1,1,dyaw,atime);
+ mTR.setWithDH(8,7,1.0,1.0,1.0,dyaw,atime);
//mTR.setWithEulers(8,7,1,1,1,dyaw,dp,dr,atime); //Switching out for DH params above
@@ -50,11 +50,11 @@
//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,atime);
+ std::cout <<"Calling getMatrix(10,8)"<<std::endl;
+ // NEWMAT::Matrix mat = mTR.getMatrix(1,1);
+ NEWMAT::Matrix mat = mTR.getMatrix(10,8,atime);
- std::cout << "Result of get(10,8,atime):" << std::endl << mat<< std::endl;
+ std::cout << "Result of getMatrix(10,8,atime):" << std::endl << mat<< std::endl;
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