diff --git a/examples/ackermann2010/ackermann2010.py b/examples/ackermann2010/ackermann2010.py new file mode 100644 index 00000000..fbed45c3 --- /dev/null +++ b/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) diff --git a/examples/ackermann2010/example_constants.yml b/examples/ackermann2010/example_constants.yml new file mode 100644 index 00000000..61eb224e --- /dev/null +++ b/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