Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [108] pkg/trunk/libTF/simple
|
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.
|
