resolves RobotLocomotion#1037 . this one was on me

aminhashemi · Apr 26, 2015 · 3878e07 · 3878e07
1 parent 67e89cd
commit 3878e07
Showing 1 changed file with 1 addition and 2 deletions.
diff --git a/systems/plants/RigidBodyManipulator.cpp b/systems/plants/RigidBodyManipulator.cpp
@@ -367,9 +367,8 @@ void RigidBodyManipulator::compile(void)
     if (bodies[i]->hasParent()) {
       auto iter = find(bodies.begin()+i+1,bodies.end(),bodies[i]->parent);
       if (iter!=bodies.end()) {
+	bodies.erase(iter);
       	bodies.insert(bodies.begin()+i,bodies[i]->parent);
-	auto next_iter = iter+1;
-	if (next_iter != bodies.end()) bodies.erase(next_iter);
       	i--;
       }
     }