[WIP] Added Ackermann & van den Bogert 2010 example.

examples/ackermann2010/ackermann2010.py

@@ -0,0 +1,117 @@
+"""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 import Problem, parse_free
+from opty.utils import f_minus_ma
+
+speed = 1.3250  # m/s
+num_nodes = 60
+h = sm.symbols('h', real=True, positive=True)
+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]
+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 = {
+    h: (0.001, 0.1),
+    qax: (0.0, 10.0),
+    qay: (0.5, 1.5),
+    qa: (-np.pi/3.0, np.pi/3.0),  # +/- 60 deg
+    uax: (0.0, 10.0),
+    uay: (-10.0, 10.0),
+}
+bounds.update({k: (-np.pi/4.0, 3.0*np.pi/4.0) for k in [qb, qe]})  # hip
+bounds.update({k: (-3.0/np.pi/4.0, 0.0) for k in [qc, qf]})  # knee
+bounds.update({k: (-np.pi/4.0, np.pi/4.0) for k in [qd, qg]})  # foot
+bounds.update({k: (-6.0, 6.0) for k in [ua, ub, uc, ud, ue, uf, ug]})  # ~200 deg/s
+bounds.update({k: (-1000.0, 1000.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 = (
+    qax(0*h),
+    qay(0*h) - 0.95,
+    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,
+)
+
+
+# Specify the objective function and it's gradient.
+def obj(free):
+    """Minimize the sum of the squares of the control torque."""
+    T, h = free[num_states*num_nodes:-1], free[-1]
+    return h*np.sum(T**2)
+
+
+def obj_grad(free):
+    T, h = free[num_states*num_nodes:-1], free[-1]
+    grad = np.zeros_like(free)
+    grad[num_states*num_nodes:-1] = 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, h,
+               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)
+initial_guess = np.zeros(prob.num_free)
+
+# Find the optimal solution.
+solution, info = prob.solve(initial_guess)
+
+
+state_vals, _, _, h_val = parse_free(solution, num_states, len(specified) -
+                                     len(traj_map), num_nodes,
+                                     variable_duration=True)
+np.savez('solution', x=state_vals, h=h_val, n=num_nodes)

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