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