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