Added the non-working ackermann example.

examples/ackermann2010/ackermann2010.py

@@ -0,0 +1,91 @@
+"""This example replicates some of the work presented in Ackermann and van
+den Bogert 2010."""
+
+import numpy as np
+from pygait2d import derive, simulate
+from pygait2d.segment import time_symbol
+from opty.direct_collocation import Problem
+from opty.utils import f_minus_ma
+
+speed = 1.3250
+speed = 0.0
+duration = 0.5
+num_nodes = 60
+
+interval_value = duration / (num_nodes - 1)
+
+symbolics = derive.derive_equations_of_motion()
+
+mass_matrix = symbolics[0]
+forcing_vector = symbolics[1]
+constants = symbolics[3]
+coordinates = symbolics[4]
+speeds = symbolics[5]
+states = coordinates + speeds
+specified = symbolics[6]
+
+num_states = len(states)
+
+eom = f_minus_ma(mass_matrix, forcing_vector, coordinates + speeds)
+
+# right: b, c, d
+# left: e, f, g
+qax, qay, qa, qb, qc, qd, qe, qf, qg = coordinates
+uax, uay, ua, ub, uc, ud, ue, uf, ug = speeds
+Fax, Fay, Ta, Tb, Tc, Td, Te, Tf, Tg = specified
+
+par_map = simulate.load_constants(constants, 'example_constants.yml')
+
+# Hand of god is nothing.
+traj_map = {Fax: np.zeros(num_nodes),
+            Fay: np.zeros(num_nodes),
+            Ta: np.zeros(num_nodes)}
+
+bounds = {qax: (-3.0, 3.0), qay: (0.5, 1.5), qa: (-np.pi / 3.0, np.pi / 3.0)}
+bounds.update({k: (-np.pi, np.pi) for k in [qb, qc, qd, qe, qf, qg]})
+bounds.update({k: (-10.0, 10.0) for k in [uax, uay]})
+bounds.update({k: (-20.0, 20.0) for k in [ua, ub, uc, ud, ue, uf, ug]})
+bounds.update({k: (-300.0, 300.0) for k in [Tb, Tc, Td, Te, Tf, Tg]})
+
+# Specify the symbolic instance constraints, i.e. initial and end
+# conditions.
+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))
+
+
+# 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)
+
+
+def obj_grad(free):
+    grad = np.zeros_like(free)
+    grad[num_states * num_nodes:] = 2.0 * interval_value * free[num_states *
+                                                                num_nodes:]
+    return grad
+
+# Create an optimization problem.
+prob = Problem(obj, obj_grad, eom, states, num_nodes, interval_value,
+               known_parameter_map=par_map,
+               known_trajectory_map=traj_map,
+               instance_constraints=instance_constraints,
+               bounds=bounds,
+               time_symbol=time_symbol,
+               tmp_dir='ufunc')
+
+# Use a random positive initial guess.
+initial_guess = prob.lower_bound + (prob.upper_bound - prob.lower_bound) * np.random.randn(prob.num_free)
+
+# Find the optimal solution.
+solution, info = prob.solve(initial_guess)

examples/ackermann2010/example_constants.yml

@@ -0,0 +1,58 @@
+# These are some example model parameters. The symbols correspond to the
+# symbols used in the symbolic equations of motion.
+#
+# The body segment parameters are from Winter's book for 75 kg body mass and
+# 1.8 m body height.
+#
+# trunk
+ma: 50.85  # mass [kg]
+ia: 3.1777 # moment of inertia about mass center wrt to the trunk reference frame [kg*m^2]
+xa: 0.0    # local x location of mass center wrt to the hip joint [m]
+ya: 0.3155 # local y location of mass center wrt to the hip joint [m]
+# rthigh
+mb: 7.5    # mass [kg]
+ib: 0.1522 # moment of inertia about mass center wrt to rthigh reference frame [kg*m^2]
+xb: 0.0    # local x location of mass center wrt to the hip joint [m]
+yb: -0.191 # local y location of mass center wrt to the hip joint [m]
+lb: 0.4410 # joint to joint segment length [m]
+# rshank:
+mc: 3.4875  # mass [kg]
+ic: 0.0624  # moment of inertia about mass center wrt to rshank reference frame [kg*m^2]
+xc: 0.0     # x location of mass center wrt to the knee joint [m]
+yc: -0.1917 # y location of mass center wrt to the knee joint [m]
+lc: 0.4428  # joint to joint segment length [m]
+# rfoot
+md: 1.0875  # mass [kg]
+id: 0.0184  # moment of inertia about mass center wrt to rfoot reference frame [kg*m^2]
+xd: 0.0768  # local x location of mass center wrt to the ankle joint [m]
+yd: -0.0351 # local y location of mass center wrt to the ankle joint [m]
+hxd: -0.06  # local x location of heel wrt to the ankle joint [m]
+txd: 0.15   # local x location of toe wrt to the ankle joint [m]
+fyd: -0.07  # local y location of heel and toe relative to ankle joint [m]
+# lthigh
+me: 7.5    # mass [kg]
+ie: 0.1522 # moment of inertia about mass center wrt to the lthigh reference frame[kg*m^2]
+xe: 0.0    # local x location of mass center wrt to the hip joint [m]
+ye: -0.191 # local y location of mass center wrt to the hip joint [m]
+le: 0.4410 # segment length [m]
+# lshank
+mf: 3.4875  # mass [kg]
+if: 0.0624  # moment of inertia about mass center wrt to the lshank reference frame [kg*m^2]
+xf: 0.0     # local x location of mass center wrt to the knee joint [m]
+yf: -0.1917 # local y location of mass center wrt to the knee joint [m]
+lf: 0.4428  # segment length [m]
+# lfoot
+mg: 1.0875  # mass [kg]
+ig: 0.0184  # moment of inertia about mass center wrt to the lfoot reference frame [kg*m^2]
+xg: 0.0768  # local x location of mass center wrt to the ankle joint [m]
+yg: -0.0351 # local y location of mass center wrt to the ankle joint [m]
+hxg: -0.06  # local x location of heel wrt to the ankle joint [m]
+txg: 0.15   # local x location of toe wrt to the ankle joint [m]
+fyg: -0.07  # local y location of heel and toe relative to ankle joint [m]
+# contact
+kc: 5.0e+7  # ground contact stiffness, N/m^3
+cc: 0.85 # ground contact damping, s/m
+mu: 1.0  # friction coefficient
+vs: 0.01 # velocity constant (m/s), for |ve| : vc -> |fx| : 0.4621*c*fy
+# other
+g: 9.81 # acceleration due to gravity, m/s^2