[Personalrobots-commit] SF.net SVN: personalrobots:[6891] pkg/trunk

[<hsu...@us...>]

Revision: 6891
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6891&view=rev
Author:   hsujohnhsu
Date:     2008-11-18 18:26:05 +0000 (Tue, 18 Nov 2008)

Log Message:
-----------
updates per math_utils API review.
  * rename math_utils to angles package
  * move MathExpressiosn to math_expr package
  * move exponentialSmoothing and clamp to control_toolbox
  * updated dependent packages accordingly

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp
    pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
    pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp
    pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp
    pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
    pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp
    pkg/trunk/deprecated/genericControllers/include/genericControllers/JointController.h
    pkg/trunk/deprecated/genericControllers/src/JointController.cpp
    pkg/trunk/deprecated/libKDL/manifest.xml
    pkg/trunk/deprecated/pr2Controllers/manifest.xml
    pkg/trunk/deprecated/pr2Core/include/pr2Core/pr2Misc.h
    pkg/trunk/deprecated/pr2Core/manifest.xml
    pkg/trunk/deprecated/rosControllers/manifest.xml.old
    pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
    pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
    pkg/trunk/motion_planning/ompl/manifest.xml
    pkg/trunk/nav/fake_localization/fake_localization.cpp
    pkg/trunk/nav/fake_localization/manifest.xml
    pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
    pkg/trunk/robot_descriptions/wg_robot_description_parser/src/URDF.cpp
    pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp
    pkg/trunk/util/angles/test/utest.cpp
    pkg/trunk/util/kinematics/robot_kinematics/manifest.xml
    pkg/trunk/util/libTF/manifest.xml
    pkg/trunk/util/libTF/src/test/testMatrixQuaternion.cc
    pkg/trunk/util/libTF/test/libTF_unittest.cpp
    pkg/trunk/util/libTF/test/pose3d_unittest.cpp
    pkg/trunk/util/tf/manifest.xml
    pkg/trunk/util/tf/test/cache_unittest.cpp
    pkg/trunk/util/tf/test/tf_unittest.cpp
    pkg/trunk/util/tf/test/tf_unittest_future.cpp

Added Paths:
-----------
    pkg/trunk/controllers/control_toolbox/include/control_toolbox/filters.h
    pkg/trunk/util/angles/
    pkg/trunk/util/angles/CMakeLists.txt
    pkg/trunk/util/angles/Makefile
    pkg/trunk/util/angles/doc.dox
    pkg/trunk/util/angles/include/
    pkg/trunk/util/angles/manifest.xml
    pkg/trunk/util/angles/src/
    pkg/trunk/util/angles/test/
    pkg/trunk/util/math_expr/
    pkg/trunk/util/math_expr/CMakeLists.txt
    pkg/trunk/util/math_expr/Makefile
    pkg/trunk/util/math_expr/doc.dox
    pkg/trunk/util/math_expr/include/
    pkg/trunk/util/math_expr/include/math_expr/
    pkg/trunk/util/math_expr/include/math_expr/MathExpression.h
    pkg/trunk/util/math_expr/manifest.xml
    pkg/trunk/util/math_expr/src/
    pkg/trunk/util/math_expr/src/MathExpression.cpp
    pkg/trunk/util/math_expr/test/
    pkg/trunk/util/math_expr/test/utest.cpp

Removed Paths:
-------------
    pkg/trunk/util/angles/CMakeLists.txt
    pkg/trunk/util/angles/Makefile
    pkg/trunk/util/angles/doc.dox
    pkg/trunk/util/angles/include/
    pkg/trunk/util/angles/manifest.xml
    pkg/trunk/util/angles/src/
    pkg/trunk/util/angles/test/
    pkg/trunk/util/math_utils/

Copied: pkg/trunk/controllers/control_toolbox/include/control_toolbox/filters.h (from rev 6882, pkg/trunk/util/math_utils/include/math_utils/math_utils.h)
===================================================================
--- pkg/trunk/controllers/control_toolbox/include/control_toolbox/filters.h	                        (rev 0)
+++ pkg/trunk/controllers/control_toolbox/include/control_toolbox/filters.h	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1,58 @@
+/*********************************************************************
+* 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 FILTERS_H
+#define FILTERS_H
+
+#include <algorithm>
+
+namespace filters
+{
+    
+    /** Clamp value a between b and c */
+    template<typename T>
+    static inline const T& clamp(const T &a, const T &b, const T &c)
+    {
+        return std::min<T>(std::max<T>(b, a), c);
+    }
+
+    /** Exponential smoothing filter. Alpha is between 0 and 1. Values closer to 0 weight the last smoothed value more heavily */
+    
+    static inline double exponentialSmoothing(double current_raw_value, double last_smoothed_value, double alpha)
+    {
+        return alpha*current_raw_value + (1-alpha)*last_smoothed_value;
+    }
+ }
+
+#endif

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h	2008-11-18 18:26:05 UTC (rev 6891)
@@ -53,9 +53,6 @@
 //Kinematics
 #include <robot_kinematics/robot_kinematics.h>
 
-// Math utils
-#include <math_utils/angles.h>
-
 // #include <libTF/Pose3D.h>
 // #include <urdf/URDF.h>
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-11-18 18:26:05 UTC (rev 6891)
@@ -52,7 +52,6 @@
 #include <std_msgs/VisualizationMarker.h>
 
 // Math utils
-#include <math_utils/angles.h>
 #include <math.h>
 #include <tf/transform_listener.h>
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h	2008-11-18 18:26:05 UTC (rev 6891)
@@ -52,7 +52,6 @@
 #include <std_msgs/VisualizationMarker.h>
 
 // Math utils
-#include <math_utils/angles.h>
 #include <math.h>
 #include <tf/transform_listener.h>
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -16,11 +16,12 @@
   <depend package="rosTF" />
   <depend package="std_msgs" />
   <depend package="pr2_msgs" />
-  <depend package="math_utils" />
   <depend package="robot_kinematics" />
   <depend package="rosout" />
   <depend package="trajectory" />
   <depend package="rosthread" />
+  <depend package="angles" />
+  <depend package="control_toolbox" />
   <url>http://pr.willowgarage.com</url>
   <repository>http://pr.willowgarage.com/repos</repository>
   <export>

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -36,6 +36,9 @@
 
 #include "pr2_mechanism_controllers/arm_dynamics_controller.h"
 
+// Math utils
+#include <angles/angles.h>
+
 using namespace controller;
 using namespace std;
 
@@ -216,12 +219,12 @@
     j_type = joint_effort_controllers_[i]->joint_state_->joint_->type_;
     if(j_type == mechanism::JOINT_ROTARY)
     {
-      if(!math_utils::shortest_angular_distance_with_limits(command, actual, joint_effort_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_effort_controllers_[i]->joint_state_->joint_->joint_limit_max_,error))
+      if(!angles::shortest_angular_distance_with_limits(command, actual, joint_effort_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_effort_controllers_[i]->joint_state_->joint_->joint_limit_max_,error))
         error = 0;
     }
     else if(j_type == mechanism::JOINT_CONTINUOUS)
     {
-      error = math_utils::shortest_angular_distance(command, actual);
+      error = angles::shortest_angular_distance(command, actual);
     }
     else
     {

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,8 +33,8 @@
  *********************************************************************/
 
 #include <pr2_mechanism_controllers/base_controller.h>
-#include <math_utils/angles.h>
-#include <math_utils/math_utils.h>
+#include <angles/angles.h>
+#include <control_toolbox/filters.h>
 #include "ros/node.h"
 
 #define NUM_TRANSFORMS 2
@@ -49,7 +49,6 @@
 using namespace control_toolbox;
 using namespace libTF;
 using namespace NEWMAT;
-using namespace math_utils;
 
 ROS_REGISTER_CONTROLLER(BaseController)
 
@@ -107,9 +106,9 @@
 {
 
   pthread_mutex_lock(&base_controller_lock_);
-  cmd_vel_t_.x = clamp(cmd_vel.x,-max_vel_.x, max_vel_.x);
-  cmd_vel_t_.y = clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
-  cmd_vel_t_.z = clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
+  cmd_vel_t_.x = filters::clamp(cmd_vel.x,-max_vel_.x, max_vel_.x);
+  cmd_vel_t_.y = filters::clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
+  cmd_vel_t_.z = filters::clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
   cmd_received_timestamp_ = robot_state_->hw_->current_time_;
 #if 0
 
@@ -132,9 +131,9 @@
 libTF::Vector BaseController::interpolateCommand(libTF::Vector start, libTF::Vector end, libTF::Vector max_rate, double dT)
 {
   libTF::Vector result;
-  result.x = start.x + clamp(end.x - start.x,-max_rate.x*dT,max_rate.x*dT);
-  result.y = start.y + clamp(end.y - start.y,-max_rate.y*dT,max_rate.y*dT);
-  result.z = start.z + clamp(end.z - start.z,-max_rate.z*dT,max_rate.z*dT);
+  result.x = start.x + filters::clamp(end.x - start.x,-max_rate.x*dT,max_rate.x*dT);
+  result.y = start.y + filters::clamp(end.y - start.y,-max_rate.y*dT,max_rate.y*dT);
+  result.z = start.z + filters::clamp(end.z - start.z,-max_rate.z*dT,max_rate.z*dT);
   return result;
 }
 */
@@ -476,10 +475,10 @@
       steer_angle_desired = atan2(result.y,result.x);
       steer_angle_stored_[i] = steer_angle_desired;
     }
-    steer_angle_desired_m_pi = normalize_angle(steer_angle_desired+M_PI);
+    steer_angle_desired_m_pi = angles::normalize_angle(steer_angle_desired+M_PI);
 
-    error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
-    error_steer_m_pi = shortest_angular_distance(steer_angle_desired_m_pi, steer_angle_actual_[i]);
+    error_steer = angles::shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
+    error_steer_m_pi = angles::shortest_angular_distance(steer_angle_desired_m_pi, steer_angle_actual_[i]);
 
     if(fabs(error_steer_m_pi) < fabs(error_steer))
     {
@@ -546,7 +545,7 @@
 
   odom_msg_.pos.x  = base_odom_position_.x;
   odom_msg_.pos.y  = base_odom_position_.y;
-  odom_msg_.pos.th = math_utils::normalize_angle(base_odom_position_.z);
+  odom_msg_.pos.th = angles::normalize_angle(base_odom_position_.z);
 
   odom_msg_.vel.x  = base_odom_velocity_.x;
   odom_msg_.vel.y  = base_odom_velocity_.y;
@@ -814,7 +813,7 @@
       out.z = 0;
       out.roll = 0;
       out.pitch = 0;
-      out.yaw = math_utils::normalize_angle(-yaw);
+      out.yaw = angles::normalize_angle(-yaw);
 
 
       rosTF::TransformEuler &out2 = transform_publisher_->msg_.eulers[1];

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -35,6 +35,8 @@
 // Original version: Melonee Wise <mw...@wi...>
 
 #include "pr2_mechanism_controllers/head_servoing_controller.h"
+// Math utils
+#include <angles/angles.h>
 
 using namespace controller;
 using namespace std;
@@ -160,7 +162,7 @@
 	
   for(unsigned int i=0; i < num_joints_;++i)
   {
-    math_utils::shortest_angular_distance_with_limits(joint_velocity_controllers_[i]->joint_state_->position_,set_pts_[i], joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_max_, error);
+    angles::shortest_angular_distance_with_limits(joint_velocity_controllers_[i]->joint_state_->position_,set_pts_[i], joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_max_, error);
     error=((gain_*error)<max_velocity_)?error:max_velocity_;
     joint_velocity_controllers_[i]->setCommand(error);
   }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,6 @@
  *********************************************************************/
 #include <algorithm>
 #include <pr2_mechanism_controllers/laser_scanner_controller.h>
-#include <math_utils/angles.h>
 
 using namespace std;
 using namespace controller;

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -44,7 +44,6 @@
 #include <algorithm>
 
 #include <pr2_mechanism_controllers/laser_scanner_qualification.h>
-#include <math_utils/angles.h>
 
 using namespace std;
 using namespace controller;

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,7 @@
  *********************************************************************/
 #include <algorithm>
 #include <pr2_mechanism_controllers/laser_scanner_traj_controller.h>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
 
 #include <math.h>
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -34,8 +34,7 @@
 #include <algorithm>
 
 #include <pr2_mechanism_controllers/laser_scanner_velocity_controller.h>
-#include <math_utils/angles.h>
-#include <math_utils/velocity.h>
+
 using namespace std;
 using namespace controller;
 #define MIN_MOVING_SPEED 0.3

Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -16,6 +16,7 @@
   <depend package="robot_srvs" />
   <depend package="robot_msgs" />
   <depend package="tf" />
+  <depend package="angles" />
   <url>http://pr.willowgarage.com</url>
   <repository>http://pr.willowgarage.com/repos</repository>
   <export>

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,6 @@
  *********************************************************************/
 
 #include <robot_mechanism_controllers/joint_autotuner.h>
-#include <math_utils/angles.h>
 #include <fstream>
 using namespace std;
 using namespace controller;

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,6 @@
  *********************************************************************/
 
 #include <robot_mechanism_controllers/joint_pd_controller.h>
-#include <math_utils/angles.h>
 
 using namespace std;
 using namespace controller;

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,7 @@
  *********************************************************************/
 
 #include <robot_mechanism_controllers/joint_position_controller.h>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
 
 using namespace std;
 using namespace controller;
@@ -134,12 +134,12 @@
 
   if(joint_state_->joint_->type_ == mechanism::JOINT_ROTARY)
   {
-    math_utils::shortest_angular_distance_with_limits(command_, joint_state_->position_, joint_state_->joint_->joint_limit_min_, joint_state_->joint_->joint_limit_max_,error);
+    angles::shortest_angular_distance_with_limits(command_, joint_state_->position_, joint_state_->joint_->joint_limit_min_, joint_state_->joint_->joint_limit_max_,error);
 
   }
   else if(joint_state_->joint_->type_ == mechanism::JOINT_CONTINUOUS)
   {
-    error = math_utils::shortest_angular_distance(command_, joint_state_->position_);
+    error = angles::shortest_angular_distance(command_, joint_state_->position_);
   }
   else //prismatic
   {

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,7 @@
  *********************************************************************/
 
 #include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
 
 using namespace std;
 using namespace controller;
@@ -145,12 +145,12 @@
 
   if(joint_state_->joint_->type_ == mechanism::JOINT_ROTARY)
   {
-    math_utils::shortest_angular_distance_with_limits(command_, joint_state_->position_, joint_state_->joint_->joint_limit_min_, joint_state_->joint_->joint_limit_max_,error);
+    angles::shortest_angular_distance_with_limits(command_, joint_state_->position_, joint_state_->joint_->joint_limit_min_, joint_state_->joint_->joint_limit_max_,error);
 
   }
   else if(joint_state_->joint_->type_ == mechanism::JOINT_CONTINUOUS)
   {
-    error = math_utils::shortest_angular_distance(command_, joint_state_->position_);
+    error = angles::shortest_angular_distance(command_, joint_state_->position_);
   }
   else //prismatic
   {

Modified: pkg/trunk/deprecated/genericControllers/include/genericControllers/JointController.h
===================================================================
--- pkg/trunk/deprecated/genericControllers/include/genericControllers/JointController.h	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/genericControllers/include/genericControllers/JointController.h	2008-11-18 18:26:05 UTC (rev 6891)
@@ -73,7 +73,7 @@
 
 #include <genericControllers/Controller.h>
 #include <genericControllers/Pid.h>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
 #include <mechanism_model/joint.h>
 #include <string>
 #include <math.h>

Modified: pkg/trunk/deprecated/genericControllers/src/JointController.cpp
===================================================================
--- pkg/trunk/deprecated/genericControllers/src/JointController.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/genericControllers/src/JointController.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -284,8 +284,8 @@
 
 double JointController::getMaxVelocity(){
       double disToMin,disToMax,closestLimit;
-      disToMin = fabs(math_utils::shortest_angular_distance(joint->position, joint->jointLimitMin));
-      disToMax = fabs(math_utils::shortest_angular_distance(joint->position, joint->jointLimitMax));
+      disToMin = fabs(angles::shortest_angular_distance(joint->position, joint->jointLimitMin));
+      disToMax = fabs(angles::shortest_angular_distance(joint->position, joint->jointLimitMax));
       closestLimit =  (disToMin<disToMax)?disToMin:disToMax; //min
       //std::cout<<"Dis to min"<<disToMin<<" Dist to Max"<<disToMax<<" Closest limit"<<closestLimit<<std::endl;
       return sqrt(fabs(closestLimit*maxAccel));
@@ -316,7 +316,7 @@
 
     case CONTROLLER_POSITION: //Close the loop around position
       if(joint->type == mechanism::JOINT_ROTARY || joint->type == mechanism::JOINT_CONTINUOUS)
-	error = math_utils::shortest_angular_distance(cmdPos, joint->position);
+	error = angles::shortest_angular_distance(cmdPos, joint->position);
       else
         error = joint->position - cmdPos;
       currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
@@ -384,7 +384,7 @@
 
     case CONTROLLER_POSITION: //Close the loop around position
       if(joint->type == mechanism::JOINT_ROTARY || joint->type == mechanism::JOINT_CONTINUOUS)
-	error = math_utils::shortest_angular_distance(cmdPos, joint->position);
+	error = angles::shortest_angular_distance(cmdPos, joint->position);
       else
         error = joint->position - cmdPos;
       currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);

Modified: pkg/trunk/deprecated/libKDL/manifest.xml
===================================================================
--- pkg/trunk/deprecated/libKDL/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/libKDL/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -8,7 +8,7 @@
 <depend package="pr2Core"/>
 <depend package="wg_robot_description_parser"/>
 <depend package="rosTF"/>
-<depend package="math_utils"/>
+<depend package="angles"/>
 <depend package="kdl"/>
 <export>
 <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lKDL"/>

Modified: pkg/trunk/deprecated/pr2Controllers/manifest.xml
===================================================================
--- pkg/trunk/deprecated/pr2Controllers/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/pr2Controllers/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -14,7 +14,7 @@
   <depend package="libKDL" />
   <depend package="wg_robot_description_parser" />
   <depend package="joy" />
-  <depend package="math_utils" />
+  <depend package="angles" />
   <url>http://pr.willowgarage.com</url>
   <repository>http://pr.willowgarage.com/repos</repository>
   <export>

Modified: pkg/trunk/deprecated/pr2Core/include/pr2Core/pr2Misc.h
===================================================================
--- pkg/trunk/deprecated/pr2Core/include/pr2Core/pr2Misc.h	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/pr2Core/include/pr2Core/pr2Misc.h	2008-11-18 18:26:05 UTC (rev 6891)
@@ -3,7 +3,7 @@
 
 #include <cmath>
 #include <cstdio>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
 
 namespace PR2
 {

Modified: pkg/trunk/deprecated/pr2Core/manifest.xml
===================================================================
--- pkg/trunk/deprecated/pr2Core/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/pr2Core/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -8,7 +8,7 @@
 <license>BSD</license>
 <review status="unreviewed" notes=""/>
 <url>http://pr.willowgarage.com</url>
-<depend package="math_utils" />
+<depend package="angles" />
 <export>
 <cpp cflags="-I${prefix}/include" lflags=""/>
 </export>

Modified: pkg/trunk/deprecated/rosControllers/manifest.xml.old
===================================================================
--- pkg/trunk/deprecated/rosControllers/manifest.xml.old	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/rosControllers/manifest.xml.old	2008-11-18 18:26:05 UTC (rev 6891)
@@ -10,7 +10,7 @@
   <depend package="std_msgs" />
   <depend package="rosthread" />
   <depend package="rosTF" />
-  <depend package="math_utils" />
+  <depend package="angles" />
   <depend package="string_utils" />
   <depend package="libKDL" />
   <depend package="robot_mechanism_model" />

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -18,7 +18,7 @@
   <depend package="robot_msgs" />
   <depend package="rosthread"/>
   <depend package="stl_utils" />
-  <depend package="math_utils" />
+  <depend package="angles" />
   <depend package="gazebo_robot_description"/>
   <depend package="hardware_interface" />
   <depend package="mechanism_control" />

Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -35,7 +35,7 @@
 /* \author Ioan Sucan */
 
 #include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
-#include <math_utils/angles.h>
+#include <angles/angles.h>
 #include <cstring>
 #include <valarray>
 #include <algorithm>
@@ -87,7 +87,7 @@
     for (unsigned int i = 0 ; i < dim ; ++i)
     {	 
 	double diff = m_si->getStateComponent(i).type == StateComponent::ANGLE ? 
-	    math_utils::shortest_angular_distance(sk1->values[i], sk2->values[i]) : sk1->values[i] - sk2->values[i];
+	    angles::shortest_angular_distance(sk1->values[i], sk2->values[i]) : sk1->values[i] - sk2->values[i];
 	dist += diff * diff;
     }
     return dist;

Modified: pkg/trunk/motion_planning/ompl/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/ompl/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/motion_planning/ompl/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -8,7 +8,7 @@
   <review status="unreviewed" notes=""/>
   <url></url>
 
-  <depend package="math_utils"/>
+  <depend package="angles"/>
   <depend package="random_utils"/>
 
   <export>

Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -78,7 +78,7 @@
 #include <std_msgs/ParticleCloud2D.h>
 #include <std_msgs/Pose2DFloat32.h>
 
-#include <math_utils/angles.h>
+#include <angles/angles.h>
 
 #include "tf/transform_broadcaster.h"
 
@@ -152,7 +152,7 @@
       double z = txi.getOrigin().z();
       double yaw, pitch, roll;
       txi.getBasis().getEulerZYX(yaw, pitch, roll);
-      yaw = math_utils::normalize_angle(yaw + m_iniPos.th);
+      yaw = angles::normalize_angle(yaw + m_iniPos.th);
 
       tf::Transform txo(tf::Quaternion(yaw, pitch, roll),
                         tf::Point(x, y, z));

Modified: pkg/trunk/nav/fake_localization/manifest.xml
===================================================================
--- pkg/trunk/nav/fake_localization/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/nav/fake_localization/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -6,5 +6,5 @@
   <depend package="roscpp" />
   <depend package="robot_msgs" />
   <depend package="tf" />
-  <depend package="math_utils" />
+  <depend package="angles" />
 </package>

Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -11,7 +11,7 @@
   <url>http://pr.willowgarage.com/wiki/RobotDescriptionFormat</url>
 
   <depend package="tinyxml"/>
-  <depend package="math_utils"/>
+  <depend package="math_expr"/>
   <depend package="string_utils"/>
   <depend package="stl_utils" />
 

Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/src/URDF.cpp
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/src/URDF.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/src/URDF.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -35,7 +35,7 @@
 /** \author Ioan Sucan */
 
 #include <urdf/URDF.h>
-#include <math_utils/MathExpression.h>
+#include <math_expr/MathExpression.h>
 #include <string_utils/string_utils.h>
 #include <algorithm>
 #include <cstring>

Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -38,7 +38,7 @@
 #include "tinyxml/tinyxml.h"
 #include "stl_utils/stl_utils.h"
 #include "string_utils/string_utils.h"
-#include "math_utils/MathExpression.h"
+#include "math_expr/MathExpression.h"
 
 
 namespace urdf {

Deleted: pkg/trunk/util/angles/CMakeLists.txt
===================================================================
--- pkg/trunk/util/math_utils/CMakeLists.txt	2008-11-18 15:19:18 UTC (rev 6882)
+++ pkg/trunk/util/angles/CMakeLists.txt	2008-11-18 18:26:05 UTC (rev 6891)
@@ -1,10 +0,0 @@
-cmake_minimum_required(VERSION 2.6)
-include(rosbuild)
-rospack(math_utils)
-rospack_add_library(math_utils src/MathExpression.cpp)
-
-# Unit tests
-rospack_add_gtest(utest test/utest.cpp)
-set_target_properties(utest PROPERTIES
-                      RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
-target_link_libraries(utest math_utils)

Copied: pkg/trunk/util/angles/CMakeLists.txt (from rev 6889, pkg/trunk/util/math_utils/CMakeLists.txt)
===================================================================
--- pkg/trunk/util/angles/CMakeLists.txt	                        (rev 0)
+++ pkg/trunk/util/angles/CMakeLists.txt	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(angles)
+
+# Unit tests
+rospack_add_gtest(utest test/utest.cpp)
+set_target_properties(utest PROPERTIES
+                      RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
+target_link_libraries(utest)

Deleted: pkg/trunk/util/angles/Makefile
===================================================================
--- pkg/trunk/util/math_utils/Makefile	2008-11-18 15:19:18 UTC (rev 6882)
+++ pkg/trunk/util/angles/Makefile	2008-11-18 18:26:05 UTC (rev 6891)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk

Copied: pkg/trunk/util/angles/Makefile (from rev 6889, pkg/trunk/util/math_utils/Makefile)
===================================================================
--- pkg/trunk/util/angles/Makefile	                        (rev 0)
+++ pkg/trunk/util/angles/Makefile	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

Deleted: pkg/trunk/util/angles/doc.dox
===================================================================
--- pkg/trunk/util/math_utils/doc.dox	2008-11-18 15:19:18 UTC (rev 6882)
+++ pkg/trunk/util/angles/doc.dox	2008-11-18 18:26:05 UTC (rev 6891)
@@ -1,12 +0,0 @@
-/**
-@mainpage
-
-@htmlinclude manifest.html
-
-Here is a list of math utilities:
- \li Symbolic expression interpretor (e.g. meval::EvaluateMathExpression)
- \li Misc. functions (e.g. math_utils::clamp) (should add max/min?)
- \li Smoothing functions (e.g. math_utils::exponentialSmoothing)
- \li Angular manipulations (e.g. math_utils::from_degrees, math_utils::to_degrees, math_utils::normalize_angle_positive, math_utils::shortest_angular_distance)
-
-**/

Copied: pkg/trunk/util/angles/doc.dox (from rev 6889, pkg/trunk/util/math_utils/doc.dox)
===================================================================
--- pkg/trunk/util/angles/doc.dox	                        (rev 0)
+++ pkg/trunk/util/angles/doc.dox	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1,12 @@
+/**
+@mainpage
+
+@htmlinclude manifest.html
+
+Here is a list of math utilities:
+ \li Symbolic expression interpretor (e.g. meval::EvaluateMathExpression)
+ \li Misc. functions (e.g. math_utils::clamp) (should add max/min?)
+ \li Smoothing functions (e.g. math_utils::exponentialSmoothing)
+ \li Angular manipulations (e.g. math_utils::from_degrees, math_utils::to_degrees, math_utils::normalize_angle_positive, math_utils::shortest_angular_distance)
+
+**/

Deleted: pkg/trunk/util/angles/manifest.xml
===================================================================
--- pkg/trunk/util/math_utils/manifest.xml	2008-11-18 15:19:18 UTC (rev 6882)
+++ pkg/trunk/util/angles/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -1,14 +0,0 @@
-<package>
-  <description brief="Math Utilities">
-      This package provides misc. math utilities,
-        such as angle conversions and unit conversions.
-      It also provides evaluation of simple mathematical expressions.
-  </description>
-  <author>John Hsu, Ioan Sucan</author>
-  <license>BSD</license>
-  <review status="unreviewed" notes="API review in progress (John)"/>
-  <export>
-    <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmath_utils"/>
-  </export>
-</package>
-

Copied: pkg/trunk/util/angles/manifest.xml (from rev 6889, pkg/trunk/util/math_utils/manifest.xml)
===================================================================
--- pkg/trunk/util/angles/manifest.xml	                        (rev 0)
+++ pkg/trunk/util/angles/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1,13 @@
+<package>
+  <description brief="Simple Angular Math Utilities">
+      This package provides simple angular math utilities,
+        such as angle conversions and shortest-angular-distance-calculations.
+  </description>
+  <author>John Hsu, Ioan Sucan</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes="API review in progress (John)"/>
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags=""/>
+  </export>
+</package>
+

Modified: pkg/trunk/util/angles/test/utest.cpp
===================================================================
--- pkg/trunk/util/math_utils/test/utest.cpp	2008-11-18 17:38:57 UTC (rev 6889)
+++ pkg/trunk/util/angles/test/utest.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -1,97 +1,55 @@
-#include "math_utils/MathExpression.h"
-#include "math_utils/angles.h"
-#include "math_utils/math_utils.h"
+#include "angles/angles.h"
 #include <gtest/gtest.h>
 
-#define TEST_EXPRESSION(a) EXPECT_EQ((a), meval::EvaluateMathExpression(#a))
-
-TEST(MathExpressions, operatorRecognition){
-  EXPECT_TRUE(meval::ContainsOperators("+"));
-  EXPECT_TRUE(meval::ContainsOperators("-"));
-  EXPECT_TRUE(meval::ContainsOperators("/"));
-  EXPECT_TRUE(meval::ContainsOperators("*"));
-  EXPECT_FALSE(meval::ContainsOperators("1234567890qwertyuiop[]asdfghjkl;'zxcvbnm,._=?8"));
-}
-
-TEST(MathExpressions, basicOperations){
-  EXPECT_EQ(5, meval::EvaluateMathExpression("2+3"));
-  EXPECT_EQ(5, meval::EvaluateMathExpression("2 + 3"));
-  EXPECT_EQ(10, meval::EvaluateMathExpression("20/2"));
-  EXPECT_EQ(-4, meval::EvaluateMathExpression("6 - 10"));
-  EXPECT_EQ(24, meval::EvaluateMathExpression("6 * 4"));
-}
-
-TEST(MathExpressions, complexOperations){
-  TEST_EXPRESSION(((3 + 4) / 2.0) + 10);
-  TEST_EXPRESSION(7 * (1 + 2 + 3 - 2 + 3.4) / 12.7);
-  TEST_EXPRESSION((1 + 2 + 3) - (8.0 / 10));
-}
-
-TEST(MathExpressions, UnaryMinus){
-  TEST_EXPRESSION(-5);
-}
-
-TEST(MathExpressions, badInput){
-  //TODO - figure out what good error behavior is and test for it properly
-  //EXPECT_EQ(0, meval::EvaluateMathExpression("4.1.3 - 4.1"));
-  //EXPECT_EQ(0, meval::EvaluateMathExpression("4.1.3"));
-}
-
-TEST(MathUtils, basicOperations){
-  EXPECT_EQ(math_utils::clamp<int>(-10, 10, 20), 10);
-  EXPECT_EQ(math_utils::clamp<int>(15, 10, 20), 15);
-  EXPECT_EQ(math_utils::clamp<int>(25, 10, 20), 20);
-}
-
-TEST(MathUtils, shortestDistanceWithLimits){
+TEST(Angles, shortestDistanceWithLimits){
   double shortest_angle;
-  bool result = math_utils::shortest_angular_distance_with_limits(-0.5, 0.5,-0.25,0.25,shortest_angle);
+  bool result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,-0.25,0.25,shortest_angle);
   EXPECT_TRUE(!result);
 
-  result = math_utils::shortest_angular_distance_with_limits(-0.5, 0.5,0.25,0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,0.25,0.25,shortest_angle);
   EXPECT_TRUE(!result);
 
-  result = math_utils::shortest_angular_distance_with_limits(-0.5, 0.5,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(result);
   EXPECT_NEAR(shortest_angle, -2*M_PI+1.0,1e-6);
 
-  result = math_utils::shortest_angular_distance_with_limits(0.5, 0.5,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(0.5, 0.5,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(result);
   EXPECT_NEAR(shortest_angle, 0,1e-6);
 
-  result = math_utils::shortest_angular_distance_with_limits(0.5, 0,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(0.5, 0,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(!result);
   EXPECT_NEAR(shortest_angle, -0.5,1e-6);
 
-  result = math_utils::shortest_angular_distance_with_limits(-0.5, 0,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(-0.5, 0,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(!result);
   EXPECT_NEAR(shortest_angle, 0.5,1e-6);
 
-  result = math_utils::shortest_angular_distance_with_limits(-0.2,0.2,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(-0.2,0.2,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(!result);
   EXPECT_NEAR(shortest_angle, -2*M_PI+0.4,1e-6);
 
-  result = math_utils::shortest_angular_distance_with_limits(0.2,-0.2,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(0.2,-0.2,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(!result);
   EXPECT_NEAR(shortest_angle,2*M_PI-0.4,1e-6);
 
-  result = math_utils::shortest_angular_distance_with_limits(0.2,0,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(0.2,0,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(!result);
   EXPECT_NEAR(shortest_angle,2*M_PI-0.2,1e-6);
 
-  result = math_utils::shortest_angular_distance_with_limits(-0.2,0,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(-0.2,0,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(!result);
   EXPECT_NEAR(shortest_angle,-2*M_PI+0.2,1e-6);
 
-  result = math_utils::shortest_angular_distance_with_limits(-0.25,-0.5,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(-0.25,-0.5,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(result);
   EXPECT_NEAR(shortest_angle,-0.25,1e-6);
 
-  result = math_utils::shortest_angular_distance_with_limits(-0.25,0.5,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(-0.25,0.5,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(result);
   EXPECT_NEAR(shortest_angle,-2*M_PI+0.75,1e-6);
 
-  result = math_utils::shortest_angular_distance_with_limits(-0.2500001,0.5,0.25,-0.25,shortest_angle);
+  result = angles::shortest_angular_distance_with_limits(-0.2500001,0.5,0.25,-0.25,shortest_angle);
   EXPECT_TRUE(result);
   EXPECT_NEAR(shortest_angle,-2*M_PI+0.5+0.2500001,1e-6);
 

Modified: pkg/trunk/util/kinematics/robot_kinematics/manifest.xml
===================================================================
--- pkg/trunk/util/kinematics/robot_kinematics/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/util/kinematics/robot_kinematics/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -7,7 +7,6 @@
 <url>http://pr.willowgarage.com</url>
 <depend package="wg_robot_description_parser"/>
 <depend package="libTF"/>
-<depend package="math_utils"/>
 <depend package="kdl"/>
 <depend package="roscpp"/>
 <depend package="std_msgs"/>

Modified: pkg/trunk/util/libTF/manifest.xml
===================================================================
--- pkg/trunk/util/libTF/manifest.xml	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/util/libTF/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -11,7 +11,7 @@
 <depend package="newmat10"/>
 <depend package="ros_exception"/>
 <depend package="rosthread"/>
-<depend package="math_utils"/>
+<depend package="angles"/>
 <depend package="std_msgs"/>
 <depend package="roscpp"/>
 <export>

Modified: pkg/trunk/util/libTF/src/test/testMatrixQuaternion.cc
===================================================================
--- pkg/trunk/util/libTF/src/test/testMatrixQuaternion.cc	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/util/libTF/src/test/testMatrixQuaternion.cc	2008-11-18 18:26:05 UTC (rev 6891)
@@ -1,11 +1,9 @@
 #include "libTF/libTF.h"
 
-#include <math_utils/angles.h>
+#include <angles/angles.h>
 #include <sys/time.h>
 #include <cstdlib>
 
-using namespace math_utils;
-
 int main(int argc, char ** argv)
 {
   //Seed random number generator with current microseond count
@@ -62,8 +60,8 @@
 	  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) &&
-              (fabs(modNPiBy2(out2.yaw) - modNPiBy2(yaw)) > 0.001 || fabs(modNPiBy2(out2.pitch) - modNPiBy2(pitch)) > 0.001 || fabs(modNPiBy2(out2.roll) -modNPiBy2(roll)) > 0.0001))
+	  if ((fabs(angles::modNPiBy2(out.yaw) - angles::modNPiBy2(yaw)) > 0.001 || fabs(angles::modNPiBy2(out.pitch) - angles::modNPiBy2(pitch)) > 0.001 || fabs(angles::modNPiBy2(out.roll) -angles::modNPiBy2(roll)) > 0.0001) &&
+              (fabs(angles::modNPiBy2(out2.yaw) - angles::modNPiBy2(yaw)) > 0.001 || fabs(angles::modNPiBy2(out2.pitch) - angles::modNPiBy2(pitch)) > 0.001 || fabs(angles::modNPiBy2(out2.roll) -angles::modNPiBy2(roll)) > 0.0001))
 	    {
 
 	      printf("in: %.3f %.3f %.3f\n",

Modified: pkg/trunk/util/libTF/test/libTF_unittest.cpp
===================================================================
--- pkg/trunk/util/libTF/test/libTF_unittest.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/util/libTF/test/libTF_unittest.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -1,6 +1,6 @@
 #include <gtest/gtest.h>
 #include <libTF/libTF.h>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
 #include <sys/time.h>
 
 
@@ -28,20 +28,20 @@
 
   odompose.x = 1;
   odompose.y = 0;
-  odompose.yaw = math_utils::from_degrees(60);
+  odompose.yaw = angles::from_degrees(60);
   odompose.frame = "3";
   odompose.time = atime;
 
   mappose.x = 30.0;
   mappose.y = 40.0;
-  //  mappose.yaw = math_utils::from_degrees(-36.0);
-  mappose.yaw = math_utils::from_degrees(90);
+  //  mappose.yaw = angles::from_degrees(-36.0);
+  mappose.yaw = angles::from_degrees(90);
   mappose.frame = "1";
   mappose.time = atime;
 
   apose.x = 0;
   apose.y = 0;
-  apose.yaw = math_utils::from_degrees(0);
+  apose.yaw = angles::from_degrees(0);
   apose.frame = "3";
   apose.time = atime;
 
@@ -52,10 +52,10 @@
   //diffpose.x = 10;
   //diffpose.y = 100;
   diffpose.z = 1000;
-  //diffpose.yaw = math_utils::from_degrees(0);
-  diffpose.pitch = math_utils::from_degrees(0);
-  //diffpose.roll = math_utils::from_degrees(90);
-  diffpose.roll = math_utils::from_degrees(0);
+  //diffpose.yaw = angles::from_degrees(0);
+  diffpose.pitch = angles::from_degrees(0);
+  //diffpose.roll = angles::from_degrees(90);
+  diffpose.roll = angles::from_degrees(0);
   //  diffpose.yaw = 0;
   diffpose.frame = "1";
   diffpose.time = atime;
@@ -106,16 +106,16 @@
 
   puts("--------------------------");
   printf("Odom: %.3f %.3f %.3f\n",
-         odompose.x, odompose.y, math_utils::to_degrees(odompose.yaw));
+         odompose.x, odompose.y, angles::to_degrees(odompose.yaw));
   printf("Map : %.3f %.3f %.3f\n",
-         mappose.x, mappose.y, math_utils::to_degrees(mappose.yaw));
+         mappose.x, mappose.y, angles::to_degrees(mappose.yaw));
   printf("Diff : %.3f %.3f %.3f %.3f %.3f %.3f\n",
-         diffpose.x, diffpose.y, diffpose.z, math_utils::to_degrees(diffpose.yaw), math_utils::to_degrees(diffpose.pitch), math_utils::to_degrees(diffpose.roll));
+         diffpose.x, diffpose.y, diffpose.z, angles::to_degrees(diffpose.yaw), angles::to_degrees(diffpose.pitch), angles::to_degrees(diffpose.roll));
   puts("--------------------------");
   printf("2D out Out odompose: %.3f %.3f %.3f\n",
-         result.x, result.y, math_utils::to_degrees(result.yaw));
+         result.x, result.y, angles::to_degrees(result.yaw));
   printf("2D out Out apose : %.3f %.3f %.3f\n",
-         result2.x, result2.y, math_utils::to_degrees(result2.yaw));
+         result2.x, result2.y, angles::to_degrees(result2.yaw));
   printf("Point : %.3f %.3f %.3f\n",
          point.x, point.y,point.z);
 
@@ -202,9 +202,9 @@
 			    diffpose.x,
 			    diffpose.y,
 			    diffpose.z,
-			    math_utils::from_degrees(90*ind),
-			    math_utils::from_degrees(90*ind1),
-			    math_utils::from_degrees(90*ind2),
+			    angles::from_degrees(90*ind),
+			    angles::from_degrees(90*ind1),
+			    angles::from_degrees(90*ind2),
 			    atime++);
 
 
@@ -270,9 +270,9 @@
 			    diffpose.x,
 			    diffpose.y,
 			    diffpose.z,
-			    math_utils::from_degrees(90*ind),
-			    math_utils::from_degrees(90*ind1),
-			    math_utils::from_degrees(90*ind2),
+			    angles::from_degrees(90*ind),
+			    angles::from_degrees(90*ind1),
+			    angles::from_degrees(90*ind2),
 			    atime++);
 
 

Modified: pkg/trunk/util/libTF/test/pose3d_unittest.cpp
===================================================================
--- pkg/trunk/util/libTF/test/pose3d_unittest.cpp	2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/util/libTF/test/pose3d_unittest.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -1,6 +1,6 @@
 #include <gtest/gtest.h>
 #include <libTF/Pose3D.h>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
 #include <sys/time.h>
 #include <cstdlib>
 
@@ -413,8 +413,8 @@
     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) &&
-                       (fabs(math_utils::modNPiBy2(out2.yaw) - math_utils::modNPiBy2(yaw)) > 0.001 || fabs(math_utils::modNPiBy2(out2.pitch) - math_utils::modNPiBy2(pitch)) > 0.001 || fabs(math_utils::modNPiBy2(out2.roll) -math_utils::modNPiBy2(roll)) > 0.0001));
+    bool difference = ((fabs(angles::modNPiBy2(out.yaw) - angles::modNPiBy2(yaw)) > 0.001 || fabs(angles::modNPiBy2(out.pitch) - angles::modNPiBy2(pitch)) > 0.001 || fabs(angles::modNPiBy2(out.roll) -angles::modNPiBy2(roll)) > 0.0001) &&
+                       (fabs(angles::modNPiBy2(out2.yaw) - angles::modNPiBy2(yaw)) > 0.001 || fabs(angles::modNPiBy2(out2.pitch) - angles::modNPiBy2(pitch)) > 0.001 || fabs(angles::modNPiBy2(out2.roll) -angles::modNPiBy2(roll)) > 0.0001));
           
     ASSERT_FALSE(difference);
     if (difference){

Added: pkg/trunk/util/math_expr/CMakeLists.txt
===================================================================
--- pkg/trunk/util/math_expr/CMakeLists.txt	                        (rev 0)
+++ pkg/trunk/util/math_expr/CMakeLists.txt	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(math_expr)
+rospack_add_library(math_expr src/MathExpression.cpp)
+
+# Unit tests
+rospack_add_gtest(utest test/utest.cpp)
+set_target_properties(utest PROPERTIES
+                      RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
+target_link_libraries(utest math_expr)

Added: pkg/trunk/util/math_expr/Makefile
===================================================================
--- pkg/trunk/util/math_expr/Makefile	                        (rev 0)
+++ pkg/trunk/util/math_expr/Makefile	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

Added: pkg/trunk/util/math_expr/doc.dox
===================================================================
--- pkg/trunk/util/math_expr/doc.dox	                        (rev 0)
+++ pkg/trunk/util/math_expr/doc.dox	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1,12 @@
+/**
+@mainpage
+
+@htmlinclude manifest.html
+
+Here is a list of math utilities:
+ \li Symbolic expression interpretor (e.g. meval::EvaluateMathExpression)
+ \li Misc. functions (e.g. math_utils::clamp) (should add max/min?)
+ \li Smoothing functions (e.g. math_utils::exponentialSmoothing)
+ \li Angular manipulations (e.g. math_utils::from_degrees, math_utils::to_degrees, math_utils::normalize_angle_positive, math_utils::shortest_angular_distance)
+
+**/

Added: pkg/trunk/util/math_expr/include/math_expr/MathExpression.h
===================================================================
--- pkg/trunk/util/math_expr/include/math_expr/MathExpression.h	                        (rev 0)
+++ pkg/trunk/util/math_expr/include/math_expr/MathExpression.h	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1,73 @@
+/*********************************************************************
+ * 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.
+ *********************************************************************/
+
+/** \Author Ioan Sucan */
+
+#ifndef MATH_EXPRESSION_
+#define MATH_EXPRESSION_
+
+#include <string>
+
+/** @htmlinclude ../../manifest.html
+
+    Evaluate simple mathematical expressions */
+
+namespace meval
+{
+
+/** \brief Callback for evaluating a variable */
+typedef double (*ExpressionVariableFn)(void *, std::string&);
+
+/** Given a mathematical expression in string format, compute
+ * what this expression evaluates to.  The expression can be
+ * arbitrarily parenthesised, but can only include the +, -, *, /
+ * mathematical operators. In addition to floating point constants,
+ * this function allows the use of named constants, if a callback is
+ * provided to evaluate those named constants. A data pointer for the
+
+ * callback is provided for convenience.
+ *
+ * Returns nan on error.
+ */
+double EvaluateMathExpression(const char *expression, ExpressionVariableFn var = NULL, void *data = NULL);
+double EvaluateMathExpression(const std::string &expression, ExpressionVariableFn var = NULL, void *data = NULL);
+
+/** Returns true if the expression contains any known mathematical operators */
+bool   ContainsOperators(const char        *expression);
+bool   ContainsOperators(const std::string &expression);
+
+}
+
+#endif
+

Added: pkg/trunk/util/math_expr/manifest.xml
===================================================================
--- pkg/trunk/util/math_expr/manifest.xml	                        (rev 0)
+++ pkg/trunk/util/math_expr/manifest.xml	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1,12 @@
+<package>
+  <description brief="Symbolic Mathematical Expression Evaluation Package">
+      This package provides simple symbolic mathematical expression evaluations.
+  </description>
+  <author>Ioan Sucan</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes="API review in progress (John)"/>
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmath_expr"/>
+  </export>
+</package>
+

Copied: pkg/trunk/util/math_expr/src/MathExpression.cpp (from rev 6882, pkg/trunk/util/math_utils/src/MathExpression.cpp)
===================================================================
--- pkg/trunk/util/math_expr/src/MathExpression.cpp	                        (rev 0)
+++ pkg/trunk/util/math_expr/src/MathExpression.cpp	2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1,167 @@
+/*********************************************************************
+ * 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, SPE...
 
[truncated message content]