Acrobot swing up example with direct collocation trajectory generation broken

[tarquasso]

All acrobot examples as of May 4th worked fine. Now I upgraded to a commit as of this morning and rebuild everything. While I can get the basic examples with trajectory optimization to run and visualize, I can not get the trajectory optimization to work anymore: acrobot_run_swing_up_traj_optimization.cc The default values are kTrajectoryTimeLowerBound=2 and kTrajectoryTimeUpperBound=10, those previously generated a prolonged swing up with a couple back and fourths and overshoots before it would go upright: Video of previously observed behaviour. Now, it compiles and runs with ipopt, but it only generates a trajectory that runs up to time t = kTrajectoryTimeUpperBound and it does not visualize the trajectory to drake_visualizer. It only shows the acrobot at the downhanging configuration, but it does not visualize a swing up motion. Also, if I adjust the lower and upper bounds for the direct collocation, it results in errors for parameter ranges that previously found feasible trajectories. kTrajectoryTimeLowerBound=1 and kTrajectoryTimeUpperBound=5 would previously find a solution that requires only a single back and forth before upright, Video of faster swing up. Now, running it with those parameters now results in an error. Naveen also confirmed that it is not working for both Ipopt and Snopt. I am running git bisect to find the commit that broke this feature, will report soon the result.

Last Good Commit

Update to address UBsan flag in implicit Euler integrator. (#6591)

Culprit Commit by Evan Drumwright edrumwri@hotmail.com on Wed Jul 19 13:19:57 2017 -0500

Fixes issue with integrators limiting rate of simulator time advancement even when no continuous state is present. (#6593)

Related to #6619, proper unit tests are missing for all Acrobot examples. This swing up example was introduced here: #5093

[tarquasso]

Undoing Evan's commit by commenting out the following allows the simulation to work again:

--- a/drake/systems/analysis/integrator_base.h
+++ b/drake/systems/analysis/integrator_base.h
@@ -1645,11 +1645,11 @@ typename IntegratorBase<T>::StepResult IntegratorBase<T>::IntegrateAtMost(

   // If there is no continuous state, there will be no need to limit the
   // integration step size.
-  if (get_context().get_continuous_state()->size() == 0) {
-    Context<T>* context = get_mutable_context();
-    context->set_time(context->get_time() + dt);
-    return candidate_result;
-  }
+//  if (get_context().get_continuous_state()->size() == 0) {
+//    Context<T>* context = get_mutable_context();
+//    context->set_time(context->get_time() + dt);
+//    return candidate_result;
+//  }

But that probably creates again the problem that was supposed to be solved by that original commit...

[RussTedrake]

I am willing to take a cleanup pass through the acrobot (and pendulum/cart-pole/quadrotor) example that will also add proper unit tests. @hongkai-dai -- after you fix the obvious defects here, feel free to assign this and #6842 to me. @tarquasso -- you need direct collocation to work well, but do you actually care about the acrobot example in particular?

[tarquasso]

@RussTedrake: Yes, having direct collocation working well would be great. Alejandro and I are building a "soft contact" demo using a double pendulum (and later a triple pendulum model), that plans trajectories to swing up with a static obstacle in the way that can be leveraged to still reach a final goal state or hit an object at that final state. For this purpose, we are using the AcrobotPlant as a template to generate a fully actuated double_pendulum (not using sfd) and then add a soft contact to the plant. Later, we replace it with a more complex dynamic model (cosserat rod theory).