Change to variable time step.

examples/ackermann2010/ackermann2010.py

@@ -1,23 +1,23 @@
 """This example replicates some of the work presented in Ackermann and van
 den Bogert 2010."""
 
+import sympy as sm
 import numpy as np
 from pygait2d import derive, simulate
 from pygait2d.segment import time_symbol
-from opty.direct_collocation import Problem
+from opty import Problem
 from opty.utils import f_minus_ma
 
-speed = 1.3250
-speed = 0.0
-duration = 0.5
+speed = 1.3250  # m/s
 num_nodes = 60
-
-interval_value = duration / (num_nodes - 1)
+h = sm.symbols('h')
+duration = (num_nodes - 1)*h
 
 symbolics = derive.derive_equations_of_motion()
 
 mass_matrix = symbolics[0]
 forcing_vector = symbolics[1]
+kane = symbolics[2]
 constants = symbolics[3]
 coordinates = symbolics[4]
 speeds = symbolics[5]
@@ -52,31 +52,37 @@
 uneval_states = [s.__class__ for s in states]
 (qax, qay, qa, qb, qc, qd, qe, qf, qg, uax, uay, ua, ub, uc, ud, ue, uf, ug) = uneval_states
 
-instance_constraints = (qb(0.0) - qe(duration),
-                        qc(0.0) - qf(duration),
-                        qd(0.0) - qg(duration),
-                        ub(0.0) - ue(duration),
-                        uc(0.0) - uf(duration),
-                        ud(0.0) - ug(duration),
-                        qax(duration) - speed * duration,
-                        qax(0.0))
+instance_constraints = (qb(0*h) - qe(duration),
+                        qc(0*h) - qf(duration),
+                        qd(0*h) - qg(duration),
+                        ub(0*h) - ue(duration),
+                        uc(0*h) - uf(duration),
+                        ud(0*h) - ug(duration),
+                        # TODO : need support for including h outside of a
+                        # Function argument.
+                        #qax(duration) - speed * duration,
+                        uax(0*h) - speed,
+                        uax(duration) - speed,
+                        qax(0*h))
 
 
 # Specify the objective function and it's gradient.
 def obj(free):
     """Minimize the sum of the squares of the control torque."""
-    T = free[num_states * num_nodes:]
-    return np.sum(T**2)
+    T, h = free[num_states * num_nodes:], free[-1]
+    return h*np.sum(T**2)
 
 
 def obj_grad(free):
+    T, h = free[num_states * num_nodes:], free[-1]
     grad = np.zeros_like(free)
-    grad[num_states * num_nodes:] = 2.0 * interval_value * free[num_states *
-                                                                num_nodes:]
+    grad[num_states * num_nodes:] = 2.0*h*T
+    grad[-1] = np.sum(T**2)
     return grad
 
+
 # Create an optimization problem.
-prob = Problem(obj, obj_grad, eom, states, num_nodes, interval_value,
+prob = Problem(obj, obj_grad, eom, states, num_nodes, h,
                known_parameter_map=par_map,
                known_trajectory_map=traj_map,
                instance_constraints=instance_constraints,