From 40f74fa2980e35f3d0f898515e57a06def78b1d7 Mon Sep 17 00:00:00 2001
From: Russ Tedrake <russt@mit.edu>
Date: Tue, 24 Feb 2015 07:05:48 -0500
Subject: [PATCH] fix testConstraintConstructor and testApproximateIK

---
 .gitignore                                  | 11 ++++
 systems/plants/URDFRigidBodyManipulator.cpp | 64 +--------------------
 2 files changed, 13 insertions(+), 62 deletions(-)

diff --git a/.gitignore b/.gitignore
index ef85abae0dab..b6889e65f4b1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -127,3 +127,14 @@ examples/IRB140/urdf/meshes/link3.wrl
 examples/IRB140/urdf/meshes/link4.wrl
 examples/IRB140/urdf/meshes/link5.wrl
 examples/IRB140/urdf/meshes/link6.wrl
+examples/PR2/pr2_description/meshes/base_v0/base.wrl
+examples/PR2/pr2_description/meshes/base_v0/caster.wrl
+examples/PR2/pr2_description/meshes/base_v0/wheel.wrl
+examples/PR2/pr2_description/meshes/forearm_v0/wrist_roll.wrl
+examples/PR2/pr2_description/meshes/head_v0/head_pan.wrl
+examples/PR2/pr2_description/meshes/head_v0/head_tilt.wrl
+examples/PR2/pr2_description/meshes/shoulder_v0/upper_arm_roll.wrl
+examples/PR2/pr2_description/meshes/tilting_laser_v0/tilting_hokuyo.wrl
+examples/PR2/pr2_description/meshes/torso_v0/torso_lift.wrl
+examples/PR2/pr2_description/meshes/upper_arm_v0/forearm_roll.wrl
+results_testRunning.mat
diff --git a/systems/plants/URDFRigidBodyManipulator.cpp b/systems/plants/URDFRigidBodyManipulator.cpp
index ecee5025cd59..b5f39fc611b9 100644
--- a/systems/plants/URDFRigidBodyManipulator.cpp
+++ b/systems/plants/URDFRigidBodyManipulator.cpp
@@ -778,69 +778,9 @@ bool URDFRigidBodyManipulator::addRobotFromURDFString(const string &xml_string,
 
   }
 
-  compile();
-
-  // parse extra tags supported by drake
-
-  TiXmlDocument xml_doc;
-  xml_doc.Parse(xml_string.c_str());  // a little inefficient to parse a second time, but ok for now
-  // eventually, we'll probably just crop out the ros urdf parser completely.
-
-  TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
-
-  // parse transmission elements
-  for (TiXmlElement* transmission_xml = robot_xml->FirstChildElement("transmission"); transmission_xml; transmission_xml = transmission_xml->NextSiblingElement("transmission"))
-  {
-    TiXmlElement* node = transmission_xml->FirstChildElement("joint");
-    if (!node) continue;
-
-    int _dofnum;
-    map<string, int>::const_iterator dn=findWithSuffix(dofname_to_dofnum,node->Attribute("name"));
-    if (dn == dofname_to_dofnum.end()) ROS_ERROR("can't find joint %s for transmission element.  this shouldn't happen");
-    _dofnum = dn->second;
-//    cout << "adding actuator to joint " << node->Attribute("name") << " (dof: " << _dofnum << ")" << endl;
+  cout << "addRobotFromURDFString: got here" << endl;
 
-    node = transmission_xml->FirstChildElement("mechanicalReduction");
-    double gain = 1.0;
-    if (node) sscanf(node->Value(),"%lf",&gain);
-
-    VectorXd B_col = VectorXd::Zero(num_velocities);
-    B_col(_dofnum) = gain;
-
-    B.conservativeResize(num_velocities, B.cols()+1);
-    B.rightCols(1) = B_col;
-  }
-
-  for (TiXmlElement* loop_xml = robot_xml->FirstChildElement("loop_joint"); loop_xml; loop_xml = loop_xml->NextSiblingElement("loop_joint"))
-  { // note: pushing this in without all of the surrounding drakeFunction logic just to get things moving.  this one needs to be fast.
-    urdf::Vector3 pt;
-    TiXmlElement* node = loop_xml->FirstChildElement("link1");
-    int bodyA=-1,bodyB=-1;
-
-    string linkname = node->Attribute("link");
-    for (int i=0; i<num_bodies; i++)
-      if (linkname==bodies[i]->linkname) {
-        bodyA=i;
-        break;
-      }
-    if (bodyA<0) ROS_ERROR("couldn't find link %s referenced in loop joint",linkname.c_str());
-    pt.init(node->Attribute("xyz"));
-    Vector3d ptA;  ptA << pt.x, pt.y, pt.z;
-
-    node = loop_xml->FirstChildElement("link2");
-    linkname = node->Attribute("link");
-    for (int i=0; i<num_bodies; i++)
-      if (linkname==bodies[i]->linkname) {
-        bodyB=i;
-        break;
-      }
-    if (bodyB<0) ROS_ERROR("couldn't find link %s referenced in loop joint",linkname.c_str());
-    pt.init(node->Attribute("xyz"));
-    Vector3d ptB;  ptB << pt.x, pt.y, pt.z;
-
-    RigidBodyLoop l(bodies[bodyA],ptA,bodies[bodyB],ptB);
-    loops.push_back(l);
-  }
+  compile();
 
   return true;
 }