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

[<stu...@us...>]

Revision: 3273
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3273&view=rev
Author:   stuglaser
Date:     2008-08-19 21:52:35 +0000 (Tue, 19 Aug 2008)

Log Message:
-----------
Subversion choked, so now these changes are being added as one commit.

Modified Paths:
--------------
    pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
    pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
    pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
    pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
    pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
    pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
    pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
    pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
    pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
    pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
    pkg/trunk/mechanism/mechanism_model/src/joint.cpp
    pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
    pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt
    pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
    pkg/trunk/robot_descriptions/wg_robot_description_parser/test/test_cpp_parser.cpp
    pkg/trunk/util/math_utils/include/math_utils/MathExpression.h
    pkg/trunk/util/math_utils/src/MathExpression.cpp
    pkg/trunk/util/string_utils/include/string_utils/string_utils.h
    pkg/trunk/util/string_utils/src/string_utils.cpp

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h	2008-08-19 21:52:35 UTC (rev 3273)
@@ -35,6 +35,8 @@
 #include <gazebo/Model.hh>
 #include "hardware_interface/hardware_interface.h"
 #include "mechanism_control/mechanism_control.h"
+#include "mechanism_model/robot.h"
+#include "tinyxml/tinyxml.h"
 
 
 namespace gazebo
@@ -63,15 +65,13 @@
   MechanismControl mc_;
   MechanismControlNode mcn_;
 
-  // Each joint in joints_ corresponds to the joint with the same
-  // index in mech_joints_.  The mech_joints_ vector exists so that
-  // each transmission has a mechanism::Joint to write to, because it
-  // would be best if the transmissions did not depend on Gazebo
-  // objects.
+  TiXmlDocument config_;
+
+  // The fake model helps Gazebo run the transmissions backwards, so
+  // that it can figure out what its joints should do based on the
+  // actuator values.
+  mechanism::Robot fake_model_;
   std::vector<gazebo::Joint*> joints_;
-  std::vector<mechanism::Joint*> mech_joints_;
-  std::vector<mechanism::Transmission*> transmissions_;
-  std::vector<std::string> actuator_names_;
 };
 
 }

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml	2008-08-19 21:52:35 UTC (rev 3273)
@@ -25,6 +25,7 @@
 <depend package="hardware_interface" />
 <depend package="mechanism_control" />
 <depend package="wg_robot_description_parser" />
+<depend package="tinyxml" />
 <export>
 <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
 </export>

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp	2008-08-19 21:52:35 UTC (rev 3273)
@@ -32,6 +32,7 @@
 #include <iostream>
 #include <math.h>
 #include <unistd.h>
+#include <set>
 #include <stl_utils/stl_utils.h>
 #include <gazebo/Global.hh>
 #include <gazebo/XMLConfig.hh>
@@ -42,18 +43,20 @@
 #include <gazebo/gazebo.h>
 #include <gazebo/GazeboError.hh>
 #include <gazebo/ControllerFactory.hh>
+#include <urdf/parser.h>
 
 namespace gazebo {
 
 GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_actuators", GazeboActuators);
 
 GazeboActuators::GazeboActuators(Entity *parent)
-  : Controller(parent), hw_(0), mc_(&hw_), mcn_(&mc_)
+  : Controller(parent), hw_(0), mc_(&hw_), mcn_(&mc_), fake_model_("fake")
 {
-   this->parent_model_ = dynamic_cast<Model*>(this->parent);
+  fake_model_.hw_ = &hw_;
+  this->parent_model_ = dynamic_cast<Model*>(this->parent);
 
-   if (!this->parent_model_)
-      gzthrow("GazeboActuators controller requires a Model as its parent");
+  if (!this->parent_model_)
+    gzthrow("GazeboActuators controller requires a Model as its parent");
 }
 
 GazeboActuators::~GazeboActuators()
@@ -63,75 +66,71 @@
 
 void GazeboActuators::LoadChild(XMLConfigNode *node)
 {
-  XMLConfigNode *xit;  // XML iterator
-
-  // Reads the joints out of Gazebo's config.
-  std::map<std::string,int> joint_lookup;
-  for (xit = node->GetChild("joint"); xit; xit = xit->GetNext("joint"))
+  XMLConfigNode *robot = node->GetChild("robot");
+  if (!robot)
   {
-    std::string joint_name = xit->GetString("name", "", 1);
-    gazebo::Joint *joint = parent_model_->GetJoint(joint_name);
-    assert(joint != NULL);
-    joints_.push_back(joint);
-    mech_joints_.push_back(new mechanism::Joint);
-
-    joint_lookup.insert(std::pair<std::string,int>(joint_name, joints_.size()-1));
+    fprintf(stderr, "Error loading gazebo_actuators config: no robot element\n");
+    return;
   }
 
-  // Reads the transmission information from the config.
-  for (xit = node->GetChild("transmission"); xit; xit = xit->GetNext("transmission"))
+  // TODO: should look for the file relative to the path of the world file.
+  std::string filename = robot->GetString("filename", "", 1);
+  printf("Loading %s\n", filename.c_str());
+
+  TiXmlDocument doc(filename);
+  doc.LoadFile();
+  assert(doc.RootElement());
+  urdf::normalizeXml(doc.RootElement());
+
+  // Pulls out the list of actuators used in the robot configuration.
+  struct GetActuators : public TiXmlVisitor
   {
-    if (0 == strcmp("SimpleTransmission", xit->GetString("type", "", 0).c_str()))
+    std::set<std::string> actuators;
+    virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
     {
-      // Looks up the joint by name
-      XMLConfigNode *joint_node = xit->GetChild("joint");
-      assert(joint_node != NULL);
-      std::map<std::string,int>::iterator jit = joint_lookup.find(joint_node->GetString("name", "", 1));
-      if (jit == joint_lookup.end())
-      {
-        // TODO: report: Could not find the joint named xxxx
-        continue;
-      }
+      if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name"))
+        actuators.insert(elt.Attribute("name"));
+      return true;
+    }
+  } get_actuators;
+  doc.RootElement()->Accept(&get_actuators);
 
-      // Creates the actuator
-      XMLConfigNode *actuator_node = xit->GetChild("actuator");
-      assert(actuator_node != NULL);
-      Actuator *actuator = new Actuator;
-      hw_.actuators_.push_back(actuator);
-      actuator_names_.push_back(actuator_node->GetString("name", "", 1));
+  // Places the found actuators into the hardware interface.
+  std::set<std::string>::iterator it;
+  for (it = get_actuators.actuators.begin(); it != get_actuators.actuators.end(); ++it)
+  {
+    hw_.actuators_.push_back(new Actuator(*it));
+  }
 
-      // Creates the transmission
-      transmissions_.push_back(
-        new mechanism::SimpleTransmission(mech_joints_[jit->second], actuator,
-                                          xit->GetDouble("mechanicalReduction", 0.0, 1),
-                                          xit->GetDouble("motorTorqueConstant", 0.0, 1),
-                                          xit->GetDouble("pulsesPerRevolution", 0.0, 1)));
+  // Initializes the fake model (for running the transmissions backwards).
+  fake_model_.initXml(doc.RootElement());
 
-    }
+  // The gazebo joints and mechanism joints should match up.
+  for (unsigned int i = 0; i < fake_model_.joints_.size(); ++i)
+  {
+    std::string joint_name = fake_model_.joints_[i]->name_;
+    gazebo::Joint *joint = parent_model_->GetJoint(joint_name);
+    if (joint)
+      joints_.push_back(joint);
     else
     {
-      // TODO: report: Unknown transmission type
-      continue;
+      fprintf(stderr, "Gazebo does not know about a joint named \"%s\"\n", joint_name.c_str());
+      joints_.push_back(NULL);
     }
   }
+
+  hw_.current_time_ = Simulator::Instance()->GetSimTime();
+  mcn_.initXml(doc.RootElement());
 }
 
 void GazeboActuators::InitChild()
 {
-  // Registers the actuators with MechanismControl
-  assert(actuator_names_.size() == hw_.actuators_.size());
-  for (unsigned int i = 0; i < actuator_names_.size(); ++i)
-    mc_.registerActuator(actuator_names_[i], i);
-
-
-  // TODO: mc.init();
-
   hw_.current_time_ = Simulator::Instance()->GetSimTime();
 }
 
 void GazeboActuators::UpdateChild()
 {
-  assert(joints_.size() == mech_joints_.size());
+  assert(joints_.size() == fake_model_.joints_.size());
 
   //--------------------------------------------------
   //  Pushes out simulation state
@@ -140,22 +139,23 @@
   // Copies the state from the gazebo joints into the mechanism joints.
   for (unsigned int i = 0; i < joints_.size(); ++i)
   {
+    if (!joints_[i])
+      continue;
     assert(joints_[i]->GetType() == Joint::HINGE);
     HingeJoint *hj = (HingeJoint*)joints_[i];
-    mech_joints_[i]->position_ = hj->GetAngle();
-    mech_joints_[i]->velocity_ = hj->GetAngleRate();
-    mech_joints_[i]->applied_effort_ = mech_joints_[i]->commanded_effort_;
+    fake_model_.joints_[i]->position_ = hj->GetAngle();
+    fake_model_.joints_[i]->velocity_ = hj->GetAngleRate();
+    fake_model_.joints_[i]->applied_effort_ = fake_model_.joints_[i]->commanded_effort_;
   }
 
   // Reverses the transmissions to propagate the joint position into the actuators.
-  for (unsigned int i = 0; i < transmissions_.size(); ++i)
-    transmissions_[i]->propagatePositionBackwards();
+  for (unsigned int i = 0; i < fake_model_.transmissions_.size(); ++i)
+    fake_model_.transmissions_[i]->propagatePositionBackwards();
 
   //--------------------------------------------------
   //  Runs Mechanism Control
   //--------------------------------------------------
   hw_.current_time_ = Simulator::Instance()->GetSimTime();
-  //mc_.update();
   mcn_.update();
 
   //--------------------------------------------------
@@ -163,15 +163,17 @@
   //--------------------------------------------------
 
   // Reverses the transmissions to propagate the actuator commands into the joints.
-  for (unsigned int i = 0; i < transmissions_.size(); ++i)
-    transmissions_[i]->propagateEffortBackwards();
+  for (unsigned int i = 0; i < fake_model_.transmissions_.size(); ++i)
+    fake_model_.transmissions_[i]->propagateEffortBackwards();
 
   // Copies the commands from the mechanism joints into the gazebo joints.
   for (unsigned int i = 0; i < joints_.size(); ++i)
   {
+    if (!joints_[i])
+      continue;
     assert(joints_[i]->GetType() == Joint::HINGE);
     HingeJoint *hj = (HingeJoint*)joints_[i];
-    hj->SetTorque(mech_joints_[i]->commanded_effort_);
+    hj->SetTorque(fake_model_.joints_[i]->commanded_effort_);
   }
 }
 

Modified: pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h	2008-08-19 21:52:35 UTC (rev 3273)
@@ -35,6 +35,8 @@
 #ifndef HARDWARE_INTERFACE_H
 #define HARDWARE_INTERFACE_H
 
+#include <string>
+#include <vector>
 #include <stl_utils/stl_utils.h>
 
 class ActuatorState{
@@ -81,6 +83,11 @@
 class Actuator
 {
 public:
+  Actuator() {}
+  Actuator(const std::string &name) : name_(name) {}
+  ~Actuator() {}
+
+  std::string name_;
   ActuatorState state_;
   ActuatorCommand command_;
 };

Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h	2008-08-19 21:52:35 UTC (rev 3273)
@@ -70,7 +70,6 @@
 
   // Non real-time functions
   bool initXml(TiXmlElement* config);
-  bool registerActuator(const std::string &name, int index);
   void getControllerNames(std::vector<std::string> &v);
   bool addController(controller::Controller *c, const std::string &name);
   bool spawnController(const std::string &type, const std::string &name, TiXmlElement *config);
@@ -78,7 +77,7 @@
   bool addJoint(mechanism::Joint* j);
   bool addSimpleTransmission(mechanism::SimpleTransmission *st);
   controller::Controller* getControllerByName(std::string name);
- 
+
   mechanism::Robot model_;
   HardwareInterface *hw_;
 
@@ -127,19 +126,19 @@
                       mechanism_control::KillController::response &resp);
 
   MechanismControl *mc_;
-  
+
   // Non-realtime thread for publishing state information.
   pthread_t *state_publishing_thread_;
   void statePublishingLoop(void);
   bool state_publishing_loop_keep_running_;
   static const double STATE_PUBLISHING_PERIOD = 0.1;  // in seconds, higher rates are useless with the current speed of the simulator
 
-  
+
   // Structure for transfering the mechanism state between realtime and non-realtime.
   pthread_mutex_t mechanism_state_lock_;
   // Blocks the ros publishing loop until an update is received from HW loop.
   bool mechanism_state_updated_;
-  pthread_cond_t mechanism_state_updated_cond_; 
+  pthread_cond_t mechanism_state_updated_cond_;
   mechanism_control::MechanismState mechanism_state_;
   void publishMechanismState(); // Not realtime safe
   const char *  const mechanism_state_topic_;

Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2008-08-19 21:52:35 UTC (rev 3273)
@@ -43,57 +43,20 @@
 {
 }
 
-bool MechanismControl::registerActuator(const std::string &name, int index)
-{
-  if (initialized_)
-    return false;
-
-  model_.actuators_lookup_.insert(Robot::IndexMap::value_type(name, index));
-
-  return true;
-}
-
 bool MechanismControl::initXml(TiXmlElement* config)
 {
-  bool successful = true;
   ros::node *node = ros::node::instance();
 
-  TiXmlElement *elt;
+  model_.initXml(config);
 
-  // Construct the joints
-  for (elt = config->FirstChildElement("joint"); elt; elt = elt->NextSiblingElement("joint"))
-  {
-    Joint *j = new Joint;
-    model_.joints_.push_back(j);
-    model_.joints_lookup_.insert(Robot::IndexMap::value_type(elt->Attribute("name"), model_.joints_.size() - 1));
-    j->initXml(elt);
-  }
-
-  // Construct the transmissions
-  elt = config->FirstChildElement("transmission");
-  for (; elt; elt = elt->NextSiblingElement("transmission"))
-  {
-    const char *type = elt->Attribute("type");
-    Transmission *t = TransmissionFactory::instance().create(type);
-    if (t == NULL)
-      node->log(ros::FATAL, "Unknown transmission type: %s\n", type);
-    t->initXml(elt, &model_);
-    model_.transmissions_.push_back(t);
-  }
-
   initialized_ = true;
-  return successful;
+  return true;
 }
 
 bool MechanismControl::addJoint(Joint* j)
 {
-  bool successful = true;
-
-  // add the joints
   model_.joints_.push_back(j);
-  model_.joints_lookup_.insert(Robot::IndexMap::value_type(j->name_, model_.joints_.size() - 1));
-
-  return successful;
+  return true;
 }
 
 bool MechanismControl::addSimpleTransmission(SimpleTransmission *st)
@@ -130,11 +93,11 @@
   // Propagates through the robot model.
   for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
     model_.transmissions_[i]->propagatePosition();
-    
+
   //zero all commands
   for (unsigned int i = 0; i < model_.joints_.size(); ++i)
     model_.joints_[i]->commanded_effort_= 0.0;
-    
+
   // TODO: update KDL model with new joint position/velocities
 
   // Update all controllers
@@ -185,7 +148,7 @@
 {
   //Add controller to list of controllers in realtime-safe manner;
   controllers_lock_.lock(); //This lock is only to prevent us from other non-realtime threads.  The realtime thread may be spinning through the list of controllers while we are in here, so we need to keep that list always in a valid state.  This is why we fully allocate and set up the controller before adding it into the list of active controllers.
-  
+
   bool spot_found = false;
   for (int i = 0; i < MAX_NUM_CONTROLLERS; i++)
   {
@@ -261,6 +224,9 @@
   assert(mc != NULL);
   assert(mechanism_state_topic_);
   if ((node = ros::node::instance()) == NULL) {
+    int argc = 0;
+    char** argv = NULL;
+    ros::init(argc, argv);
     node = new ros::node("mechanism_control", ros::node::DONT_HANDLE_SIGINT);
   }
 
@@ -282,7 +248,7 @@
 }
 
 MechanismControlNode::~MechanismControlNode()
-{ 
+{
   state_publishing_loop_keep_running_ = false;
 }
 
@@ -297,7 +263,7 @@
 void MechanismControlNode::update()
 {
   mc_->update();
- 
+
   // Attempts to lock the transfer structure
   // If we get it, update it
   if(pthread_mutex_trylock(&mechanism_state_lock_))

Modified: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt	2008-08-19 21:52:35 UTC (rev 3273)
@@ -1,4 +1,4 @@
 cmake_minimum_required(VERSION 2.6)
 include(rosbuild)
 rospack(mechanism_model)
-rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp)
+rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp src/robot.cpp)

Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h	2008-08-19 21:52:35 UTC (rev 3273)
@@ -31,6 +31,9 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
+/*
+ *
+ */
 #ifndef JOINT_H
 #define JOINT_H
 
@@ -40,8 +43,11 @@
 
 class Joint{
 public:
+  Joint() : commanded_effort_(0) {}
+  ~Joint() {}
+
   void enforceLimits();
-  void initXml(TiXmlElement *elt);
+  bool initXml(TiXmlElement *elt);
 
   std::string name_;
   int type_;

Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h	2008-08-19 21:52:35 UTC (rev 3273)
@@ -45,13 +45,15 @@
 #include "mechanism_model/transmission.h"
 #include "hardware_interface/hardware_interface.h"
 
+class TiXmlElement;
+
 namespace mechanism
 {
 
 class Robot
 {
 public:
-  Robot(char *ns){}
+  Robot(const char *ns){}
 
   ~Robot()
   {
@@ -59,28 +61,13 @@
     deleteElements(&joints_);
   }
 
+  bool initXml(TiXmlElement *root);
+
   std::vector<Joint*> joints_;
   std::vector<Transmission*> transmissions_;
 
-  // Supports looking up joints and actuators by name.  The IndexMap
-  // structure maps the name of the item to its index in the vectors.
-  typedef std::map<std::string,int> IndexMap;
-  IndexMap joints_lookup_;
-  IndexMap actuators_lookup_;
-  Joint* getJoint(const std::string &name)
-  {
-    IndexMap::iterator it = joints_lookup_.find(name);
-    if (it == joints_lookup_.end())
-      return NULL;
-    return joints_[it->second];
-  }
-  Actuator* getActuator(const std::string &name)
-  {
-    IndexMap::iterator it = actuators_lookup_.find(name);
-    if (it == actuators_lookup_.end())
-      return NULL;
-    return hw_->actuators_[it->second];
-  }
+  Joint* getJoint(const std::string &name);
+  Actuator* getActuator(const std::string &name);
 
   HardwareInterface *hw_;
 };

Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h	2008-08-19 21:52:35 UTC (rev 3273)
@@ -61,7 +61,7 @@
   virtual ~Transmission() {}
 
   // Initialize transmission from XML data
-  virtual void initXml(TiXmlElement *config, Robot *robot) = 0;
+  virtual bool initXml(TiXmlElement *config, Robot *robot) = 0;
 
   // another way to initialize simple transmission
   virtual void initTransmission(std::string transmission_name,std::string joint_name,std::string actuator_name,double mechanical_reduction,double motor_torque_constant,double pulses_per_revolution, Robot *robot) = 0;
@@ -88,7 +88,7 @@
   SimpleTransmission(Joint *joint, Actuator *actuator, double mechanical_reduction, double motor_torque_constant, double pulses_per_revolution);
   ~SimpleTransmission() {}
 
-  void initXml(TiXmlElement *config, Robot *robot);
+  bool initXml(TiXmlElement *config, Robot *robot);
 
   void initTransmission(std::string transmission_name,std::string joint_name,std::string actuator_name,double mechanical_reduction,double motor_torque_constant,double pulses_per_revolution, Robot *robot);
   std::string name_;

Modified: pkg/trunk/mechanism/mechanism_model/src/joint.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/joint.cpp	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/src/joint.cpp	2008-08-19 21:52:35 UTC (rev 3273)
@@ -32,9 +32,10 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
+#include <mechanism_model/joint.h>
 #include <map>
 #include <string>
-#include <mechanism_model/joint.h>
+#include <cfloat>
 
 using namespace std;
 using namespace mechanism;
@@ -54,20 +55,48 @@
   commanded_effort_ = min(max(commanded_effort_, -effort_limit_), effort_limit_);
 }
 
-void Joint::initXml(TiXmlElement *elt)
+bool Joint::initXml(TiXmlElement *elt)
 {
-  TiXmlElement *min = elt->FirstChildElement("limitMin");
-  TiXmlElement *max = elt->FirstChildElement("limitMax");
-  joint_limit_min_ = min ? atof(min->GetText()) : 0;
-  joint_limit_max_ = max ? atof(max->GetText()) : 0;
-  effort_limit_ = atof(elt->FirstChildElement("effortLimit")->GetText());
-  velocity_limit_ = atof(elt->FirstChildElement("velocityLimit")->GetText());
+  const char *name = elt->Attribute("name");
+  if (!name)
+  {
+    fprintf(stderr, "Error: unnamed joint found\n");
+    return false;
+  }
+  name_ = name;
 
-  type_ = g_type_map[elt->Attribute("type")];
-  // If type is revolute and limits aren't set, then it is continuous
-  if (type_ == JOINT_ROTARY && min == NULL && max == NULL)
+  const char *type = elt->Attribute("type");
+  if (!type)
   {
-    type_ = JOINT_CONTINUOUS;
+    fprintf(stderr, "Error: Joint \"%s\" has no type.\n", name_.c_str());
+    return false;
   }
+  type_ = g_type_map[type];
+
+  TiXmlElement *limits = elt->FirstChildElement("limit");
+  if (limits)
+  {
+    if (limits->QueryDoubleAttribute("effort", &effort_limit_) != TIXML_SUCCESS)
+      effort_limit_ = 0.0;
+    if (limits->QueryDoubleAttribute("velocity", &velocity_limit_) != TIXML_SUCCESS)
+      velocity_limit_ = 0.0;
+
+
+    int min_ret = limits->QueryDoubleAttribute("min", &joint_limit_min_);
+    int max_ret = limits->QueryDoubleAttribute("max", &joint_limit_max_);
+
+    if (type_ == JOINT_ROTARY && min_ret == TIXML_NO_ATTRIBUTE && max_ret == TIXML_NO_ATTRIBUTE)
+    {
+      type_ = JOINT_CONTINUOUS;
+    }
+  }
+  else
+  {
+    if (type_ == JOINT_ROTARY)
+      type_ = JOINT_CONTINUOUS;
+    effort_limit_ = DBL_MAX;
+  }
+
+  return true;
 }
 

Modified: pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp	2008-08-19 21:52:35 UTC (rev 3273)
@@ -50,13 +50,30 @@
   joint_ = joint;
 }
 
-void SimpleTransmission::initXml(TiXmlElement *elt, Robot *robot)
+bool SimpleTransmission::initXml(TiXmlElement *elt, Robot *robot)
 {
-  joint_ = robot->getJoint(elt->FirstChildElement("joint")->Attribute("name"));
-  actuator_ = robot->getActuator(elt->FirstChildElement("actuator")->Attribute("name"));
+  TiXmlElement *jel = elt->FirstChildElement("joint");
+  const char *joint_name = jel ? jel->Attribute("name") : NULL;
+  joint_ = joint_name ? robot->getJoint(joint_name) : NULL;
+  if (!joint_)
+  {
+    fprintf(stderr, "SimpleTransmission could not find joint named \"%s\"\n", joint_name);
+    return false;
+  }
+
+  TiXmlElement *ael = elt->FirstChildElement("actuator");
+  const char *actuator_name = ael ? ael->Attribute("name") : NULL;
+  actuator_ = actuator_name ? robot->getActuator(actuator_name) : NULL;
+  if (!actuator_)
+  {
+    fprintf(stderr, "SimpleTransmission could not find actuator named \"%s\"\n", actuator_name);
+    return false;
+  }
+
   mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText()),
   motor_torque_constant_ = atof(elt->FirstChildElement("motorTorqueConstant")->GetText()),
   pulses_per_revolution_ = atof(elt->FirstChildElement("pulsesPerRevolution")->GetText());
+  return true;
 }
 
 void SimpleTransmission::initTransmission(std::string transmission_name,std::string joint_name,std::string actuator_name,double mechanical_reduction,double motor_torque_constant,double pulses_per_revolution, Robot *robot)
@@ -73,14 +90,16 @@
 
 void SimpleTransmission::propagatePosition()
 {
+  assert(joint_);  assert(actuator_);
   joint_->position_ = ((double)actuator_->state_.encoder_count_*2*M_PI)/(pulses_per_revolution_ * mechanical_reduction_);
   joint_->velocity_ = ((double)actuator_->state_.encoder_velocity_*2*M_PI)/(pulses_per_revolution_ * mechanical_reduction_);
   joint_->applied_effort_ = actuator_->state_.last_measured_current_ * (motor_torque_constant_ * mechanical_reduction_);
-  
+
 }
 
 void SimpleTransmission::propagatePositionBackwards()
 {
+  assert(joint_);  assert(actuator_);
   actuator_->state_.encoder_count_ = (int)(joint_->position_ * pulses_per_revolution_ * mechanical_reduction_ / (2*M_PI));
   actuator_->state_.encoder_velocity_ = joint_->velocity_ * pulses_per_revolution_ * mechanical_reduction_ / (2*M_PI);
   actuator_->state_.last_measured_current_ = joint_->applied_effort_ / (motor_torque_constant_ * mechanical_reduction_);
@@ -88,13 +107,14 @@
 
 void SimpleTransmission::propagateEffort()
 {
+  assert(joint_);  assert(actuator_);
   actuator_->command_.current_ = joint_->commanded_effort_/(motor_torque_constant_ * mechanical_reduction_);
   actuator_->command_.enable_ = true;
- 
+
 }
 
 void SimpleTransmission::propagateEffortBackwards()
 {
+  assert(joint_);  assert(actuator_);
   joint_->commanded_effort_ = actuator_->command_.current_ * motor_torque_constant_ * mechanical_reduction_;
-  
 }

Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt	2008-08-19 21:52:35 UTC (rev 3273)
@@ -4,7 +4,7 @@
 
 set(ROS_COMPILE_FLAGS "-W -Wall -Wextra")
 
-rospack_add_library(URDF src/urdf/URDF.cpp)
+rospack_add_library(URDF src/urdf/URDF.cpp src/parser.cpp)
 rospack_add_executable(parse src/urdf/parse.cpp)
 target_link_libraries(parse URDF)
 rospack_add_executable(merge src/urdf/merge.cpp)

Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml	2008-08-19 21:52:35 UTC (rev 3273)
@@ -12,10 +12,11 @@
   <depend package="tinyxml"/>
   <depend package="math_utils"/>
   <depend package="string_utils"/>
+  <depend package="stl_utils" />
   <depend package="gtest"/>
-  
+
   <export>
     <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lURDF"/>
   </export>
-  
+
 </package>

Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/test/test_cpp_parser.cpp
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/test/test_cpp_parser.cpp	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/test/test_cpp_parser.cpp	2008-08-19 21:52:35 UTC (rev 3273)
@@ -1,13 +1,13 @@
 /*********************************************************************
  * 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
@@ -17,7 +17,7 @@
  *   * 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
@@ -33,8 +33,10 @@
  *********************************************************************/
 
 #include <urdf/URDF.h>
+#include <urdf/parser.h>
 #include <gtest/gtest.h>
 #include <cstdlib>
+#include <cmath>
 #include <sstream>
 #include <fstream>
 #include <string>
@@ -50,12 +52,12 @@
     URDF file;
     file.loadFile("test/data/test1.xml");
     EXPECT_TRUE(file.getErrorCount() == 0);
-    
+
     std::ofstream f("/tmp/test1.txt");
     file.print(f);
     f.close();
     int result = runExternalProcess("diff", "test/data/test1.txt /tmp/test1.txt");
-        
+
     EXPECT_TRUE(result == 0);
 }
 
@@ -64,12 +66,12 @@
     URDF file;
     file.loadFile("test/data/test2.xml");
     EXPECT_TRUE(file.getErrorCount() == 0);
-    
+
     std::ofstream f("/tmp/test2.txt");
     file.print(f);
     f.close();
     int result = runExternalProcess("diff", "test/data/test2.txt /tmp/test2.txt");
-        
+
     EXPECT_TRUE(result == 0);
 }
 
@@ -84,17 +86,93 @@
     file.print(f);
     f.close();
     int result = runExternalProcess("diff", "test/data/test3.txt /tmp/test3.txt");
-        
+
     EXPECT_TRUE(result == 0);
 }
 
 TEST(URDF, Valgrind)
 {
     int result = runExternalProcess("./run_valgrind.py", "./parse test/data/test3.xml");
-        
+
     EXPECT_TRUE(result == 0);
 }
 
+//------------------------------------------------------------------------------
+//  Normalization
+
+bool xmlMatches(TiXmlElement *a, TiXmlElement *b)
+{
+  if (a == NULL && b == NULL)
+    return true;
+  if (a == NULL || b == NULL)
+    return false;
+
+  if (a->ValueStr() != b->ValueStr())
+    return false;
+
+  for (TiXmlAttribute *att = a->FirstAttribute(); att; att = att->Next())
+  {
+    std::string va = att->ValueStr();
+    std::string vb = b->Attribute(att->Name());
+
+    if (atof(va.c_str()) != 0)
+    {
+      if (fabs(atof(va.c_str()) - atof(vb.c_str())) > 0.00000001)
+        return false;
+    }
+    else if (att->ValueStr() != b->Attribute(att->Name()))
+      return false;
+  }
+
+  if (!xmlMatches(a->FirstChildElement(), b->FirstChildElement()))
+    return false;
+  if (!xmlMatches(a->NextSiblingElement(), b->NextSiblingElement()))
+    return false;
+
+  return true;
+}
+
+TEST(URDF, NormalizeASimpleDoc)
+{
+  TiXmlDocument xml, desired;
+  xml.Parse("<should do='nothing'>hopefully</should>");
+  desired.Parse("<should do='nothing'>hopefully</should>");
+  EXPECT_TRUE(urdf::normalizeXml(xml.RootElement()));
+  EXPECT_TRUE(xmlMatches(xml.RootElement(), desired.RootElement()));
+}
+
+TEST(URDF, NormalizeOneConstant)
+{
+  TiXmlDocument xml, desired;
+  xml.Parse("<x><const name='huzzah' value='3.0' /><foo bar='huzzah' /></x>");
+  desired.Parse("<x><foo bar='3.0' /></x>");
+  EXPECT_TRUE(urdf::normalizeXml(xml.RootElement()));
+  EXPECT_TRUE(xmlMatches(xml.RootElement(), desired.RootElement()));
+}
+
+TEST(URDF, NormalizeOneBlock)
+{
+  TiXmlDocument xml, desired;
+  xml.Parse("<x><const_block name='foo'><bar meaning='nothing' /></const_block> \
+<insert_const_block name='foo' /></x>");
+  desired.Parse("<x><bar meaning='nothing' /></x>");
+  EXPECT_TRUE(urdf::normalizeXml(xml.RootElement()));
+  EXPECT_TRUE(xmlMatches(xml.RootElement(), desired.RootElement()));
+}
+
+TEST(URDF, NormalizeAnExpressionWithConstants)
+{
+  TiXmlDocument xml, desired;
+  xml.Parse("<x> \
+<const name='aaa' value='4.5'/> \
+<const name='bbb' value='7.3'/> \
+<const name='ccc' value='2.0'/>  \
+<y><z foo='(aaa*bbb)/ccc' /></y></x>");
+  desired.Parse("<x><y><z foo='16.425' /></y></x>");
+  EXPECT_TRUE(urdf::normalizeXml(xml.RootElement()));
+  EXPECT_TRUE(xmlMatches(xml.RootElement(), desired.RootElement()));
+}
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);

Modified: pkg/trunk/util/math_utils/include/math_utils/MathExpression.h
===================================================================
--- pkg/trunk/util/math_utils/include/math_utils/MathExpression.h	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/util/math_utils/include/math_utils/MathExpression.h	2008-08-19 21:52:35 UTC (rev 3273)
@@ -53,7 +53,11 @@
  * 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. */
+
+ * 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);
 

Modified: pkg/trunk/util/math_utils/src/MathExpression.cpp
===================================================================
--- pkg/trunk/util/math_utils/src/MathExpression.cpp	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/util/math_utils/src/MathExpression.cpp	2008-08-19 21:52:35 UTC (rev 3273)
@@ -111,18 +111,24 @@
     {
       bool variable = false;
       for (unsigned int i = 0 ; i < exp.size() ; ++i)
+      {
         if (!((exp[i] <= '9' && exp[i] >= '0') || exp[i] == '+' || exp[i] == '-' || exp[i] == '.'))
         {
           variable = true;
           break;
         }
+      }
+
       if (variable)
       {
         if (var)
           return var(data, exp);
+        return NAN;
       }
       else
+      {
         return atof(exp.c_str());
+      }
     }
   }
   else
@@ -130,18 +136,28 @@
     unsigned int pos = ops[0];
     std::string exp1 = exp.substr(0, pos);
     std::string exp2 = exp.substr(pos + 1);
+    double val1, val2;
+    val1 = EvaluateMathExpression(exp1, var, data);
+    val2 = EvaluateMathExpression(exp2, var, data);
+
+    // Hack: handles unary minus
+    if (exp1.size() == 0)
+      val1 = 0.0;
+
     switch (exp[pos])
     {
     case '+':
-      return EvaluateMathExpression(exp1, var, data) + EvaluateMathExpression(exp2, var, data);
+      return val1 + val2;
     case '-':
-      return EvaluateMathExpression(exp1, var, data) - EvaluateMathExpression(exp2, var, data);
+      return val1 - val2;
     case '*':
-      return EvaluateMathExpression(exp1, var, data) * EvaluateMathExpression(exp2, var, data);
+      return val1 * val2;
     case '/':
-      return EvaluateMathExpression(exp1, var, data) / EvaluateMathExpression(exp2, var, data);
+      return val1 / val2;
+    default:
+      return NAN;
     }
   }
 
-  return 0.0;
+  return NAN;
 }

Modified: pkg/trunk/util/string_utils/include/string_utils/string_utils.h
===================================================================
--- pkg/trunk/util/string_utils/include/string_utils/string_utils.h	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/util/string_utils/include/string_utils/string_utils.h	2008-08-19 21:52:35 UTC (rev 3273)
@@ -7,19 +7,22 @@
 
 namespace string_utils
 {
-    void split(const std::string &str, 
-	       std::vector<std::string> &token_vec, 
-	       const std::string &delim);
-    
-    std::string trim(const std::string &str);
-    
-    template<typename T>
-    static inline std::string convert2str(const T &value)
-    {
-	std::stringstream ss;
-	ss << value;
-	return ss.str();
-    }
+void split(const std::string &str,
+           std::vector<std::string> &token_vec,
+           const std::string &delim);
+
+// Default version, which splits on whitespace.
+void split(const std::string &str, std::vector<std::string> &tokens);
+
+std::string trim(const std::string &str);
+
+template<typename T>
+static inline std::string convert2str(const T &value)
+{
+  std::stringstream ss;
+  ss << value;
+  return ss.str();
 }
+}
 
 #endif

Modified: pkg/trunk/util/string_utils/src/string_utils.cpp
===================================================================
--- pkg/trunk/util/string_utils/src/string_utils.cpp	2008-08-19 21:40:21 UTC (rev 3272)
+++ pkg/trunk/util/string_utils/src/string_utils.cpp	2008-08-19 21:52:35 UTC (rev 3273)
@@ -12,6 +12,29 @@
   t.push_back(s.substr(start));
 }
 
+void string_utils::split(const std::string &str, std::vector<std::string> &tokens)
+{
+  std::size_t start = 0, i;
+
+  while (true)
+  {
+    while (start < str.size() && isspace(str[start]))
+      ++start;
+    if (start == str.size())
+      return;
+
+    i = start;
+    while (i < str.size() && !isspace(str[i]))
+      ++i;
+
+    tokens.push_back(str.substr(start, i - start));
+    if (i == str.size())
+      return;
+
+    start = i;
+  }
+}
+
 std::string string_utils::trim(const std::string &str)
 {
     std::string res = str;
@@ -27,5 +50,5 @@
 			    res[0] == '\n' || 
 			    res[0] == '\r'))
 	res.erase(0, 1); 
-    return res;    
+    return res;
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.