Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3968] pkg/trunk
|
From: <tf...@us...> - 2008-09-05 00:09:31
|
Revision: 3968
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3968&view=rev
Author: tfoote
Date: 2008-09-05 00:09:37 +0000 (Fri, 05 Sep 2008)
Log Message:
-----------
moving Position, Euler, Vector, and Quaternion structs out of Pose3D class to allow swigging of Pose3D
Modified Paths:
--------------
pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
pkg/trunk/motion_planning/collision_space/include/collision_space/util.h
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h
pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
pkg/trunk/util/libTF/include/libTF/Pose3D.h
pkg/trunk/util/libTF/src/Pose3D.cpp
pkg/trunk/util/libTF/src/libTF.cpp
pkg/trunk/util/libTF/src/test/main.cpp
pkg/trunk/util/libTF/src/test/testMatrix.cc
pkg/trunk/util/libTF/src/test/testMatrixQuaternion.cc
pkg/trunk/util/libTF/test/pose3d_unittest.cpp
pkg/trunk/util/rosTF/src/rosTF.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp
Modified: pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-05 00:09:37 UTC (rev 3968)
@@ -75,7 +75,7 @@
~BaseParam(){}
- libTF::Pose3D::Vector pos_; /** position of the link*/
+ libTF::Vector pos_; /** position of the link*/
std::string name_; /** name of joint corresponding to the link */
@@ -120,13 +120,13 @@
*
* \param double pos Position command to issue
*/
- void setCommand(libTF::Pose3D::Vector cmd_vel);
+ void setCommand(libTF::Vector cmd_vel);
/*!
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
*/
- libTF::Pose3D::Vector getCommand();
+ libTF::Vector getCommand();
/*!
@@ -212,7 +212,7 @@
* \param vel - Vector, vel.x and vel.y represent translational velocities in X and Y directions, vel.z represents rotational(angular velocity)
* \return point 2D velocity with .z component set to zero.
*/
- libTF::Pose3D::Vector computePointVelocity2D(const libTF::Pose3D::Vector& pos, const libTF::Pose3D::Vector& vel);
+ libTF::Vector computePointVelocity2D(const libTF::Vector& pos, const libTF::Vector& vel);
/*!
* \brief update the individual joint controllers
@@ -223,25 +223,25 @@
/*!
* \brief speed command vector used internally
*/
- libTF::Pose3D::Vector cmd_vel_;
+ libTF::Vector cmd_vel_;
/*!
* \brief Input speed command vector.
*/
- libTF::Pose3D::Vector cmd_vel_t_;
+ libTF::Vector cmd_vel_t_;
/*!
* \brief Position of the robot computed by odometry.
*/
- libTF::Pose3D::Vector base_odom_position_;
+ libTF::Vector base_odom_position_;
/*!
* \brief Speed of the robot computed by odometry.
*/
- libTF::Pose3D::Vector base_odom_velocity_;
+ libTF::Vector base_odom_velocity_;
/*!
@@ -286,7 +286,7 @@
void computeWheelPositions();
- std::vector<libTF::Pose3D::Vector> base_wheels_position_; /** vector of current wheel positions */
+ std::vector<libTF::Vector> base_wheels_position_; /** vector of current wheel positions */
/*!
Modified: pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-05 00:09:37 UTC (rev 3968)
@@ -67,7 +67,7 @@
}
// Set the joint position command
-void BaseController::setCommand(libTF::Pose3D::Vector cmd_vel)
+void BaseController::setCommand(libTF::Vector cmd_vel)
{
pthread_mutex_lock(&base_controller_lock_);
//std::cout << "command received : " << cmd_vel_t_ << std::endl;
@@ -77,9 +77,9 @@
pthread_mutex_unlock(&base_controller_lock_);
}
-libTF::Pose3D::Vector BaseController::getCommand()// Return the current position command
+libTF::Vector BaseController::getCommand()// Return the current position command
{
- libTF::Pose3D::Vector cmd_vel;
+ libTF::Vector cmd_vel;
pthread_mutex_lock(&base_controller_lock_);
cmd_vel.x = cmd_vel_t_.x;
cmd_vel.y = cmd_vel_t_.y;
@@ -139,7 +139,7 @@
base_wheels_.push_back(base_object);
wheel_speed_actual_.push_back(0);
- libTF::Pose3D::Vector *v=new libTF::Pose3D::Vector();
+ libTF::Vector *v=new libTF::Vector();
base_wheels_position_.push_back(*v);
num_wheels_++;
}
@@ -247,7 +247,7 @@
void BaseController::computeWheelPositions()
{
- libTF::Pose3D::Vector res1;
+ libTF::Vector res1;
double steer_angle;
for(int i=0; i < num_wheels_; i++)
{
@@ -302,7 +302,7 @@
void BaseController::computeAndSetCasterSteer()
{
- libTF::Pose3D::Vector result;
+ libTF::Vector result;
double steer_angle_desired;
double error_steer;
for(int i=0; i < num_casters_; i++)
@@ -320,10 +320,10 @@
void BaseController::computeAndSetWheelSpeeds()
{
- libTF::Pose3D::Vector wheel_point_velocity;
- libTF::Pose3D::Vector wheel_point_velocity_projected;
- libTF::Pose3D::Vector wheel_caster_steer_component;
- libTF::Pose3D::Vector caster_2d_velocity;
+ libTF::Vector wheel_point_velocity;
+ libTF::Vector wheel_point_velocity_projected;
+ libTF::Vector wheel_caster_steer_component;
+ libTF::Vector caster_2d_velocity;
caster_2d_velocity.x = 0;
caster_2d_velocity.y = 0;
@@ -379,7 +379,7 @@
pr2_controllers::SetBaseCommand::request &req,
pr2_controllers::SetBaseCommand::response &resp)
{
- libTF::Pose3D::Vector command;
+ libTF::Vector command;
command.x = req.x_vel;
command.y = req.y_vel;
command.z = req.theta_vel;
@@ -394,7 +394,7 @@
void BaseControllerNode::setCommand(double vx, double vy, double vw)
{
- libTF::Pose3D::Vector command;
+ libTF::Vector command;
command.x = vx;
command.y = vy;
command.z = vw;
@@ -407,7 +407,7 @@
pr2_controllers::GetBaseCommand::request &req,
pr2_controllers::GetBaseCommand::response &resp)
{
- libTF::Pose3D::Vector command;
+ libTF::Vector command;
command = c_->getCommand();
resp.x_vel = command.x;
resp.y_vel = command.y;
@@ -449,9 +449,9 @@
}
-Pose3D::Vector BaseController::computePointVelocity2D(const Pose3D::Vector& pos, const Pose3D::Vector& vel)
+Vector BaseController::computePointVelocity2D(const Vector& pos, const Vector& vel)
{
- Pose3D::Vector result;
+ Vector result;
result.x = vel.x - pos.y * vel.z;
result.y = vel.y + pos.x * vel.z;
@@ -463,11 +463,11 @@
void BaseController::computeOdometry(double time)
{
double dt = time-last_time_;
-// libTF::Pose3D::Vector base_odom_delta = rotate2D(base_odom_velocity_*dt,base_odom_position_.z);
+// libTF::Vector base_odom_delta = rotate2D(base_odom_velocity_*dt,base_odom_position_.z);
computeBaseVelocity();
- libTF::Pose3D::Vector base_odom_delta = (base_odom_velocity_*dt).rot2D(base_odom_position_.z);
+ libTF::Vector base_odom_delta = (base_odom_velocity_*dt).rot2D(base_odom_position_.z);
// if(isnan(dt))
// {
@@ -604,8 +604,8 @@
// wheel_radius_ = wheel_geom->radius;
// BaseCasterGeomParam caster;
-// libTF::Pose3D::Vector wheel_l;
-// libTF::Pose3D::Vector wheel_r;
+// libTF::Vector wheel_l;
+// libTF::Vector wheel_r;
// wheel_l.x = 0;
// wheel_l.y = wheel_base_/2.0;
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/util.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/util.h 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/util.h 2008-09-05 00:09:37 UTC (rev 3968)
@@ -78,11 +78,11 @@
bool containsPoint(double x, double y, double z) const
{
- libTF::Pose3D::Position pt(x, y, z);
+ libTF::Position pt(x, y, z);
return containsPoint(pt);
}
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const = 0;
+ virtual bool containsPoint(const libTF::Position &p) const = 0;
protected:
@@ -106,7 +106,7 @@
{
}
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ virtual bool containsPoint(const libTF::Position &p) const
{
double dx = m_center.x - p.x;
double dy = m_center.y - p.y;
@@ -128,7 +128,7 @@
m_pose.getPosition(m_center);
}
- libTF::Pose3D::Position m_center;
+ libTF::Position m_center;
double m_radius;
double m_radius2;
@@ -146,7 +146,7 @@
{
}
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ virtual bool containsPoint(const libTF::Position &p) const
{
double vx = p.x - m_center.x;
double vy = p.y - m_center.y;
@@ -188,10 +188,10 @@
m_pose.applyToVector(m_normalB2);
}
- libTF::Pose3D::Position m_center;
- libTF::Pose3D::Vector m_normalH;
- libTF::Pose3D::Vector m_normalB1;
- libTF::Pose3D::Vector m_normalB2;
+ libTF::Position m_center;
+ libTF::Vector m_normalH;
+ libTF::Vector m_normalB1;
+ libTF::Vector m_normalB2;
double m_length;
double m_length2;
@@ -212,7 +212,7 @@
{
}
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ virtual bool containsPoint(const libTF::Position &p) const
{
double vx = p.x - m_center.x;
double vy = p.y - m_center.y;
@@ -263,10 +263,10 @@
m_pose.applyToVector(m_normalW);
}
- libTF::Pose3D::Position m_center;
- libTF::Pose3D::Vector m_normalL;
- libTF::Pose3D::Vector m_normalW;
- libTF::Pose3D::Vector m_normalH;
+ libTF::Position m_center;
+ libTF::Vector m_normalL;
+ libTF::Vector m_normalW;
+ libTF::Vector m_normalH;
double m_length;
double m_width;
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-09-05 00:09:37 UTC (rev 3968)
@@ -131,9 +131,9 @@
libTF::Pose3D &pose = m_kgeoms[model_id].geom[i]->link->globalTrans;
dGeomID geom = m_kgeoms[model_id].geom[i]->geom;
- libTF::Pose3D::Position pos = pose.getPosition();
+ libTF::Position pos = pose.getPosition();
dGeomSetPosition(geom, pos.x, pos.y, pos.z);
- libTF::Pose3D::Quaternion quat = pose.getQuaternion();
+ libTF::Quaternion quat = pose.getQuaternion();
dQuaternion q; q[0] = quat.w; q[1] = quat.x; q[2] = quat.y; q[3] = quat.z;
dGeomSetQuaternion(geom, q);
}
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-09-05 00:09:37 UTC (rev 3968)
@@ -79,7 +79,7 @@
for (unsigned int k=0; k<3; k++)
axes[j][k] = mat.element(j,k);
- libTF::Pose3D::Position libTFpos;
+ libTF::Position libTFpos;
links[i]->globalTrans.getPosition(libTFpos);
float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z};
const double *size = static_cast<planning_models::KinematicModel::Box*>(links[i]->shape)->size;
@@ -99,7 +99,7 @@
for (unsigned int k=0; k<3; k++)
axes[j][k] = mat.element(j,k);
- libTF::Pose3D::Position libTFpos;
+ libTF::Position libTFpos;
links[i]->globalTrans.getPosition(libTFpos);
float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z };
float radius = static_cast<planning_models::KinematicModel::Cylinder*>(links[i]->shape)->radius;
@@ -113,7 +113,7 @@
case planning_models::KinematicModel::Shape::SPHERE:
{
- libTF::Pose3D::Position libTFpos;
+ libTF::Position libTFpos;
links[i]->globalTrans.getPosition(libTFpos);
float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z};
float radius = static_cast<planning_models::KinematicModel::Sphere*>(links[i]->shape)->radius;
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h 2008-09-05 00:09:37 UTC (rev 3968)
@@ -108,7 +108,7 @@
case robot_msgs::PoseConstraint::ONLY_POSITION:
if (distPos)
{
- libTF::Pose3D::Position bodyPos;
+ libTF::Position bodyPos;
m_link->globalTrans.getPosition(bodyPos);
double dx = bodyPos.x - m_pc.pose.position.x;
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-09-05 00:09:37 UTC (rev 3968)
@@ -75,11 +75,11 @@
void addTransform(TiXmlElement *elem, const::libTF::Pose3D& transform)
{
- libTF::Pose3D::Position pz;
+ libTF::Position pz;
transform.getPosition(pz);
double cpos[3] = { pz.x, pz.y, pz.z };
- libTF::Pose3D::Euler eu;
+ libTF::Euler eu;
transform.getEuler(eu);
double crot[3] = { eu.roll, eu.pitch, eu.yaw };
Modified: pkg/trunk/util/libTF/include/libTF/Pose3D.h
===================================================================
--- pkg/trunk/util/libTF/include/libTF/Pose3D.h 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/util/libTF/include/libTF/Pose3D.h 2008-09-05 00:09:37 UTC (rev 3968)
@@ -46,183 +46,184 @@
namespace libTF
{
- /** \brief A class used to store and do basic minipulations of 3D transformations
- *
- */
- class Pose3D
- {
- friend class Pose3DCache;
-
- public:
- /** \brief A struct to represent the translational component (a point) */
- struct Position
- {
- double x,y,z;
- /** \brief Constructor */
- Position():x(0),y(0),z(0){;};
- /** \brief Constructor */
- Position(double x, double y, double z):x(x),y(y),z(z){;};
- /** \brief operator overloading for the + operator */
- Position operator+(const Position &rhs){
- Position result;
- result.x = x + rhs.x;
- result.y = y + rhs.y;
- result.z = z + rhs.z;
- return result;
- }
+/** \brief A struct to represent the translational component (a point) */
+struct Position
+{
+ double x,y,z;
+ /** \brief Constructor */
+ Position():x(0),y(0),z(0){;};
+ /** \brief Constructor */
+ Position(double x, double y, double z):x(x),y(y),z(z){;};
+ /** \brief operator overloading for the + operator */
+ Position operator+(const Position &rhs){
+ Position result;
+ result.x = x + rhs.x;
+ result.y = y + rhs.y;
+ result.z = z + rhs.z;
+ return result;
+ }
- /** \brief operator overloading for the += operator */
- Position & operator+=(const Position &rhs){
- x += rhs.x;
- y += rhs.y;
- z += rhs.z;
- return *this;
- }
+ /** \brief operator overloading for the += operator */
+ Position & operator+=(const Position &rhs){
+ x += rhs.x;
+ y += rhs.y;
+ z += rhs.z;
+ return *this;
+ }
- /** \brief operator overloading for the - operator */
- Position operator-(const Position &rhs){
- Position result;
- result.x = x - rhs.x;
- result.y = y - rhs.y;
- result.z = z - rhs.z;
- return result;
- }
+ /** \brief operator overloading for the - operator */
+ Position operator-(const Position &rhs){
+ Position result;
+ result.x = x - rhs.x;
+ result.y = y - rhs.y;
+ result.z = z - rhs.z;
+ return result;
+ }
- /** \brief operator overloading for the -= operator */
- Position & operator-=(const Position &rhs){
- x -= rhs.x;
- y -= rhs.y;
- z -= rhs.z;
- return *this;
- }
+ /** \brief operator overloading for the -= operator */
+ Position & operator-=(const Position &rhs){
+ x -= rhs.x;
+ y -= rhs.y;
+ z -= rhs.z;
+ return *this;
+ }
- /** \brief operator overloading for the *= operator */
- Position & operator*=(double rhs){
- x *= rhs;
- y *= rhs;
- z *= rhs;
- return *this;
- }
+ /** \brief operator overloading for the *= operator */
+ Position & operator*=(double rhs){
+ x *= rhs;
+ y *= rhs;
+ z *= rhs;
+ return *this;
+ }
- /** \brief operator overloading for the * operator */
- Position operator*(double rhs){
- Position result;
- result.x = x*rhs;
- result.y = y*rhs;
- result.z = z*rhs;
- return result;
- }
+ /** \brief operator overloading for the * operator */
+ Position operator*(double rhs){
+ Position result;
+ result.x = x*rhs;
+ result.y = y*rhs;
+ result.z = z*rhs;
+ return result;
+ }
- /** \brief Rotate a position about the z-axis */
- Position rot2D(double angle){
- Position result;
- double cosa = cos(angle);
- double sina = sin(angle);
- result.x = cosa*x - sina*y;
- result.y = sina*x + cosa*y;
- result.z = z;
- return result;
+ /** \brief Rotate a position about the z-axis */
+ Position rot2D(double angle){
+ Position result;
+ double cosa = cos(angle);
+ double sina = sin(angle);
+ result.x = cosa*x - sina*y;
+ result.y = sina*x + cosa*y;
+ result.z = z;
+ return result;
- }
- };
+ }
+};
- /** \brief A struct to represent vectors */
- struct Vector
- {
- double x,y,z;
+/** \brief A struct to represent vectors */
+struct Vector
+{
+ double x,y,z;
- /** \brief Constructor */
- Vector():x(0),y(0),z(0){;};
- /** \brief Constructor */
- Vector(double x, double y, double z):x(x),y(y),z(z){;};
+ /** \brief Constructor */
+ Vector():x(0),y(0),z(0){;};
+ /** \brief Constructor */
+ Vector(double x, double y, double z):x(x),y(y),z(z){;};
- /** \brief operator overloading for the + operator */
- Vector operator+(const Vector &rhs){
- Vector result;
- result.x = x + rhs.x;
- result.y = y + rhs.y;
- result.z = z + rhs.z;
- return result;
- }
+ /** \brief operator overloading for the + operator */
+ Vector operator+(const Vector &rhs){
+ Vector result;
+ result.x = x + rhs.x;
+ result.y = y + rhs.y;
+ result.z = z + rhs.z;
+ return result;
+ }
- /** \brief operator overloading for the += operator */
- Vector & operator+=(const Vector &rhs){
- x += rhs.x;
- y += rhs.y;
- z += rhs.z;
- return *this;
- }
+ /** \brief operator overloading for the += operator */
+ Vector & operator+=(const Vector &rhs){
+ x += rhs.x;
+ y += rhs.y;
+ z += rhs.z;
+ return *this;
+ }
- /** \brief operator overloading for the - operator */
- Vector operator-(const Vector &rhs){
- Vector result;
- result.x = x - rhs.x;
- result.y = y - rhs.y;
- result.z = z - rhs.z;
- return result;
- }
+ /** \brief operator overloading for the - operator */
+ Vector operator-(const Vector &rhs){
+ Vector result;
+ result.x = x - rhs.x;
+ result.y = y - rhs.y;
+ result.z = z - rhs.z;
+ return result;
+ }
- /** \brief operator overloading for the -= operator */
- Vector & operator-=(const Vector &rhs){
- x -= rhs.x;
- y -= rhs.y;
- z -= rhs.z;
- return *this;
- }
+ /** \brief operator overloading for the -= operator */
+ Vector & operator-=(const Vector &rhs){
+ x -= rhs.x;
+ y -= rhs.y;
+ z -= rhs.z;
+ return *this;
+ }
- /** \brief operator overloading for the *= operator */
- Vector & operator*=(double rhs){
- x *= rhs;
- y *= rhs;
- z *= rhs;
- return *this;
- }
+ /** \brief operator overloading for the *= operator */
+ Vector & operator*=(double rhs){
+ x *= rhs;
+ y *= rhs;
+ z *= rhs;
+ return *this;
+ }
- /** \brief operator overloading for the * operator */
- Vector operator*(double rhs){
- Vector result;
- result.x = x*rhs;
- result.y = y*rhs;
- result.z = z*rhs;
- return result;
- }
+ /** \brief operator overloading for the * operator */
+ Vector operator*(double rhs){
+ Vector result;
+ result.x = x*rhs;
+ result.y = y*rhs;
+ result.z = z*rhs;
+ return result;
+ }
- /** \brief Rotate a vector about the z-axis */
- Vector rot2D(double angle){
- Vector result;
- double cosa = cos(angle);
- double sina = sin(angle);
- result.x = cosa*x - sina*y;
- result.y = sina*x + cosa*y;
- result.z = z;
- return result;
- }
+ /** \brief Rotate a vector about the z-axis */
+ Vector rot2D(double angle){
+ Vector result;
+ double cosa = cos(angle);
+ double sina = sin(angle);
+ result.x = cosa*x - sina*y;
+ result.y = sina*x + cosa*y;
+ result.z = z;
+ return result;
+ }
- };
+};
- /** \brief A struct to represent the quaternion component */
- struct Quaternion
- {
- double x,y,z,w;
+/** \brief A struct to represent the quaternion component */
+struct Quaternion
+{
+ double x,y,z,w;
- /** \brief Constructor */
- Quaternion():x(0),y(0),z(0),w(1){;};
- /** \brief Constructor */
- Quaternion(double x, double y, double z, double w):x(x),y(y),z(z),w(w){;};
+ /** \brief Constructor */
+ Quaternion():x(0),y(0),z(0),w(1){;};
+ /** \brief Constructor */
+ Quaternion(double x, double y, double z, double w):x(x),y(y),z(z),w(w){;};
- };
- /** \brief A struct to represent Euler angles */
- struct Euler
- {
- double yaw, pitch, roll;
- /** \brief Constructor */
- Euler():yaw(0),pitch(0),roll(0){;};
- /** \brief Constructor */
- Euler(double yaw, double pitch, double roll):yaw(yaw),pitch(pitch),roll(roll){;};
+};
+/** \brief A struct to represent Euler angles */
+struct Euler
+{
+ double yaw, pitch, roll;
+ /** \brief Constructor */
+ Euler():yaw(0),pitch(0),roll(0){;};
+ /** \brief Constructor */
+ Euler(double yaw, double pitch, double roll):yaw(yaw),pitch(pitch),roll(roll){;};
- };
+};
+
+/** \brief A class used to store and do basic minipulations of 3D transformations
+ *
+ */
+ class Pose3D
+ {
+ friend class Pose3DCache;
+
+ public:
/* Constructors */
/** \brief Empty Constructor initialize to zero */
@@ -352,7 +353,7 @@
/** \brief A namespace ostream overload for displaying poses */
std::ostream & operator<<(std::ostream& mystream, const Pose3D &pose);
- std::ostream & operator<<(std::ostream& mystream, const Pose3D::Vector &p);
+ std::ostream & operator<<(std::ostream& mystream, const libTF::Vector &p);
}
Modified: pkg/trunk/util/libTF/src/Pose3D.cpp
===================================================================
--- pkg/trunk/util/libTF/src/Pose3D.cpp 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/util/libTF/src/Pose3D.cpp 2008-09-05 00:09:37 UTC (rev 3968)
@@ -275,7 +275,7 @@
}
-Pose3D::Euler Pose3D::eulerFromMatrix(const NEWMAT::Matrix & matrix_in, unsigned int solution_number)
+Euler Pose3D::eulerFromMatrix(const NEWMAT::Matrix & matrix_in, unsigned int solution_number)
{
Euler euler_out;
@@ -328,7 +328,7 @@
return euler_out2;
};
-Pose3D::Position Pose3D::positionFromMatrix(const NEWMAT::Matrix & matrix_in)
+Position Pose3D::positionFromMatrix(const NEWMAT::Matrix & matrix_in)
{
Position position;
//get the pointer to the raw data
@@ -410,7 +410,7 @@
axis[2] = zr / d;
}
-Pose3D::Euler Pose3D::getEuler(void) const
+Euler Pose3D::getEuler(void) const
{
return eulerFromMatrix(asMatrix());
}
@@ -420,7 +420,7 @@
eu = eulerFromMatrix(asMatrix());
}
-Pose3D::Quaternion Pose3D::getQuaternion(void) const
+Quaternion Pose3D::getQuaternion(void) const
{
Quaternion quat;
quat.x = xr;
@@ -430,7 +430,7 @@
return quat;
};
-Pose3D::Position Pose3D::getPosition(void) const
+Position Pose3D::getPosition(void) const
{
Position pos;
pos.x = xt;
@@ -599,13 +599,13 @@
//Note not member function
std::ostream & libTF::operator<<(std::ostream& mystream, const Pose3D &storage)
{
- Pose3D::Quaternion q = storage.getQuaternion();
- Pose3D::Position p = storage.getPosition();
+ Quaternion q = storage.getQuaternion();
+ Position p = storage.getPosition();
mystream << "Storage: " << p.x << ", " << p.y << ", " << p.z << ", " << q.x << ", " << q.y << ", " << q.z << ", " << q.w << std::endl;
return mystream;
};
-std::ostream & libTF::operator<<(std::ostream& mystream, const Pose3D::Vector &p)
+std::ostream & libTF::operator<<(std::ostream& mystream, const Vector &p)
{
mystream << p.x << ", " << p.y << ", " << p.z << ", " << std::endl;
return mystream;
Modified: pkg/trunk/util/libTF/src/libTF.cpp
===================================================================
--- pkg/trunk/util/libTF/src/libTF.cpp 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/util/libTF/src/libTF.cpp 2008-09-05 00:09:37 UTC (rev 3968)
@@ -223,7 +223,7 @@
NEWMAT::Matrix output = local.i() * Transform;
- Pose3D::Euler eulers = Pose3D::eulerFromMatrix(output,1);
+ Euler eulers = Pose3D::eulerFromMatrix(output,1);
TFEulerYPR retEuler;
retEuler.yaw = eulers.yaw;
Modified: pkg/trunk/util/libTF/src/test/main.cpp
===================================================================
--- pkg/trunk/util/libTF/src/test/main.cpp 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/util/libTF/src/test/main.cpp 2008-09-05 00:09:37 UTC (rev 3968)
@@ -21,7 +21,7 @@
NEWMAT::Matrix m = pz.asMatrix();
cout << m;
- Pose3D::Position p;
+ Position p;
p.x = 0;
p.y = 0;
p.z = 1;
Modified: pkg/trunk/util/libTF/src/test/testMatrix.cc
===================================================================
--- pkg/trunk/util/libTF/src/test/testMatrix.cc 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/util/libTF/src/test/testMatrix.cc 2008-09-05 00:09:37 UTC (rev 3968)
@@ -48,7 +48,7 @@
pitch,
roll);
- libTF::Pose3D::Euler out = libTF::Pose3D::eulerFromMatrix(m);
+ libTF::Euler out = libTF::Pose3D::eulerFromMatrix(m);
//TODO add +- 2PI checking/redundant angles checking
// see if input is the same as output (accounting for floating point errors)
Modified: pkg/trunk/util/libTF/src/test/testMatrixQuaternion.cc
===================================================================
--- pkg/trunk/util/libTF/src/test/testMatrixQuaternion.cc 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/util/libTF/src/test/testMatrixQuaternion.cc 2008-09-05 00:09:37 UTC (rev 3968)
@@ -58,8 +58,8 @@
aPose.setFromMatrix(m);
m = aPose.asMatrix();
- libTF::Pose3D::Euler out = libTF::Pose3D::eulerFromMatrix(m);
- libTF::Pose3D::Euler out2 = libTF::Pose3D::eulerFromMatrix(m,2);
+ libTF::Euler out = libTF::Pose3D::eulerFromMatrix(m);
+ libTF::Euler out2 = libTF::Pose3D::eulerFromMatrix(m,2);
// see if input is the same as output (accounting for floating point errors)
if ((fabs(modNPiBy2(out.yaw) - modNPiBy2(yaw)) > 0.001 || fabs(modNPiBy2(out.pitch) - modNPiBy2(pitch)) > 0.001 || fabs(modNPiBy2(out.roll) -modNPiBy2(roll)) > 0.0001) &&
@@ -95,7 +95,7 @@
aPose.setFromMatrix(matrix);
- libTF::Pose3D::Quaternion myquat;
+ libTF::Quaternion myquat;
aPose.getQuaternion(myquat);
std::cout << " MY Quaternion" << myquat.x << " " << myquat.y << " " << myquat.z << " " << myquat.w << std::endl;
Modified: pkg/trunk/util/libTF/test/pose3d_unittest.cpp
===================================================================
--- pkg/trunk/util/libTF/test/pose3d_unittest.cpp 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/util/libTF/test/pose3d_unittest.cpp 2008-09-05 00:09:37 UTC (rev 3968)
@@ -16,22 +16,22 @@
TEST(Pose3D, DefaultConstructor){
Pose3D p;
//EXPECT_EQ(0, p.getMagnitude());
- Pose3D::Euler e = p.getEuler();
+ Euler e = p.getEuler();
EXPECT_EQ(0, e.yaw);
EXPECT_EQ(0, e.pitch);
EXPECT_EQ(0, e.roll);
- Pose3D::Quaternion q = p.getQuaternion();
+ Quaternion q = p.getQuaternion();
EXPECT_EQ(0, q.x);
EXPECT_EQ(0, q.y);
EXPECT_EQ(0, q.z);
EXPECT_EQ(1, q.w);
- Pose3D::Position x = p.getPosition();
+ Position x = p.getPosition();
EXPECT_EQ(0, x.x);
EXPECT_EQ(0, x.y);
EXPECT_EQ(0, x.z);
}
TEST(Vector, DefaultConstructor){
- Pose3D::Vector v;
+ Vector v;
EXPECT_EQ(0, v.x);
EXPECT_EQ(0, v.y);
EXPECT_EQ(0, v.z);
@@ -40,7 +40,7 @@
TEST(Vector, CopyConstructor){
seed_rand();
- Pose3D::Vector a,b,c;
+ Vector a,b,c;
for (unsigned int i = 0; i < 1000; i++)
{
a.x = rand();
@@ -60,7 +60,7 @@
TEST(Vector, Addition){
seed_rand();
- Pose3D::Vector a,b,c;
+ Vector a,b,c;
for (unsigned int i = 0; i < 1000; i++)
{
a.x = rand();
@@ -78,7 +78,7 @@
TEST(Vector, PlusEqual){
seed_rand();
- Pose3D::Vector a,b,c;
+ Vector a,b,c;
for (unsigned int i = 0; i < 1000; i++)
{
a.x = rand();
@@ -97,7 +97,7 @@
TEST(Vector, Subtraction){
seed_rand();
- Pose3D::Vector a,b,c;
+ Vector a,b,c;
for (unsigned int i = 0; i < 1000; i++)
{
a.x = rand();
@@ -116,7 +116,7 @@
TEST(Vector, MinusEqual){
seed_rand();
- Pose3D::Vector a,b,c;
+ Vector a,b,c;
for (unsigned int i = 0; i < 1000; i++)
{
a.x = rand();
@@ -135,7 +135,7 @@
TEST(Vector, TimesEqual){
seed_rand();
- Pose3D::Vector a,b,c;
+ Vector a,b,c;
double scalar;
for (unsigned int i = 0; i < 1000; i++)
{
@@ -153,7 +153,7 @@
TEST(Vector, Times){
seed_rand();
- Pose3D::Vector a,b,c;
+ Vector a,b,c;
double scalar;
for (unsigned int i = 0; i < 1000; i++)
{
@@ -170,7 +170,7 @@
TEST(Vector, rot2D){
seed_rand();
- Pose3D::Vector a,b,c;
+ Vector a,b,c;
double scalar;
for (unsigned int i = 0; i < 1000; i++)
{
@@ -186,7 +186,7 @@
}
TEST(Position, DefaultConstructor){
- Pose3D::Position po;
+ Position po;
EXPECT_EQ(0, po.x);
EXPECT_EQ(0, po.y);
EXPECT_EQ(0, po.z);
@@ -194,7 +194,7 @@
TEST(Position, CopyConstructor){
seed_rand();
- Pose3D::Position a,b;
+ Position a,b;
for (unsigned int i = 0; i < 1000; i++)
{
a.x = rand();
@@ -214,7 +214,7 @@
TEST(Position, Addition){
seed_rand();
- Pose3D::Position a,b,c;
+ Position a,b,c;
for (unsigned int i = 0; i < 1000; i++)
{
a.x = rand();
@@ -232,7 +232,7 @@
TEST(Position, PlusEquals){
seed_rand();
- Pose3D::Position a,b,c;
+ Position a,b,c;
for (unsigned int i = 0; i < 1000; i++)
{
a.x = rand();
@@ -251,7 +251,7 @@
TEST(Position, Subtraction){
seed_rand();
- Pose3D::Position a,b,c;
+ Position a,b,c;
for (unsigned int i = 0; i < 1000; i++)
{
a.x = rand();
@@ -269,7 +269,7 @@
TEST(Position, MinusEquals){
seed_rand();
- Pose3D::Position a,b,c;
+ Position a,b,c;
for (unsigned int i = 0; i < 1000; i++)
{
a.x = rand();
@@ -288,7 +288,7 @@
TEST(Position, TimesEqual){
seed_rand();
- Pose3D::Position a,b,c;
+ Position a,b,c;
double scalar;
for (unsigned int i = 0; i < 1000; i++)
{
@@ -306,7 +306,7 @@
TEST(Position, Times){
seed_rand();
- Pose3D::Position a,b,c;
+ Position a,b,c;
double scalar;
for (unsigned int i = 0; i < 1000; i++)
{
@@ -323,7 +323,7 @@
TEST(Position, rot2D){
seed_rand();
- Pose3D::Position a,b,c;
+ Position a,b,c;
double scalar;
for (unsigned int i = 0; i < 1000; i++)
{
@@ -339,14 +339,14 @@
}
TEST(Quaterion, DefaultConstructor){
- Pose3D::Quaternion quat;
+ Quaternion quat;
EXPECT_EQ(0, quat.x);
EXPECT_EQ(0, quat.y);
EXPECT_EQ(0, quat.z);
}
TEST(Euler, DefaultConstructor){
- Pose3D::Euler e;
+ Euler e;
EXPECT_EQ(0, e.yaw);
EXPECT_EQ(0, e.pitch);
EXPECT_EQ(0, e.roll);
@@ -355,7 +355,7 @@
void testEulerConversion(double yaw, double pitch, double roll){
Pose3D p;
p.setFromEuler(0, 0, 0, yaw, pitch, roll);
- Pose3D::Euler e = p.getEuler();
+ Euler e = p.getEuler();
EXPECT_FLOAT_EQ(e.yaw, yaw);
EXPECT_FLOAT_EQ(e.pitch, pitch);
EXPECT_FLOAT_EQ(e.roll, roll);
@@ -409,8 +409,8 @@
{
EXPECT_LT(fabs(n(row,col) - m(row,col)), 0.00000001);
}
- libTF::Pose3D::Euler out = libTF::Pose3D::eulerFromMatrix(n);
- libTF::Pose3D::Euler out2 = libTF::Pose3D::eulerFromMatrix(n,2);
+ libTF::Euler out = libTF::Pose3D::eulerFromMatrix(n);
+ libTF::Euler out2 = libTF::Pose3D::eulerFromMatrix(n,2);
//Test the difference between input and output accounting for 2Pi redundancy.
bool difference = ((fabs(math_utils::modNPiBy2(out.yaw) - math_utils::modNPiBy2(yaw)) > 0.001 || fabs(math_utils::modNPiBy2(out.pitch) - math_utils::modNPiBy2(pitch)) > 0.001 || fabs(math_utils::modNPiBy2(out.roll) -math_utils::modNPiBy2(roll)) > 0.0001) &&
@@ -446,7 +446,7 @@
<< 0 << 0 << 0 << rand()
<< 0 << 0 << 0 << 1;
- Pose3D::Position t = Pose3D::positionFromMatrix(m);
+ Position t = Pose3D::positionFromMatrix(m);
EXPECT_EQ(t.x, m(1,4));
EXPECT_EQ(t.y, m(2,4));
Modified: pkg/trunk/util/rosTF/src/rosTF.cpp
===================================================================
--- pkg/trunk/util/rosTF/src/rosTF.cpp 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/util/rosTF/src/rosTF.cpp 2008-09-05 00:09:37 UTC (rev 3968)
@@ -249,8 +249,8 @@
rosTF::TransformArray tfArray;
tfArray.set_eulers_size(1);
//Invert the transform
- libTF::Pose3D::Euler odomeuler = libTF::Pose3D::eulerFromMatrix(libTF::Pose3D::matrixFromEuler(x, y, z, yaw, pitch, roll).i()); //todo optimize
- libTF::Pose3D::Position odompos = libTF::Pose3D::positionFromMatrix(libTF::Pose3D::matrixFromEuler(x, y, z, yaw, pitch, roll).i());
+ libTF::Euler odomeuler = libTF::Pose3D::eulerFromMatrix(libTF::Pose3D::matrixFromEuler(x, y, z, yaw, pitch, roll).i()); //todo optimize
+ libTF::Position odompos = libTF::Pose3D::positionFromMatrix(libTF::Pose3D::matrixFromEuler(x, y, z, yaw, pitch, roll).i());
tfArray.eulers[0].header.frame_id = frame;
tfArray.eulers[0].parent = parent;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp 2008-09-05 00:03:02 UTC (rev 3967)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp 2008-09-05 00:09:37 UTC (rev 3968)
@@ -341,15 +341,15 @@
continue;
}
- libTF::Pose3D::Position robot_visual_position = link->globalTransFwd.getPosition();
- libTF::Pose3D::Quaternion robot_visual_orientation = link->globalTransFwd.getQuaternion();
+ libTF::Position robot_visual_position = link->globalTransFwd.getPosition();
+ libTF::Quaternion robot_visual_orientation = link->globalTransFwd.getQuaternion();
Ogre::Vector3 visual_position( robot_visual_position.x, robot_visual_position.y, robot_visual_position.z );
Ogre::Quaternion visual_orientation( robot_visual_orientation.w, robot_visual_orientation.x, robot_visual_orientation.y, robot_visual_orientation.z );
robotToOgre( visual_position );
robotToOgre( visual_orientation );
- libTF::Pose3D::Position robot_collision_position = link->globalTrans.getPosition();
- libTF::Pose3D::Quaternion robot_collision_orientation = link->globalTrans.getQuaternion();
+ libTF::Position robot_collision_position = link->globalTrans.getPosition();
+ libTF::Quaternion robot_collision_orientation = link->globalTrans.getQuaternion();
Ogre::Vector3 collision_position( robot_collision_position.x, robot_collision_position.y, robot_collision_position.z );
Ogre::Quaternion collision_orientation( robot_collision_orientation.w, robot_collision_orientation.x, robot_collision_orientation.y, robot_collision_orientation.z );
robotToOgre( collision_position );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
