diff --git a/examples/ackermann2010/ackermann2010.py b/examples/ackermann2010/ackermann2010.py index 9832aa41..be5eb41f 100644 --- a/examples/ackermann2010/ackermann2010.py +++ b/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,