Merge pull request RobotLocomotion#1777 from liangfok/feature/fixRigi…

…dBodySDFParser Feature/fix rigid body sdf parser
YinjieLin · Mar 1, 2016 · 120ae97 · 120ae97
2 parents 0675426 + 3395ddd
commit 120ae97
Showing 1 changed file with 12 additions and 7 deletions.
diff --git a/drake/systems/plants/RigidBodyTreeSDF.cpp b/drake/systems/plants/RigidBodyTreeSDF.cpp
@@ -317,15 +317,20 @@ void parseSDFJoint(RigidBodyTree * model, std::string model_name, XMLElement* no
       if (axis_node) {
         setSDFDynamics(model, axis_node, fjoint);
         setSDFLimits(axis_node, fjoint);
+
+        double effort_limit = std::numeric_limits<double>::infinity();
+
+        XMLElement* limit_node = axis_node->FirstChildElement("limit");
+        if (limit_node) parseScalarValue(limit_node,"effort",effort_limit);
+
+        if (effort_limit != 0.0) {
+          RigidBodyActuator actuator(name,child,1.0,-effort_limit,effort_limit);
+          model->actuators.push_back(actuator);
+        }
       }
+
       joint = fjoint;
-      double effort_limit = std::numeric_limits<double>::infinity();
-      XMLElement* limit_node = axis_node->FirstChildElement("limit");
-      if (limit_node) parseScalarValue(limit_node,"effort",effort_limit);
-      if (effort_limit != 0.0) {
-        RigidBodyActuator actuator(name,child,1.0,-effort_limit,effort_limit);
-        model->actuators.push_back(actuator);
-      }
+
     } else if (type.compare("fixed") == 0) {
       joint = new FixedJoint(name, transform_to_parent_body);
     } else if (type.compare("prismatic") == 0) {