From 739df4780c45356f4cecebe66b0b4d674ee914c4 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 9 Jun 2024 09:24:06 +0200 Subject: [PATCH 01/40] Started drafting a cycling leg optimization example. --- .../plot_one_legged_time_trial.py | 84 +++++++++++++++++++ 1 file changed, 84 insertions(+) create mode 100644 examples-gallery/plot_one_legged_time_trial.py diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py new file mode 100644 index 0000000..448c7d1 --- /dev/null +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -0,0 +1,84 @@ +import sympy as sm +import sympy.physics.mechanics as me + +# bike frame, crank, pedal/foot, lower leg, upper leg +N, A, B, C, D = sm.symbols('N, A, B, C, D', cls=me.ReferenceFrame) + +q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4') +u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4') +q = sm.Matrix([q1, q2, q3, q4]) +u = sm.Matrix([u1, u2, u3, u4]) + +ls, lc, lf, ll, lu = sm.symbols('ls, lc, lf, ll, lu', real=True, positive=True) +lam, g = sm.symbols('lambda, g') +mA, mB, mC, mD = sm.symbols('mA, mB, mC, mD') +IAzz, IBzz, ICzz, IDzz = sm.symbols('IAzz, IBzz, ICzz, IDzz') + +A.orient_axis(N, N.z, q1) # crank angle +B.orient_axis(A, A.z, q2) # pedal/foot angle +C.orient_axis(B, B.z, q3) # ankle angle +D.orient_axis(C, C.z, q4) # knee angle + +A.set_vel(N, u1*N.z) +B.set_vel(A, u2*N.z) +C.set_vel(B, u3*N.z) +D.set_vel(C, u4*N.z) + +P1, P2, P3, P4, P5, P6 = sm.symbols('P1, P2, P3, P4, P5, P6', cls=me.Point) +Ao, Bo, Co, Do = sm.symbols('Ao, Bo, Co, Do', cls=me.Point) + +Ao.set_pos_from(P1, 0*A.x) +P2.set_pos_from(P1, lc*A.x) # pedal center +Bo.set_pos_from(P2, lf/2*B.x) # foot mass center +P3.set_pos_from(P2, lf*B.x) # ankle +Co.set_pos_from(P3, ll/2*C.x) # lower leg mass center +P4.set_pos_from(P3, ll*C.x) # knee +Do.set_pos_from(P4, lu/2*C.x) # upper leg mass center +P5.set_pos_from(P4, lu*D.x) # hip +P6.set_pos_from(P1, ls*sm.cos(lam)*N.x + ls*sm.sin(lam)*N.y) # seat + +P1.set_vel(N, 0) +P6.set_vel(N, 0) +Ao.v2pt_theory(P1, N, A) +P2.v2pt_theory(P1, N, A) +Bo.v2pt_theory(P2, N, B) +P3.v2pt_theory(P2, N, B) +Co.v2pt_theory(P3, N, C) +P4.v2pt_theory(P3, N, C) +Do.v2pt_theory(P4, N, D) +P5.v2pt_theory(P4, N, D) + +kindiff = sm.Matrix([ui - qi.diff() for ui, qi in zip(u, q)]) + +holonomic = (P5.pos_from(P1) - P6.pos_from(P1)).to_matrix(N)[:2] + +IA = me.Inertia.from_inertia_scalars(Ao, A, 0, 0, IAzz) +IB = me.Inertia.from_inertia_scalars(Bo, B, 0, 0, IBzz) +IC = me.Inertia.from_inertia_scalars(Co, C, 0, 0, ICzz) +ID = me.Inertia.from_inertia_scalars(Do, D, 0, 0, IDzz) + +crank = me.RigidBody('crank', masscenter=Ao, frame=A, mass=mA, inertia=IA) +foot = me.RigidBody('foot', masscenter=Bo, frame=B, mass=mB, inertia=IB) +lower_leg = me.RigidBody('lower leg', masscenter=Co, frame=C, mass=mC, + inertia=IC) +upper_leg = me.RigidBody('upper leg', masscenter=Do, frame=D, mass=mD, + inertia=ID) + +gravB = me.Force(foot, -mB*g*N.y) +gravC = me.Force(foot, -mC*g*N.y) +gravD = me.Force(foot, -mD*g*N.y) + +load = me.Torque(crank, TA) + +# add inertia due to bike and wheels to crank +# add resitance torque due to air drag and rolling resitance to crank +# omega = gear_ratio*ul +# T = TA/gear_ratio +# (2*J + m*r**2)*omega' = -sign(omega*r)*1/2*rho*Cd*(omega*r)**2 - Cr*m*g + T +# w*r = v -> w'*r = v' -> w' = v'/r + +# four muscles +knee_top_pathway = +knee_bot_pathway = me.LinearPathway(butt, thigh) +ankle_top +ankle_bot_pathway = me.LinearPathway(calf, heel) From dcaf3ab877a8237d433991100331125034d7659a Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 9 Jun 2024 20:10:29 +0200 Subject: [PATCH 02/40] Equations of motion work, will have to check if they are correct. --- .../plot_one_legged_time_trial.py | 211 +++++++++++++++--- 1 file changed, 185 insertions(+), 26 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 448c7d1..19ec6bc 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -1,5 +1,6 @@ import sympy as sm import sympy.physics.mechanics as me +import sympy.physics.biomechanics as bm # bike frame, crank, pedal/foot, lower leg, upper leg N, A, B, C, D = sm.symbols('N, A, B, C, D', cls=me.ReferenceFrame) @@ -10,37 +11,48 @@ u = sm.Matrix([u1, u2, u3, u4]) ls, lc, lf, ll, lu = sm.symbols('ls, lc, lf, ll, lu', real=True, positive=True) -lam, g = sm.symbols('lambda, g') +lam, g, rk = sm.symbols('lambda, g, rk', real=True) mA, mB, mC, mD = sm.symbols('mA, mB, mC, mD') IAzz, IBzz, ICzz, IDzz = sm.symbols('IAzz, IBzz, ICzz, IDzz') +J, m, rw, G, Cr, CD, rho, Ar = sm.symbols('J, m, rw, G, Cr, CD, rho, Ar') A.orient_axis(N, N.z, q1) # crank angle B.orient_axis(A, A.z, q2) # pedal/foot angle C.orient_axis(B, B.z, q3) # ankle angle D.orient_axis(C, C.z, q4) # knee angle -A.set_vel(N, u1*N.z) -B.set_vel(A, u2*N.z) -C.set_vel(B, u3*N.z) -D.set_vel(C, u4*N.z) - -P1, P2, P3, P4, P5, P6 = sm.symbols('P1, P2, P3, P4, P5, P6', cls=me.Point) +A.set_ang_vel(N, u1*N.z) +B.set_ang_vel(A, u2*A.z) +C.set_ang_vel(B, u3*B.z) +D.set_ang_vel(C, u4*C.z) + +# P1 : crank center +# P2 : pedal center +# P3 : ankle +# P4 : knee +# P5 : hip +# P6 : seat center +# P7 : heel +P1, P2, P3, P4, P5, P6, P7 = sm.symbols('P1, P2, P3, P4, P5, P6, P7', + cls=me.Point) Ao, Bo, Co, Do = sm.symbols('Ao, Bo, Co, Do', cls=me.Point) -Ao.set_pos_from(P1, 0*A.x) -P2.set_pos_from(P1, lc*A.x) # pedal center -Bo.set_pos_from(P2, lf/2*B.x) # foot mass center -P3.set_pos_from(P2, lf*B.x) # ankle -Co.set_pos_from(P3, ll/2*C.x) # lower leg mass center -P4.set_pos_from(P3, ll*C.x) # knee -Do.set_pos_from(P4, lu/2*C.x) # upper leg mass center -P5.set_pos_from(P4, lu*D.x) # hip -P6.set_pos_from(P1, ls*sm.cos(lam)*N.x + ls*sm.sin(lam)*N.y) # seat +Ao.set_pos(P1, 0*A.x) +P2.set_pos(P1, lc*A.x) # pedal center +Bo.set_pos(P2, lf/2*B.x) # foot mass center +P3.set_pos(P2, lf*B.x) # ankle +P7.set_pos(P2, 3*lf/2*B.x) # heel +Co.set_pos(P3, ll/2*C.x) # lower leg mass center +P4.set_pos(P3, ll*C.x) # knee +Do.set_pos(P4, lu/2*D.x) # upper leg mass center +P5.set_pos(P4, lu*D.x) # hip +P6.set_pos(P1, -ls*sm.sin(lam)*N.x + ls*sm.cos(lam)*N.y) # seat P1.set_vel(N, 0) P6.set_vel(N, 0) Ao.v2pt_theory(P1, N, A) P2.v2pt_theory(P1, N, A) +P7.v2pt_theory(P2, N, B) Bo.v2pt_theory(P2, N, B) P3.v2pt_theory(P2, N, B) Co.v2pt_theory(P3, N, C) @@ -50,9 +62,9 @@ kindiff = sm.Matrix([ui - qi.diff() for ui, qi in zip(u, q)]) -holonomic = (P5.pos_from(P1) - P6.pos_from(P1)).to_matrix(N)[:2] +holonomic = (P5.pos_from(P1) - P6.pos_from(P1)).to_matrix(N)[:2, :] -IA = me.Inertia.from_inertia_scalars(Ao, A, 0, 0, IAzz) +IA = me.Inertia.from_inertia_scalars(Ao, A, 0, 0, IAzz + (J + m*rw**2)*G) IB = me.Inertia.from_inertia_scalars(Bo, B, 0, 0, IBzz) IC = me.Inertia.from_inertia_scalars(Co, C, 0, 0, ICzz) ID = me.Inertia.from_inertia_scalars(Do, D, 0, 0, IDzz) @@ -64,11 +76,14 @@ upper_leg = me.RigidBody('upper leg', masscenter=Do, frame=D, mass=mD, inertia=ID) -gravB = me.Force(foot, -mB*g*N.y) -gravC = me.Force(foot, -mC*g*N.y) -gravD = me.Force(foot, -mD*g*N.y) +gravB = me.Force(Bo, -mB*g*N.y) +gravC = me.Force(Co, -mC*g*N.y) +gravD = me.Force(Do, -mD*g*N.y) -load = me.Torque(crank, TA) +resistance = me.Torque( + crank, + (-Cr*m*g*rw - sm.sign(G*u1*rw)*CD*rho*Ar*G**2*u1**2*rw**3/2)*N.z +) # add inertia due to bike and wheels to crank # add resitance torque due to air drag and rolling resitance to crank @@ -77,8 +92,152 @@ # (2*J + m*r**2)*omega' = -sign(omega*r)*1/2*rho*Cd*(omega*r)**2 - Cr*m*g + T # w*r = v -> w'*r = v' -> w' = v'/r + +class ExtensorPathway(me.PathwayBase): + def __init__(self, origin, insertion, axis_point, axis, parent_axis, + child_axis, radius, coordinate): + """A custom pathway that wraps a circular arc around a pin joint. + This is intended to be used for extensor muscles. For example, a + triceps wrapping around the elbow joint to extend the upper arm at + the elbow. + Parameters + ========== + origin : Point + Muscle origin point fixed on the parent body (A). + insertion : Point + Muscle insertion point fixed on the child body (B). + axis_point : Point + Pin joint location fixed in both the parent and child. + axis : Vector + Pin joint rotation axis. + parent_axis : Vector + Axis fixed in the parent frame (A) that is directed from the pin + joint point to the muscle origin point. + child_axis : Vector + Axis fixed in the child frame (B) that is directed from the pin + joint point to the muscle insertion point. + radius : sympyfiable + Radius of the arc that the muscle wraps around. + coordinate : sympfiable function of time + Joint angle, zero when parent and child frames align. Positive + rotation about the pin joint axis, B with respect to A. + Notes + ===== + Only valid for coordinate >= 0. + """ + super().__init__(origin, insertion) + self.origin = origin + self.insertion = insertion + self.axis_point = axis_point + self.axis = axis.normalize() + self.parent_axis = parent_axis.normalize() + self.child_axis = child_axis.normalize() + self.radius = radius + self.coordinate = coordinate + self.origin_distance = axis_point.pos_from(origin).magnitude() + self.insertion_distance = axis_point.pos_from(insertion).magnitude() + self.origin_angle = sm.asin(self.radius/self.origin_distance) + self.insertion_angle = sm.asin(self.radius/self.insertion_distance) + + @property + def length(self): + """Length of the pathway. + Length of two fixed length line segments and a changing arc length + of a circle. + """ + angle = self.origin_angle + self.coordinate + self.insertion_angle + arc_length = self.radius*angle + origin_segment_length = self.origin_distance*sm.cos(self.origin_angle) + insertion_segment_length = self.insertion_distance*sm.cos( + self.insertion_angle) + return origin_segment_length + arc_length + insertion_segment_length + + @property + def extension_velocity(self): + """Extension velocity of the pathway. + Arc length of circle is the only thing that changes when the elbow + flexes and extends. + """ + return self.radius*self.coordinate.diff(me.dynamicsymbols._t) + + def to_loads(self, force_magnitude): + """Loads in the correct format to be supplied to `KanesMethod`. + Forces applied to origin, insertion, and P from the muscle wrapped + over circular arc of radius r. + """ + parent_tangency_point = me.Point('Aw') # fixed in parent + child_tangency_point = me.Point('Bw') # fixed in child + parent_tangency_point.set_pos( + self.axis_point, + -self.radius*sm.cos(self.origin_angle)*self.parent_axis.cross( + self.axis) + + self.radius*sm.sin(self.origin_angle)*self.parent_axis, + ) + child_tangency_point.set_pos( + self.axis_point, + self.radius*sm.cos(self.insertion_angle)*self.child_axis.cross( + self.axis) + + self.radius*sm.sin(self.insertion_angle)*self.child_axis), + parent_force_direction_vector = self.origin.pos_from( + parent_tangency_point) + child_force_direction_vector = self.insertion.pos_from( + child_tangency_point) + force_on_parent = (force_magnitude* + parent_force_direction_vector.normalize()) + force_on_child = (force_magnitude* + child_force_direction_vector.normalize()) + loads = [ + me.Force(self.origin, force_on_parent), + me.Force(self.axis_point, -(force_on_parent + force_on_child)), + me.Force(self.insertion, force_on_child), + ] + return loads + + # four muscles -knee_top_pathway = -knee_bot_pathway = me.LinearPathway(butt, thigh) -ankle_top -ankle_bot_pathway = me.LinearPathway(calf, heel) +knee_top_pathway = ExtensorPathway(P5, Co, P4, C.z, D.x, -C.x, rk, q4) +knee_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_top') +knee_top_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_top', + knee_top_pathway, + knee_top_act) +knee_bot_pathway = me.LinearPathway(P5, Co) +knee_bot_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_bot') +knee_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_bot', + knee_bot_pathway, + knee_bot_act) +ankle_top_pathway = me.LinearPathway(Co, P2) +ankle_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults('ankle_top') +ankle_top_mus = bm.MusculotendonDeGroote2016.with_defaults('ankle_top', + ankle_top_pathway, + ankle_top_act) +ankle_bot_pathway = me.LinearPathway(Co, P7) +ankle_bot_act = bm.FirstOrderActivationDeGroote2016.with_defaults('ankle_bot') +ankle_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('ankle_bot', + ankle_bot_pathway, + ankle_bot_act) + +loads = ( + knee_top_mus.to_loads() + + knee_bot_mus.to_loads() + + ankle_top_mus.to_loads() + + ankle_bot_mus.to_loads() + + [resistance, gravB, gravC, gravD] +) + +kane = me.KanesMethod( + N, + (q1, q2), + (u1, u2), + kd_eqs=( + u1 - q1.diff(), + u2 - q2.diff(), + u3 - q3.diff(), + u4 - q4.diff(), + ), + q_dependent=(q3, q4), + configuration_constraints=holonomic, + velocity_constraints=holonomic.diff(me.dynamicsymbols._t), + u_dependent=(u3, u4), +) +bodies = (crank, foot, lower_leg, upper_leg) +Fr, Frs = kane.kanes_equations(bodies, loads) From cdbe81961605620e5a605721b9e969c23b027170 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 9 Jun 2024 20:27:55 +0200 Subject: [PATCH 03/40] Added the full constrained equations of motion and some basic explanation. --- .../plot_one_legged_time_trial.py | 25 ++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 19ec6bc..6b3836d 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -1,3 +1,16 @@ +""" +Single human leg with four driving lumped muscles. The crank inertia and +resistance mimic the torque felt if accelerating the whole bicycle with rider +on flat ground. + +The goal will be travel a specific distance in the shortest amount of time +given that the leg muscles have to coordinate and can only be excited from 0 to +1. + +Second goal will then be to solve for crank length and seat height that gives +optimal performance. + +""" import sympy as sm import sympy.physics.mechanics as me import sympy.physics.biomechanics as bm @@ -46,7 +59,7 @@ P4.set_pos(P3, ll*C.x) # knee Do.set_pos(P4, lu/2*D.x) # upper leg mass center P5.set_pos(P4, lu*D.x) # hip -P6.set_pos(P1, -ls*sm.sin(lam)*N.x + ls*sm.cos(lam)*N.y) # seat +P6.set_pos(P1, -ls*sm.cos(lam)*N.x + ls*sm.sin(lam)*N.y) # seat P1.set_vel(N, 0) P6.set_vel(N, 0) @@ -80,6 +93,7 @@ gravC = me.Force(Co, -mC*g*N.y) gravD = me.Force(Do, -mD*g*N.y) +# TODO : Check why gear ratio is not applied to rolling resistance. resistance = me.Torque( crank, (-Cr*m*g*rw - sm.sign(G*u1*rw)*CD*rho*Ar*G**2*u1**2*rw**3/2)*N.z @@ -241,3 +255,12 @@ def to_loads(self, force_magnitude): ) bodies = (crank, foot, lower_leg, upper_leg) Fr, Frs = kane.kanes_equations(bodies, loads) + +muscle_diff_eq = sm.Matrix([ + knee_top_mus.x[0, 0].diff() - knee_top_mus.rhs()[0, 0], + knee_bot_mus.x[0, 0].diff() - knee_bot_mus.rhs()[0, 0], + ankle_top_mus.x[0, 0].diff() - ankle_top_mus.rhs()[0, 0], + ankle_bot_mus.x[0, 0].diff() - ankle_bot_mus.rhs()[0, 0], +]) + +eoms = kindiff.col_join(Fr + Frs).col_join(muscle_diff_eq).col_join(holonomic) From db649b94206a14737248a6f82ddbe5f5562c3021 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Wed, 12 Jun 2024 11:41:00 +0200 Subject: [PATCH 04/40] Added start of work for the OCP. --- .../plot_one_legged_time_trial.py | 87 ++++++++++++++++++- 1 file changed, 86 insertions(+), 1 deletion(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 6b3836d..3d593c2 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -11,9 +11,11 @@ optimal performance. """ +import numpy as np import sympy as sm import sympy.physics.mechanics as me import sympy.physics.biomechanics as bm +import opty # bike frame, crank, pedal/foot, lower leg, upper leg N, A, B, C, D = sm.symbols('N, A, B, C, D', cls=me.ReferenceFrame) @@ -263,4 +265,87 @@ def to_loads(self, force_magnitude): ankle_bot_mus.x[0, 0].diff() - ankle_bot_mus.rhs()[0, 0], ]) -eoms = kindiff.col_join(Fr + Frs).col_join(muscle_diff_eq).col_join(holonomic) +eom = kindiff.col_join(Fr + Frs).col_join(muscle_diff_eq).col_join(holonomic) + +state_vars = ( + q1, q2, q3, q4, u1, u2, u3, u4, + knee_top_mus.x[0, 0], + knee_bot_mus.x[0, 0], + ankle_top_mus.x[0, 0], + ankle_bot_mus.x[0, 0], +) + +num_nodes = 200 +h = sm.symbols('h', real=True, positive=True) + +# objective +# minimize h +# gradient of h wrt free variables: states, unknown_inputs, unknown_pars, h +# so then there would be all zeros and a single 1 in the gradient for the last +# entry + + +def obj(free): + return free[-1] + + +def gradient(free): + grad = np.zeros_like(free) + grad[-1] = 1.0 + return grad + + +# body segment inertia from https://nbviewer.org/github/pydy/pydy-tutorial-human-standing/blob/master/notebooks/n07_simulation.ipynb +par_vals = { + Ar: 0.55, # m^2, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike + CD: 1.15, # unitless, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike + Cr: 0.006, # unitless, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike + G: 2.0, + IAzz: 0.0, + IBzz: 0.01, # guess, TODO + ICzz: 0.101, # lower_leg_inertia [kg*m^2] + IDzz: 0.282, # upper_leg_inertia [kg*m^2], + J: 2*0.1524, # from Browser Jason's thesis (rear wheel times 2) + g: 9.81, + lam: np.deg2rad(75.0), + lc: 0.175, + lf: 0.2, # guess TODO + ll: 0.611, # lower_leg_length [m] + ls: 0.6, # guess TODO + lu: 0.424, # upper_leg_length [m], + m: 175.0, # kg + mA: 0.0, + mB: 1.0, # guess TODO + mC: 6.769, # lower_leg_mass [kg] + mD: 17.01, # upper_leg_mass [kg], + rho: 1.204, # kg/m^3 + rk: 0.04, # m + rw: 0.3, # m +} + +# TODO : Solve for dependent q's so that config is correct at start. +instance_constraints = ( + q1(0*h), # crank starts horizontal + q2(0*h), # foot/pedal aligned with crank + u1(0*h), # start stationary + u2(0*h), # start stationary + u3(0*h), # start stationary + u4(0*h), # start stationary +) + +# TODO bounds +# excitations should be bound 0 to 1 +# only allow forward motion + + +#problem = opty.Problem( + #obj, + #gradient, + #eom, + #state_vars, + #num_nodes, + #h, + #known_parameter_map=par_vals, + #instance_constraints=instance_constraints, + #bounds=bounds, +#) From d0309be3f6f399d0c0153a5fb81a2f881088e5e9 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Fri, 14 Jun 2024 23:35:47 +0200 Subject: [PATCH 05/40] One legged example runs, need to tune parameters and initial guess. --- .../plot_one_legged_time_trial.py | 121 +++++++++++------- 1 file changed, 77 insertions(+), 44 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 3d593c2..b1ac436 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -15,18 +15,20 @@ import sympy as sm import sympy.physics.mechanics as me import sympy.physics.biomechanics as bm -import opty +from opty.direct_collocation import Problem + +t = me.dynamicsymbols._t # bike frame, crank, pedal/foot, lower leg, upper leg N, A, B, C, D = sm.symbols('N, A, B, C, D', cls=me.ReferenceFrame) -q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4') -u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4') +q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4', real=True) +u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4', real=True) q = sm.Matrix([q1, q2, q3, q4]) u = sm.Matrix([u1, u2, u3, u4]) ls, lc, lf, ll, lu = sm.symbols('ls, lc, lf, ll, lu', real=True, positive=True) -lam, g, rk = sm.symbols('lambda, g, rk', real=True) +lam, g, rk = sm.symbols('lam, g, rk', real=True) mA, mB, mC, mD = sm.symbols('mA, mB, mC, mD') IAzz, IBzz, ICzz, IDzz = sm.symbols('IAzz, IBzz, ICzz, IDzz') J, m, rw, G, Cr, CD, rho, Ar = sm.symbols('J, m, rw, G, Cr, CD, rho, Ar') @@ -98,7 +100,9 @@ # TODO : Check why gear ratio is not applied to rolling resistance. resistance = me.Torque( crank, - (-Cr*m*g*rw - sm.sign(G*u1*rw)*CD*rho*Ar*G**2*u1**2*rw**3/2)*N.z + # remove sign...causes derivative in jacobian + #(-Cr*m*g*rw - sm.sign(G*u1*rw)*CD*rho*Ar*G**2*u1**2*rw**3/2)*N.z + (-Cr*m*g*rw - CD*rho*Ar*G**2*u1**2*rw**3/2)*N.z ) # add inertia due to bike and wheels to crank @@ -116,6 +120,7 @@ def __init__(self, origin, insertion, axis_point, axis, parent_axis, This is intended to be used for extensor muscles. For example, a triceps wrapping around the elbow joint to extend the upper arm at the elbow. + Parameters ========== origin : Point @@ -137,9 +142,11 @@ def __init__(self, origin, insertion, axis_point, axis, parent_axis, coordinate : sympfiable function of time Joint angle, zero when parent and child frames align. Positive rotation about the pin joint axis, B with respect to A. + Notes ===== Only valid for coordinate >= 0. + """ super().__init__(origin, insertion) self.origin = origin @@ -211,6 +218,7 @@ def to_loads(self, force_magnitude): # four muscles +# NOTE : pathway only valid for q4>=0 knee_top_pathway = ExtensorPathway(P5, Co, P4, C.z, D.x, -C.x, rk, q4) knee_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_top') knee_top_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_top', @@ -240,43 +248,43 @@ def to_loads(self, force_magnitude): [resistance, gravB, gravC, gravD] ) +qd_repl = {q1.diff(): u1, q2.diff(): u2, q3.diff(): u3, q4.diff(): u4} + kane = me.KanesMethod( N, (q1, q2), (u1, u2), - kd_eqs=( - u1 - q1.diff(), - u2 - q2.diff(), - u3 - q3.diff(), - u4 - q4.diff(), - ), + kd_eqs=kindiff[:], q_dependent=(q3, q4), configuration_constraints=holonomic, - velocity_constraints=holonomic.diff(me.dynamicsymbols._t), + velocity_constraints=me.msubs(holonomic.diff(me.dynamicsymbols._t), + qd_repl), u_dependent=(u3, u4), + constraint_solver='CRAMER', ) bodies = (crank, foot, lower_leg, upper_leg) Fr, Frs = kane.kanes_equations(bodies, loads) muscle_diff_eq = sm.Matrix([ - knee_top_mus.x[0, 0].diff() - knee_top_mus.rhs()[0, 0], - knee_bot_mus.x[0, 0].diff() - knee_bot_mus.rhs()[0, 0], - ankle_top_mus.x[0, 0].diff() - ankle_top_mus.rhs()[0, 0], - ankle_bot_mus.x[0, 0].diff() - ankle_bot_mus.rhs()[0, 0], + knee_top_mus.a.diff() - knee_top_mus.rhs()[0, 0], + knee_bot_mus.a.diff() - knee_bot_mus.rhs()[0, 0], + ankle_top_mus.a.diff() - ankle_top_mus.rhs()[0, 0], + ankle_bot_mus.a.diff() - ankle_bot_mus.rhs()[0, 0], ]) eom = kindiff.col_join(Fr + Frs).col_join(muscle_diff_eq).col_join(holonomic) +#eom = me.msubs(eom, qd_repl) state_vars = ( q1, q2, q3, q4, u1, u2, u3, u4, - knee_top_mus.x[0, 0], - knee_bot_mus.x[0, 0], - ankle_top_mus.x[0, 0], - ankle_bot_mus.x[0, 0], + knee_top_mus.a, + knee_bot_mus.a, + ankle_top_mus.a, + ankle_bot_mus.a, ) num_nodes = 200 -h = sm.symbols('h', real=True, positive=True) +h = sm.symbols('h', real=True) # objective # minimize h @@ -314,38 +322,63 @@ def gradient(free): ls: 0.6, # guess TODO lu: 0.424, # upper_leg_length [m], m: 175.0, # kg - mA: 0.0, + #mA: 0.0, # not in eom mB: 1.0, # guess TODO mC: 6.769, # lower_leg_mass [kg] mD: 17.01, # upper_leg_mass [kg], rho: 1.204, # kg/m^3 rk: 0.04, # m rw: 0.3, # m + ankle_bot_mus.F_M_max: 500.0, + ankle_bot_mus.l_M_opt: 0.31, + ankle_bot_mus.l_T_slack: 0.3, + ankle_top_mus.F_M_max: 500.0, + ankle_top_mus.l_M_opt: 0.31, + ankle_top_mus.l_T_slack: 0.3, + knee_bot_mus.F_M_max: 500.0, + knee_bot_mus.l_M_opt: 0.9, + knee_bot_mus.l_T_slack: 0.8, + knee_top_mus.F_M_max: 500.0, + knee_top_mus.l_M_opt: 1.1, + knee_top_mus.l_T_slack: 1.0, } # TODO : Solve for dependent q's so that config is correct at start. instance_constraints = ( - q1(0*h), # crank starts horizontal - q2(0*h), # foot/pedal aligned with crank - u1(0*h), # start stationary - u2(0*h), # start stationary - u3(0*h), # start stationary - u4(0*h), # start stationary + q1.replace(t, 0*h), # crank starts horizontal + q2.replace(t, 0*h) - np.pi, # foot/pedal aligned with crank + u1.replace(t, 0*h), # start stationary + u2.replace(t, 0*h), # start stationary + u3.replace(t, 0*h), # start stationary + u4.replace(t, 0*h), # start stationary ) -# TODO bounds -# excitations should be bound 0 to 1 -# only allow forward motion - - -#problem = opty.Problem( - #obj, - #gradient, - #eom, - #state_vars, - #num_nodes, - #h, - #known_parameter_map=par_vals, - #instance_constraints=instance_constraints, - #bounds=bounds, -#) +bounds = { + q1: (-np.inf, 0.0), # can only pedal forward + # ankle angle, q3=0: ankle maximally plantar flexed, q3=-3*pi/2: ankle + # maximally dorsiflexed + q3: (-3*np.pi/2, 0.0), + # knee angle, q4 = 0: upper and lower leg aligned, q4 = pi/2: knee is + # flexed 90 degs + q4: (0.0, 3*np.pi/2), + u1: (-20.0, 0.0), # about 200 rpm + ankle_bot_mus.e: (0.0, 1.0), + ankle_top_mus.e: (0.0, 1.0), + knee_bot_mus.e: (0.0, 1.0), + knee_top_mus.e: (0.0, 1.0), +} + +problem = Problem( + obj, + gradient, + eom, + state_vars, + num_nodes, + h, + known_parameter_map=par_vals, + instance_constraints=instance_constraints, + bounds=bounds, +) + + +solution, info = problem.solve(np.random.random(problem.num_free)) From 1e07d450f478a357b8a09e35cb713e93f089155b Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 15 Jun 2024 07:49:12 +0200 Subject: [PATCH 06/40] Try to choose better muscle numbers and adjusted extensor pathway to swap bodies. --- .../plot_one_legged_time_trial.py | 105 +++++++++++++----- 1 file changed, 75 insertions(+), 30 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index b1ac436..8fe9aca 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -11,11 +11,13 @@ optimal performance. """ +from opty.direct_collocation import Problem +from scipy.optimize import fsolve +import matplotlib.pyplot as plt import numpy as np import sympy as sm -import sympy.physics.mechanics as me import sympy.physics.biomechanics as bm -from opty.direct_collocation import Problem +import sympy.physics.mechanics as me t = me.dynamicsymbols._t @@ -100,9 +102,11 @@ # TODO : Check why gear ratio is not applied to rolling resistance. resistance = me.Torque( crank, - # remove sign...causes derivative in jacobian - #(-Cr*m*g*rw - sm.sign(G*u1*rw)*CD*rho*Ar*G**2*u1**2*rw**3/2)*N.z - (-Cr*m*g*rw - CD*rho*Ar*G**2*u1**2*rw**3/2)*N.z + # NOTE : we enforce later that u1 < 0 (forward pedaling), thus the + # resistance should be a posistive torque to resist the negative speed + # NOTE : using sm.sign() will break the constraing jacobian due taking the + # derivative of sm.sign(). + (Cr*m*g*rw + CD*rho*Ar*G**2*u1**2*rw**3/2)*N.z ) # add inertia due to bike and wheels to crank @@ -219,12 +223,12 @@ def to_loads(self, force_magnitude): # four muscles # NOTE : pathway only valid for q4>=0 -knee_top_pathway = ExtensorPathway(P5, Co, P4, C.z, D.x, -C.x, rk, q4) +knee_top_pathway = ExtensorPathway(Co, P5, P4, C.z, -C.x, D.x, rk, q4) knee_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_top') knee_top_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_top', knee_top_pathway, knee_top_act) -knee_bot_pathway = me.LinearPathway(P5, Co) +knee_bot_pathway = me.LinearPathway(Co, P5) knee_bot_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_bot') knee_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_bot', knee_bot_pathway, @@ -273,7 +277,6 @@ def to_loads(self, force_magnitude): ]) eom = kindiff.col_join(Fr + Frs).col_join(muscle_diff_eq).col_join(holonomic) -#eom = me.msubs(eom, qd_repl) state_vars = ( q1, q2, q3, q4, u1, u2, u3, u4, @@ -304,7 +307,7 @@ def gradient(free): # body segment inertia from https://nbviewer.org/github/pydy/pydy-tutorial-human-standing/blob/master/notebooks/n07_simulation.ipynb -par_vals = { +par_map = { Ar: 0.55, # m^2, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike CD: 1.15, # unitless, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike Cr: 0.006, # unitless, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike @@ -316,45 +319,88 @@ def gradient(free): J: 2*0.1524, # from Browser Jason's thesis (rear wheel times 2) g: 9.81, lam: np.deg2rad(75.0), - lc: 0.175, - lf: 0.2, # guess TODO + lc: 0.175, # crank length [m] + lf: 0.14, # pedal to ankle [m] ll: 0.611, # lower_leg_length [m] - ls: 0.6, # guess TODO + ls: 0.7, # seat tube length [m] lu: 0.424, # upper_leg_length [m], m: 175.0, # kg #mA: 0.0, # not in eom - mB: 1.0, # guess TODO + mB: 1.0, # foot mass [kg] guess TODO mC: 6.769, # lower_leg_mass [kg] mD: 17.01, # upper_leg_mass [kg], rho: 1.204, # kg/m^3 rk: 0.04, # m rw: 0.3, # m - ankle_bot_mus.F_M_max: 500.0, - ankle_bot_mus.l_M_opt: 0.31, - ankle_bot_mus.l_T_slack: 0.3, - ankle_top_mus.F_M_max: 500.0, - ankle_top_mus.l_M_opt: 0.31, - ankle_top_mus.l_T_slack: 0.3, + ankle_bot_mus.F_M_max: 200.0, + ankle_bot_mus.l_M_opt: np.nan, + ankle_bot_mus.l_T_slack: np.nan, + ankle_top_mus.F_M_max: 200.0, + ankle_top_mus.l_M_opt: np.nan, + ankle_top_mus.l_T_slack: np.nan, knee_bot_mus.F_M_max: 500.0, - knee_bot_mus.l_M_opt: 0.9, - knee_bot_mus.l_T_slack: 0.8, - knee_top_mus.F_M_max: 500.0, - knee_top_mus.l_M_opt: 1.1, - knee_top_mus.l_T_slack: 1.0, + knee_bot_mus.l_M_opt: np.nan, + knee_bot_mus.l_T_slack: np.nan, + knee_top_mus.F_M_max: 1000.0, + knee_top_mus.l_M_opt: np.nan, + knee_top_mus.l_T_slack: np.nan, } -# TODO : Solve for dependent q's so that config is correct at start. +p = np.array(list(par_map.keys())) +p_vals = np.array(list(par_map.values())) + +# solve for maximal knee extension and flat foot +q1_ext = -par_map[lam] # aligned with seat post +q2_ext = 3*np.pi/2 # foot perpendicular to crank +eval_holonomic = sm.lambdify((q, p), holonomic) +q3_ext, q4_ext = fsolve(lambda x: eval_holonomic([q1_ext, q2_ext, x[0], x[1]], + p_vals).squeeze(), + x0=np.deg2rad([-100.0, 20.0])) +q_ext = np.array([q1_ext, q2_ext, q3_ext, q4_ext]) + +eval_ankle_top_len = sm.lambdify((q, p), ankle_top_pathway.length) +eval_ankle_bot_len = sm.lambdify((q, p), ankle_bot_pathway.length) +eval_knee_top_len = sm.lambdify((q, p), knee_top_pathway.length) +eval_knee_bot_len = sm.lambdify((q, p), knee_bot_pathway.length) +# length of muscle path when fully extended +par_map[ankle_top_mus.l_T_slack] = eval_ankle_top_len(q_ext, p_vals) +par_map[ankle_bot_mus.l_T_slack] = eval_ankle_bot_len(q_ext, p_vals) +par_map[knee_top_mus.l_T_slack] = eval_knee_top_len(q_ext, p_vals) +par_map[knee_bot_mus.l_T_slack] = eval_knee_bot_len(q_ext, p_vals) +par_map[ankle_top_mus.l_M_opt] = par_map[ankle_top_mus.l_T_slack] + 0.01 +par_map[ankle_bot_mus.l_M_opt] = par_map[ankle_bot_mus.l_T_slack] + 0.01 +par_map[knee_top_mus.l_M_opt] = par_map[knee_top_mus.l_T_slack] + 0.01 +par_map[knee_bot_mus.l_M_opt] = par_map[knee_bot_mus.l_T_slack] + 0.01 + +# solve for initial configuration +q1_0 = 0.0 # horizontal and forward +q2_0 = np.pi # toe forward and foot aligned with the crank (horizontal) +eval_holonomic = sm.lambdify((q, p), holonomic) +q3_0, q4_0 = fsolve(lambda x: eval_holonomic([q1_0, q2_0, x[0], x[1]], + p_vals).squeeze(), + x0=np.deg2rad([-90.0, 90.0])) +q_0 = np.array([q1_0, q2_0, q3_0, q4_0]) + +crank_revs = 10 + instance_constraints = ( - q1.replace(t, 0*h), # crank starts horizontal - q2.replace(t, 0*h) - np.pi, # foot/pedal aligned with crank + q1.replace(t, 0*h) - q1_0, # crank starts horizontal + q2.replace(t, 0*h) - q2_0, # foot/pedal aligned with crank + q3.replace(t, 0*h) - q3_0, + q4.replace(t, 0*h) - q4_0, + q1.replace(t, (num_nodes - 1)*h) + crank_revs*2*np.pi, # travel number of revolutions u1.replace(t, 0*h), # start stationary u2.replace(t, 0*h), # start stationary u3.replace(t, 0*h), # start stationary u4.replace(t, 0*h), # start stationary + knee_top_mus.a.replace(t, 0*h), + knee_bot_mus.a.replace(t, 0*h), + ankle_top_mus.a.replace(t, 0*h), + ankle_bot_mus.a.replace(t, 0*h), ) bounds = { - q1: (-np.inf, 0.0), # can only pedal forward + q1: (-crank_revs*2*np.pi, 0.0), # can only pedal forward # ankle angle, q3=0: ankle maximally plantar flexed, q3=-3*pi/2: ankle # maximally dorsiflexed q3: (-3*np.pi/2, 0.0), @@ -375,10 +421,9 @@ def gradient(free): state_vars, num_nodes, h, - known_parameter_map=par_vals, + known_parameter_map=par_map, instance_constraints=instance_constraints, bounds=bounds, ) - solution, info = problem.solve(np.random.random(problem.num_free)) From 577fdc6b1d2879d1b157384a12fd61a64784ba2f Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 15 Jun 2024 08:39:04 +0200 Subject: [PATCH 07/40] Added configuration plots." --- .../plot_one_legged_time_trial.py | 44 ++++++++++++++++++- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 8fe9aca..ddc25b7 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -320,9 +320,9 @@ def gradient(free): g: 9.81, lam: np.deg2rad(75.0), lc: 0.175, # crank length [m] - lf: 0.14, # pedal to ankle [m] + lf: 0.08, # pedal to ankle [m] ll: 0.611, # lower_leg_length [m] - ls: 0.7, # seat tube length [m] + ls: 0.8, # seat tube length [m] lu: 0.424, # upper_leg_length [m], m: 175.0, # kg #mA: 0.0, # not in eom @@ -349,6 +349,7 @@ def gradient(free): p = np.array(list(par_map.keys())) p_vals = np.array(list(par_map.values())) + # solve for maximal knee extension and flat foot q1_ext = -par_map[lam] # aligned with seat post q2_ext = 3*np.pi/2 # foot perpendicular to crank @@ -358,6 +359,40 @@ def gradient(free): x0=np.deg2rad([-100.0, 20.0])) q_ext = np.array([q1_ext, q2_ext, q3_ext, q4_ext]) +plot_points = [P1, P2, P7, P3, P4, P5, P1] +coordinates = P1.pos_from(P1).to_matrix(N) +for Pi in plot_points[1:]: + coordinates = coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) +eval_coordinates = sm.lambdify((q, p), coordinates) + +mus_points = [P7, Co, Co, P5] +mus_coordinates = P7.pos_from(P1).to_matrix(N) +for Pi in mus_points[1:]: + mus_coordinates = mus_coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) +eval_mus_coordinates = sm.lambdify((q, p), mus_coordinates) + + +def plot_configuration(q_vals, p_vals, ax=None): + if ax is None: + fig, ax = plt.subplots(layout='constrained') + + x, y, _ = eval_coordinates(q_vals, p_vals) + xm, ym, _ = eval_mus_coordinates(q_vals, p_vals) + ax.plot(x, y, 'o-') + ax.plot(xm, ym, 'o-', color='red',) + crank_circle = plt.Circle((0.0, 0.0), par_map[lc], fill=False, + linestyle='--') + knee_circle = plt.Circle((x[4], y[4]), par_map[rk], color='red', + fill=False) + ax.add_patch(crank_circle) + ax.add_patch(knee_circle) + ax.set_aspect('equal') + return ax + + +plot_configuration(q_ext, p_vals) + + eval_ankle_top_len = sm.lambdify((q, p), ankle_top_pathway.length) eval_ankle_bot_len = sm.lambdify((q, p), ankle_bot_pathway.length) eval_knee_top_len = sm.lambdify((q, p), knee_top_pathway.length) @@ -381,6 +416,11 @@ def gradient(free): x0=np.deg2rad([-90.0, 90.0])) q_0 = np.array([q1_0, q2_0, q3_0, q4_0]) +plot_configuration(q_0, p_vals) +plt.show() + +pause + crank_revs = 10 instance_constraints = ( From 52f593d8b25b4dba10e811764ada3059cfe7146d Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 15 Jun 2024 09:20:55 +0200 Subject: [PATCH 08/40] Gives a solution that is close! --- .../plot_one_legged_time_trial.py | 108 +++++++++++------- 1 file changed, 69 insertions(+), 39 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index ddc25b7..1057dbb 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -12,7 +12,9 @@ """ from opty.direct_collocation import Problem +from opty.utils import parse_free from scipy.optimize import fsolve +import matplotlib.animation as animation import matplotlib.pyplot as plt import numpy as np import sympy as sm @@ -286,7 +288,7 @@ def to_loads(self, force_magnitude): ankle_bot_mus.a, ) -num_nodes = 200 +num_nodes = 201 h = sm.symbols('h', real=True) # objective @@ -359,38 +361,6 @@ def gradient(free): x0=np.deg2rad([-100.0, 20.0])) q_ext = np.array([q1_ext, q2_ext, q3_ext, q4_ext]) -plot_points = [P1, P2, P7, P3, P4, P5, P1] -coordinates = P1.pos_from(P1).to_matrix(N) -for Pi in plot_points[1:]: - coordinates = coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) -eval_coordinates = sm.lambdify((q, p), coordinates) - -mus_points = [P7, Co, Co, P5] -mus_coordinates = P7.pos_from(P1).to_matrix(N) -for Pi in mus_points[1:]: - mus_coordinates = mus_coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) -eval_mus_coordinates = sm.lambdify((q, p), mus_coordinates) - - -def plot_configuration(q_vals, p_vals, ax=None): - if ax is None: - fig, ax = plt.subplots(layout='constrained') - - x, y, _ = eval_coordinates(q_vals, p_vals) - xm, ym, _ = eval_mus_coordinates(q_vals, p_vals) - ax.plot(x, y, 'o-') - ax.plot(xm, ym, 'o-', color='red',) - crank_circle = plt.Circle((0.0, 0.0), par_map[lc], fill=False, - linestyle='--') - knee_circle = plt.Circle((x[4], y[4]), par_map[rk], color='red', - fill=False) - ax.add_patch(crank_circle) - ax.add_patch(knee_circle) - ax.set_aspect('equal') - return ax - - -plot_configuration(q_ext, p_vals) eval_ankle_top_len = sm.lambdify((q, p), ankle_top_pathway.length) @@ -416,11 +386,6 @@ def plot_configuration(q_vals, p_vals, ax=None): x0=np.deg2rad([-90.0, 90.0])) q_0 = np.array([q1_0, q2_0, q3_0, q4_0]) -plot_configuration(q_0, p_vals) -plt.show() - -pause - crank_revs = 10 instance_constraints = ( @@ -466,4 +431,69 @@ def plot_configuration(q_vals, p_vals, ax=None): bounds=bounds, ) -solution, info = problem.solve(np.random.random(problem.num_free)) +initial_guess = np.random.random(problem.num_free) +q1_guess = np.linspace(0.0, -crank_revs*2*np.pi, num=num_nodes) +q2_guess = np.linspace(0.0, crank_revs*2*np.pi, num=num_nodes) +u1_guess = -3.0*np.ones(num_nodes) +u2_guess = 3.0*np.ones(num_nodes) +initial_guess[0*num_nodes:1*num_nodes] = q1_guess +initial_guess[1*num_nodes:2*num_nodes] = q2_guess +initial_guess[4*num_nodes:5*num_nodes] = u1_guess +initial_guess[5*num_nodes:6*num_nodes] = u2_guess + +solution, info = problem.solve(initial_guess) + +problem.plot_objective_value() +problem.plot_constraint_violations(solution) +problem.plot_trajectories(solution) + +xs, us, ps = parse_free(solution, len(state_vars), 4, num_nodes) + +plot_points = [P1, P2, P7, P3, P4, P5, P1] +coordinates = P1.pos_from(P1).to_matrix(N) +for Pi in plot_points[1:]: + coordinates = coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) +eval_coordinates = sm.lambdify((q, p), coordinates) + +mus_points = [P7, Co, Co, P5] +mus_coordinates = P7.pos_from(P1).to_matrix(N) +for Pi in mus_points[1:]: + mus_coordinates = mus_coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) +eval_mus_coordinates = sm.lambdify((q, p), mus_coordinates) + + +def plot_configuration(q_vals, p_vals, ax=None): + if ax is None: + fig, ax = plt.subplots(layout='constrained') + + x, y, _ = eval_coordinates(q_vals, p_vals) + xm, ym, _ = eval_mus_coordinates(q_vals, p_vals) + leg_lines, = ax.plot(x, y, 'o-') + mus_lines, = ax.plot(xm, ym, 'o-', color='red',) + crank_circle = plt.Circle((0.0, 0.0), par_map[lc], fill=False, + linestyle='--') + knee_circle = plt.Circle((x[4], y[4]), par_map[rk], color='red', + fill=False) + ax.add_patch(crank_circle) + ax.add_patch(knee_circle) + ax.set_aspect('equal') + return ax, fig, leg_lines, mus_lines + + +plot_configuration(q_ext, p_vals) +ax, fig, leg_lines, mus_lines = plot_configuration(q_0, p_vals) + + +def animate(i): + qi = xs[0:4, i] + x, y, _ = eval_coordinates(qi, p_vals) + xm, ym, _ = eval_mus_coordinates(qi, p_vals) + leg_lines.set_data(x, y) + mus_lines.set_data(xm, ym) + + +interval_value = solution[-1] +ani = animation.FuncAnimation(fig, animate, num_nodes, + interval=int(interval_value*1000)) + +plt.show() From 8c63d20ffe91400b4de92b3542b3bb8b0786fa07 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 15 Jun 2024 09:34:31 +0200 Subject: [PATCH 09/40] Added missing muscle point and made more restriction on ankle motion. --- examples-gallery/plot_one_legged_time_trial.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 1057dbb..61a4442 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -406,9 +406,9 @@ def gradient(free): bounds = { q1: (-crank_revs*2*np.pi, 0.0), # can only pedal forward - # ankle angle, q3=0: ankle maximally plantar flexed, q3=-3*pi/2: ankle - # maximally dorsiflexed - q3: (-3*np.pi/2, 0.0), + # ankle angle, q3=-np.pi/4: ankle maximally plantar flexed, q3=-3*pi/2: + # ankle maximally dorsiflexed + q3: (-np.pi/2, -np.pi/4), # knee angle, q4 = 0: upper and lower leg aligned, q4 = pi/2: knee is # flexed 90 degs q4: (0.0, 3*np.pi/2), @@ -455,7 +455,7 @@ def gradient(free): coordinates = coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) eval_coordinates = sm.lambdify((q, p), coordinates) -mus_points = [P7, Co, Co, P5] +mus_points = [P7, Co, P2, Co, P5] mus_coordinates = P7.pos_from(P1).to_matrix(N) for Pi in mus_points[1:]: mus_coordinates = mus_coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) From ca822b724c12850b032dda4cfd3cee8241eec609 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 15 Jun 2024 10:03:02 +0200 Subject: [PATCH 10/40] The u1 = 0 at t = 0 seemed to never stick, so relaxed it. --- examples-gallery/plot_one_legged_time_trial.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 61a4442..7571020 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -394,7 +394,7 @@ def gradient(free): q3.replace(t, 0*h) - q3_0, q4.replace(t, 0*h) - q4_0, q1.replace(t, (num_nodes - 1)*h) + crank_revs*2*np.pi, # travel number of revolutions - u1.replace(t, 0*h), # start stationary + #u1.replace(t, 0*h), # start stationary u2.replace(t, 0*h), # start stationary u3.replace(t, 0*h), # start stationary u4.replace(t, 0*h), # start stationary @@ -431,11 +431,14 @@ def gradient(free): bounds=bounds, ) +# segmentation fault if I set initial guess to zero initial_guess = np.random.random(problem.num_free) q1_guess = np.linspace(0.0, -crank_revs*2*np.pi, num=num_nodes) q2_guess = np.linspace(0.0, crank_revs*2*np.pi, num=num_nodes) -u1_guess = -3.0*np.ones(num_nodes) -u2_guess = 3.0*np.ones(num_nodes) +u1_guess = np.linspace(0.0, -6.0, num=num_nodes) +u1_guess[num_nodes//2:] = -3.0 +u2_guess = np.linspace(0.0, 6.0, num=num_nodes) +u2_guess[num_nodes//2:] = 3.0 initial_guess[0*num_nodes:1*num_nodes] = q1_guess initial_guess[1*num_nodes:2*num_nodes] = q2_guess initial_guess[4*num_nodes:5*num_nodes] = u1_guess @@ -449,13 +452,13 @@ def gradient(free): xs, us, ps = parse_free(solution, len(state_vars), 4, num_nodes) -plot_points = [P1, P2, P7, P3, P4, P5, P1] +plot_points = [P1, P2, P7, P3, P4, P6, P1] coordinates = P1.pos_from(P1).to_matrix(N) for Pi in plot_points[1:]: coordinates = coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) eval_coordinates = sm.lambdify((q, p), coordinates) -mus_points = [P7, Co, P2, Co, P5] +mus_points = [P7, Co, P2, Co, P6] mus_coordinates = P7.pos_from(P1).to_matrix(N) for Pi in mus_points[1:]: mus_coordinates = mus_coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) From c9197726ece73e5bc41bb402e38fb35e63d7742c Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 15 Jun 2024 11:08:08 +0200 Subject: [PATCH 11/40] Use 400 nodes and higher gear ratio. --- .../plot_one_legged_time_trial.py | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 7571020..b8f930f 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -288,7 +288,7 @@ def to_loads(self, force_magnitude): ankle_bot_mus.a, ) -num_nodes = 201 +num_nodes = 401 h = sm.symbols('h', real=True) # objective @@ -313,7 +313,7 @@ def gradient(free): Ar: 0.55, # m^2, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike CD: 1.15, # unitless, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike Cr: 0.006, # unitless, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike - G: 2.0, + G: 3.0, IAzz: 0.0, IBzz: 0.01, # guess, TODO ICzz: 0.101, # lower_leg_inertia [kg*m^2] @@ -417,6 +417,7 @@ def gradient(free): ankle_top_mus.e: (0.0, 1.0), knee_bot_mus.e: (0.0, 1.0), knee_top_mus.e: (0.0, 1.0), + h: (0.0, 0.2), } problem = Problem( @@ -443,6 +444,7 @@ def gradient(free): initial_guess[1*num_nodes:2*num_nodes] = q2_guess initial_guess[4*num_nodes:5*num_nodes] = u1_guess initial_guess[5*num_nodes:6*num_nodes] = u2_guess +initial_guess[-1] = 0.1 solution, info = problem.solve(initial_guess) @@ -450,7 +452,22 @@ def gradient(free): problem.plot_constraint_violations(solution) problem.plot_trajectories(solution) -xs, us, ps = parse_free(solution, len(state_vars), 4, num_nodes) +xs, us, ps, h_val= parse_free(solution, len(state_vars), 4, num_nodes, + variable_duration=True) +interval_value = h_val + + +def plot_sim_compact(): + fig, axes = plt.subplots(3, 1, sharex=True) + time = np.linspace(0, num_nodes*interval_value, num=num_nodes) + axes[0].plot(time, xs[0:4, :].T) + axes[0].legend(q) + axes[1].plot(time, xs[4:8, :].T) + axes[1].legend(u) + axes[2].plot(time, us.T) + + +plot_sim_compact() plot_points = [P1, P2, P7, P3, P4, P6, P1] coordinates = P1.pos_from(P1).to_matrix(N) @@ -495,7 +512,6 @@ def animate(i): mus_lines.set_data(xm, ym) -interval_value = solution[-1] ani = animation.FuncAnimation(fig, animate, num_nodes, interval=int(interval_value*1000)) From ccdf6a02447492b0376dd4ec9f8f05fff559b8be Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 15 Jun 2024 11:19:53 +0200 Subject: [PATCH 12/40] Made new points for muscle attachments on the lower leg closer to joints, adjusted plots. --- .../plot_one_legged_time_trial.py | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index b8f930f..869ce66 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -54,8 +54,8 @@ # P5 : hip # P6 : seat center # P7 : heel -P1, P2, P3, P4, P5, P6, P7 = sm.symbols('P1, P2, P3, P4, P5, P6, P7', - cls=me.Point) +P1, P2, P3, P4, P5, P6, P7, P8, P9 = sm.symbols( + 'P1, P2, P3, P4, P5, P6, P7, P8, P9', cls=me.Point) Ao, Bo, Co, Do = sm.symbols('Ao, Bo, Co, Do', cls=me.Point) Ao.set_pos(P1, 0*A.x) @@ -68,6 +68,9 @@ Do.set_pos(P4, lu/2*D.x) # upper leg mass center P5.set_pos(P4, lu*D.x) # hip P6.set_pos(P1, -ls*sm.cos(lam)*N.x + ls*sm.sin(lam)*N.y) # seat +P8.set_pos(P3, ll/4*C.x) +P9.set_pos(P4, -2*rk*C.x) + P1.set_vel(N, 0) P6.set_vel(N, 0) @@ -77,6 +80,8 @@ Bo.v2pt_theory(P2, N, B) P3.v2pt_theory(P2, N, B) Co.v2pt_theory(P3, N, C) +P8.v2pt_theory(P3, N, C) +P9.v2pt_theory(P3, N, C) P4.v2pt_theory(P3, N, C) Do.v2pt_theory(P4, N, D) P5.v2pt_theory(P4, N, D) @@ -225,22 +230,22 @@ def to_loads(self, force_magnitude): # four muscles # NOTE : pathway only valid for q4>=0 -knee_top_pathway = ExtensorPathway(Co, P5, P4, C.z, -C.x, D.x, rk, q4) +knee_top_pathway = ExtensorPathway(P9, P5, P4, C.z, -C.x, D.x, rk, q4) knee_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_top') knee_top_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_top', knee_top_pathway, knee_top_act) -knee_bot_pathway = me.LinearPathway(Co, P5) +knee_bot_pathway = me.LinearPathway(P9, P5) knee_bot_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_bot') knee_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_bot', knee_bot_pathway, knee_bot_act) -ankle_top_pathway = me.LinearPathway(Co, P2) +ankle_top_pathway = me.LinearPathway(P8, P2) ankle_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults('ankle_top') ankle_top_mus = bm.MusculotendonDeGroote2016.with_defaults('ankle_top', ankle_top_pathway, ankle_top_act) -ankle_bot_pathway = me.LinearPathway(Co, P7) +ankle_bot_pathway = me.LinearPathway(P8, P7) ankle_bot_act = bm.FirstOrderActivationDeGroote2016.with_defaults('ankle_bot') ankle_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('ankle_bot', ankle_bot_pathway, @@ -475,7 +480,7 @@ def plot_sim_compact(): coordinates = coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) eval_coordinates = sm.lambdify((q, p), coordinates) -mus_points = [P7, Co, P2, Co, P6] +mus_points = [P7, P8, P2, P8, P9, P6] mus_coordinates = P7.pos_from(P1).to_matrix(N) for Pi in mus_points[1:]: mus_coordinates = mus_coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) @@ -497,11 +502,11 @@ def plot_configuration(q_vals, p_vals, ax=None): ax.add_patch(crank_circle) ax.add_patch(knee_circle) ax.set_aspect('equal') - return ax, fig, leg_lines, mus_lines + return ax, fig, leg_lines, mus_lines, knee_circle plot_configuration(q_ext, p_vals) -ax, fig, leg_lines, mus_lines = plot_configuration(q_0, p_vals) +ax, fig, leg_lines, mus_lines, knee_circle = plot_configuration(q_0, p_vals) def animate(i): @@ -510,6 +515,7 @@ def animate(i): xm, ym, _ = eval_mus_coordinates(qi, p_vals) leg_lines.set_data(x, y) mus_lines.set_data(xm, ym) + knee_circle.set_center((x[4], y[4])) ani = animation.FuncAnimation(fig, animate, num_nodes, From e72c000899e8591c41fefaf3aeef67e7de77fbd3 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 15 Jun 2024 11:51:26 +0200 Subject: [PATCH 13/40] Scaling seemed to deal with the jump I was getting in u1. --- .../plot_one_legged_time_trial.py | 27 +++++++++++-------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 869ce66..50d71be 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -339,16 +339,16 @@ def gradient(free): rho: 1.204, # kg/m^3 rk: 0.04, # m rw: 0.3, # m - ankle_bot_mus.F_M_max: 200.0, + ankle_bot_mus.F_M_max: 100.0, ankle_bot_mus.l_M_opt: np.nan, ankle_bot_mus.l_T_slack: np.nan, - ankle_top_mus.F_M_max: 200.0, + ankle_top_mus.F_M_max: 100.0, ankle_top_mus.l_M_opt: np.nan, ankle_top_mus.l_T_slack: np.nan, - knee_bot_mus.F_M_max: 500.0, + knee_bot_mus.F_M_max: 300.0, knee_bot_mus.l_M_opt: np.nan, knee_bot_mus.l_T_slack: np.nan, - knee_top_mus.F_M_max: 1000.0, + knee_top_mus.F_M_max: 600.0, knee_top_mus.l_M_opt: np.nan, knee_top_mus.l_T_slack: np.nan, } @@ -435,7 +435,9 @@ def gradient(free): known_parameter_map=par_map, instance_constraints=instance_constraints, bounds=bounds, + #integration_method='midpoint', ) +problem.add_option('nlp_scaling_method', 'gradient-based') # segmentation fault if I set initial guess to zero initial_guess = np.random.random(problem.num_free) @@ -449,6 +451,7 @@ def gradient(free): initial_guess[1*num_nodes:2*num_nodes] = q2_guess initial_guess[4*num_nodes:5*num_nodes] = u1_guess initial_guess[5*num_nodes:6*num_nodes] = u2_guess +initial_guess[8*num_nodes:] = 0.5 # e initial_guess[-1] = 0.1 solution, info = problem.solve(initial_guess) @@ -459,17 +462,19 @@ def gradient(free): xs, us, ps, h_val= parse_free(solution, len(state_vars), 4, num_nodes, variable_duration=True) -interval_value = h_val def plot_sim_compact(): - fig, axes = plt.subplots(3, 1, sharex=True) - time = np.linspace(0, num_nodes*interval_value, num=num_nodes) + fig, axes = plt.subplots(4, 1, sharex=True) + time = np.linspace(0, num_nodes*h_val, num=num_nodes) axes[0].plot(time, xs[0:4, :].T) axes[0].legend(q) - axes[1].plot(time, xs[4:8, :].T) - axes[1].legend(u) - axes[2].plot(time, us.T) + axes[1].plot(time, -xs[4, :]*60/2/np.pi, time, xs[5, :]*60/2/np.pi) + axes[1].legend(['Cadence', 'Pedal Cadence']) + axes[2].plot(time, us[0:2, :].T) + axes[2].legend(problem.collocator.unknown_input_trajectories[0:2]) + axes[3].plot(time, us[2:4, :].T) + axes[3].legend(problem.collocator.unknown_input_trajectories[2:4]) plot_sim_compact() @@ -519,6 +524,6 @@ def animate(i): ani = animation.FuncAnimation(fig, animate, num_nodes, - interval=int(interval_value*1000)) + interval=int(h_val*1000)) plt.show() From 135666752506a80f043953537c66878eb32bc83f Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 15 Jun 2024 12:08:09 +0200 Subject: [PATCH 14/40] Make num nodes function of crank revolutions. (u1 jump came back). --- examples-gallery/plot_one_legged_time_trial.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 50d71be..51c2a45 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -293,7 +293,8 @@ def to_loads(self, force_magnitude): ankle_bot_mus.a, ) -num_nodes = 401 +crank_revs = 10 +num_nodes = crank_revs*50 + 1 h = sm.symbols('h', real=True) # objective @@ -391,7 +392,6 @@ def gradient(free): x0=np.deg2rad([-90.0, 90.0])) q_0 = np.array([q1_0, q2_0, q3_0, q4_0]) -crank_revs = 10 instance_constraints = ( q1.replace(t, 0*h) - q1_0, # crank starts horizontal From 8e4b0d731541263b9b1900a608af829cff3316de Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 15 Jun 2024 22:28:00 +0200 Subject: [PATCH 15/40] Added lots of text, moved Extensor to utils, and tweaked the solution inputs parameters. --- .../plot_one_legged_time_trial.py | 414 ++++++++++-------- opty/utils.py | 104 +++++ 2 files changed, 346 insertions(+), 172 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 51c2a45..bb990f4 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -1,4 +1,8 @@ """ +============================= +One-Legged Cycling Time Trial +============================= + Single human leg with four driving lumped muscles. The crank inertia and resistance mimic the torque felt if accelerating the whole bicycle with rider on flat ground. @@ -13,6 +17,7 @@ """ from opty.direct_collocation import Problem from opty.utils import parse_free +from opty.utils import _ExtensorPathway as ExtensorPathway from scipy.optimize import fsolve import matplotlib.animation as animation import matplotlib.pyplot as plt @@ -21,22 +26,69 @@ import sympy.physics.biomechanics as bm import sympy.physics.mechanics as me +# %% +# Coordinates +# =========== +# +# - :math:`q_1`: crank angle +# - :math:`q_2`: pedal angle +# - :math:`q_3`: ankle angle +# - :math:`q_4`: knee angle +# - :math:`u_1`: crank angular rate (cadence) +# - :math:`u_2`: pedal angular rate +# - :math:`u_3`: ankle angular rate +# - :math:`u_4`: knee anglular rate t = me.dynamicsymbols._t - -# bike frame, crank, pedal/foot, lower leg, upper leg -N, A, B, C, D = sm.symbols('N, A, B, C, D', cls=me.ReferenceFrame) - q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4', real=True) u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4', real=True) q = sm.Matrix([q1, q2, q3, q4]) u = sm.Matrix([u1, u2, u3, u4]) +# %% +# Constants +# ========= +# +# - :math:`l_s`: seat tube length +# - :math:`l_c`: crank length +# - :math:`l_f`: distance from pedal-foot contact to ankle +# - :math:`l_l`: lower leg length +# - :math:`l_u`: upper leg length +# - :math:`\lambda`: seat tube angle +# - :math:`g`: acceleration due to gravity +# - :math:`r_k`: knee wrapping radius +# - :math:`m_A`: mass of crank +# - :math:`m_B`: mass of foot and pedal +# - :math:`m_C`: mass of lower leg +# - :math:`m_D`: mass of upper leg +# - :math:`I_{Axx}`: moment of inertia of crank +# - :math:`I_{Bxx}`: moment of inertia of foot and pedal +# - :math:`I_{Cxx}`: moment of inertia of lower leg +# - :math:`I_{Dxx}`: moment of inertia of upper leg +# - :math:`J`: rotational moment of inertia of the bicycle wheel +# - :math:`m`: mass of the bicycle and cyclist +# - :math:`r_w`: wheel radius +# - :math:`G`: gear ratio between crank and wheel +# - :math:`C_r`: coefficient of rolling resistance +# - :math:`C_D`: coefficient of drag +# - :math:`\rho`: density of air +# - :math:`A_r`: frontal area of bicycle and cyclist ls, lc, lf, ll, lu = sm.symbols('ls, lc, lf, ll, lu', real=True, positive=True) lam, g, rk = sm.symbols('lam, g, rk', real=True) mA, mB, mC, mD = sm.symbols('mA, mB, mC, mD') IAzz, IBzz, ICzz, IDzz = sm.symbols('IAzz, IBzz, ICzz, IDzz') J, m, rw, G, Cr, CD, rho, Ar = sm.symbols('J, m, rw, G, Cr, CD, rho, Ar') +# %% +# Reference Frames +# ================ +# +# - :math:`N`: inertial reference frame for leg dynamics +# - :math:`A`: crank +# - :math:`B`: foot +# - :math:`C`: lower leg +# - :math:`D`: upper leg +N, A, B, C, D = sm.symbols('N, A, B, C, D', cls=me.ReferenceFrame) + A.orient_axis(N, N.z, q1) # crank angle B.orient_axis(A, A.z, q2) # pedal/foot angle C.orient_axis(B, B.z, q3) # ankle angle @@ -47,13 +99,18 @@ C.set_ang_vel(B, u3*B.z) D.set_ang_vel(C, u4*C.z) -# P1 : crank center -# P2 : pedal center -# P3 : ankle -# P4 : knee -# P5 : hip -# P6 : seat center -# P7 : heel +# %% +# Point Kinematics +# ================ +# - :math:`P_1` : crank center +# - :math:`P_2` : pedal center +# - :math:`P_3` : ankle +# - :math:`P_4` : knee +# - :math:`P_5` : hip +# - :math:`P_6` : seat center +# - :math:`P_7` : heel +# - :math:`P_8` : knee muscle lower leg insertion point +# - :math:`P_9` : ankle muscle lower leg insertion point P1, P2, P3, P4, P5, P6, P7, P8, P9 = sm.symbols( 'P1, P2, P3, P4, P5, P6, P7, P8, P9', cls=me.Point) Ao, Bo, Co, Do = sm.symbols('Ao, Bo, Co, Do', cls=me.Point) @@ -68,10 +125,9 @@ Do.set_pos(P4, lu/2*D.x) # upper leg mass center P5.set_pos(P4, lu*D.x) # hip P6.set_pos(P1, -ls*sm.cos(lam)*N.x + ls*sm.sin(lam)*N.y) # seat -P8.set_pos(P3, ll/4*C.x) +P8.set_pos(P3, ll/6*C.x) P9.set_pos(P4, -2*rk*C.x) - P1.set_vel(N, 0) P6.set_vel(N, 0) Ao.v2pt_theory(P1, N, A) @@ -88,9 +144,20 @@ kindiff = sm.Matrix([ui - qi.diff() for ui, qi in zip(u, q)]) +# %% +# Holonomic Constraints +# ===================== +# +# The leg forms a kinematic loop and two holonomic constraints arise from this +# loop. holonomic = (P5.pos_from(P1) - P6.pos_from(P1)).to_matrix(N)[:2, :] -IA = me.Inertia.from_inertia_scalars(Ao, A, 0, 0, IAzz + (J + m*rw**2)*G) +qd_repl = {q1.diff(): u1, q2.diff(): u2, q3.diff(): u3, q4.diff(): u4} + +mocon = me.msubs(holonomic.diff(t), qd_repl) + +# %% +IA = me.Inertia.from_inertia_scalars(Ao, A, 0, 0, IAzz) IB = me.Inertia.from_inertia_scalars(Bo, B, 0, 0, IBzz) IC = me.Inertia.from_inertia_scalars(Co, C, 0, 0, ICzz) ID = me.Inertia.from_inertia_scalars(Do, D, 0, 0, IDzz) @@ -102,134 +169,70 @@ upper_leg = me.RigidBody('upper leg', masscenter=Do, frame=D, mass=mD, inertia=ID) +# %% +# Forces +# ====== gravB = me.Force(Bo, -mB*g*N.y) gravC = me.Force(Co, -mC*g*N.y) gravD = me.Force(Do, -mD*g*N.y) -# TODO : Check why gear ratio is not applied to rolling resistance. +# %% +# Crank Resistance +# ---------------- +# +# We model the resistance torque at the crank to be that which you would feel +# when accelerating the bicycle and cyclist along flat ground. The basic +# equations of motion of a point mass model of a cyclist are: +# +# .. math:: +# +# (2J + m r_w^2)\dot{\omega} = +# -C_r m g r_w - \sgn \omega \frac{1}{2} \rho C_D A_r (\omega r_w)^2 + +# T_w +# +# The angular speed of the rear wheel is related to the crank cadence by the +# gear ratio :math:`G`: +# +# .. math:: +# +# \omega = G u_1 \\ +# G T_w = T_c +# +# .. math:: +# +# T_c = +# -(2J + m r_w^2)\dot{u}_1 +# - C_r m g r_w G +# - \sgn u_1 \frac{1}{2} \rho C_D A_r G (u_1 r_w)^2 +# +# :math:`u_1 < 0` give forward motion in the pedaling sign convention and we +# will restrict :math:`u_1 < 0` below, so the torque felt at the crank is: +# +# .. math:: +# +# T_c = +# - (2J + m r_w^2)\dot{u}_1 +# - C_r m g r_w G +# + \frac{1}{2} \rho C_D A_r G (u_1 r_w)^2 +# TODO : Fix equations written above, they are not correct. + resistance = me.Torque( crank, # NOTE : we enforce later that u1 < 0 (forward pedaling), thus the # resistance should be a posistive torque to resist the negative speed - # NOTE : using sm.sign() will break the constraing jacobian due taking the + # NOTE : using sm.sign() will break the constraint Jacobian due taking the # derivative of sm.sign(). - (Cr*m*g*rw + CD*rho*Ar*G**2*u1**2*rw**3/2)*N.z + (-(2*J + m*rw**2)*G**2*u1.diff() + Cr*m*g*rw*G + + rho*CD*Ar*G**3*rw**3*u1**2/2)*N.z, ) -# add inertia due to bike and wheels to crank -# add resitance torque due to air drag and rolling resitance to crank -# omega = gear_ratio*ul -# T = TA/gear_ratio -# (2*J + m*r**2)*omega' = -sign(omega*r)*1/2*rho*Cd*(omega*r)**2 - Cr*m*g + T -# w*r = v -> w'*r = v' -> w' = v'/r - - -class ExtensorPathway(me.PathwayBase): - def __init__(self, origin, insertion, axis_point, axis, parent_axis, - child_axis, radius, coordinate): - """A custom pathway that wraps a circular arc around a pin joint. - This is intended to be used for extensor muscles. For example, a - triceps wrapping around the elbow joint to extend the upper arm at - the elbow. - - Parameters - ========== - origin : Point - Muscle origin point fixed on the parent body (A). - insertion : Point - Muscle insertion point fixed on the child body (B). - axis_point : Point - Pin joint location fixed in both the parent and child. - axis : Vector - Pin joint rotation axis. - parent_axis : Vector - Axis fixed in the parent frame (A) that is directed from the pin - joint point to the muscle origin point. - child_axis : Vector - Axis fixed in the child frame (B) that is directed from the pin - joint point to the muscle insertion point. - radius : sympyfiable - Radius of the arc that the muscle wraps around. - coordinate : sympfiable function of time - Joint angle, zero when parent and child frames align. Positive - rotation about the pin joint axis, B with respect to A. - - Notes - ===== - Only valid for coordinate >= 0. - - """ - super().__init__(origin, insertion) - self.origin = origin - self.insertion = insertion - self.axis_point = axis_point - self.axis = axis.normalize() - self.parent_axis = parent_axis.normalize() - self.child_axis = child_axis.normalize() - self.radius = radius - self.coordinate = coordinate - self.origin_distance = axis_point.pos_from(origin).magnitude() - self.insertion_distance = axis_point.pos_from(insertion).magnitude() - self.origin_angle = sm.asin(self.radius/self.origin_distance) - self.insertion_angle = sm.asin(self.radius/self.insertion_distance) - - @property - def length(self): - """Length of the pathway. - Length of two fixed length line segments and a changing arc length - of a circle. - """ - angle = self.origin_angle + self.coordinate + self.insertion_angle - arc_length = self.radius*angle - origin_segment_length = self.origin_distance*sm.cos(self.origin_angle) - insertion_segment_length = self.insertion_distance*sm.cos( - self.insertion_angle) - return origin_segment_length + arc_length + insertion_segment_length - - @property - def extension_velocity(self): - """Extension velocity of the pathway. - Arc length of circle is the only thing that changes when the elbow - flexes and extends. - """ - return self.radius*self.coordinate.diff(me.dynamicsymbols._t) - - def to_loads(self, force_magnitude): - """Loads in the correct format to be supplied to `KanesMethod`. - Forces applied to origin, insertion, and P from the muscle wrapped - over circular arc of radius r. - """ - parent_tangency_point = me.Point('Aw') # fixed in parent - child_tangency_point = me.Point('Bw') # fixed in child - parent_tangency_point.set_pos( - self.axis_point, - -self.radius*sm.cos(self.origin_angle)*self.parent_axis.cross( - self.axis) - + self.radius*sm.sin(self.origin_angle)*self.parent_axis, - ) - child_tangency_point.set_pos( - self.axis_point, - self.radius*sm.cos(self.insertion_angle)*self.child_axis.cross( - self.axis) - + self.radius*sm.sin(self.insertion_angle)*self.child_axis), - parent_force_direction_vector = self.origin.pos_from( - parent_tangency_point) - child_force_direction_vector = self.insertion.pos_from( - child_tangency_point) - force_on_parent = (force_magnitude* - parent_force_direction_vector.normalize()) - force_on_child = (force_magnitude* - child_force_direction_vector.normalize()) - loads = [ - me.Force(self.origin, force_on_parent), - me.Force(self.axis_point, -(force_on_parent + force_on_child)), - me.Force(self.insertion, force_on_child), - ] - return loads - - -# four muscles -# NOTE : pathway only valid for q4>=0 +# %% +# Muscles +# ------- +# +# We lump all the muscles that contribute to joint torques at the knee and +# ankle into four simplified muscles. The quadriceps wrap over the knee. The +# other muscles act on linear pathways. knee_top_pathway = ExtensorPathway(P9, P5, P4, C.z, -C.x, D.x, rk, q4) knee_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_top') knee_top_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_top', @@ -250,7 +253,6 @@ def to_loads(self, force_magnitude): ankle_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('ankle_bot', ankle_bot_pathway, ankle_bot_act) - loads = ( knee_top_mus.to_loads() + knee_bot_mus.to_loads() + @@ -259,7 +261,9 @@ def to_loads(self, force_magnitude): [resistance, gravB, gravC, gravD] ) -qd_repl = {q1.diff(): u1, q2.diff(): u2, q3.diff(): u3, q4.diff(): u4} +# %% +# Form the Equations of Motion +# ============================ kane = me.KanesMethod( N, @@ -268,8 +272,7 @@ def to_loads(self, force_magnitude): kd_eqs=kindiff[:], q_dependent=(q3, q4), configuration_constraints=holonomic, - velocity_constraints=me.msubs(holonomic.diff(me.dynamicsymbols._t), - qd_repl), + velocity_constraints=mocon, u_dependent=(u3, u4), constraint_solver='CRAMER', ) @@ -293,38 +296,57 @@ def to_loads(self, force_magnitude): ankle_bot_mus.a, ) -crank_revs = 10 -num_nodes = crank_revs*50 + 1 -h = sm.symbols('h', real=True) - -# objective -# minimize h -# gradient of h wrt free variables: states, unknown_inputs, unknown_pars, h -# so then there would be all zeros and a single 1 in the gradient for the last -# entry +# %% +# Objective +# ========= +# +# The objective is to cycle as fast as possible, so we need to find the minmal +# time duration for a fixed distance (or more simply crank revolutions). This +# can be written mathmatically as: +# +# .. math:: +# +# min \int^{t_0}_{t_f} dt +# +# This discretizes to: +# +# .. math:: +# +# min \sum_1^N h +# +# If :math:`h` is constant, then we can simply minimize :math:`h`. With the +# objective being :math:`h`, the gradient of the objective with respect to all +# of the free optimization variables is zero except for the single entry of +# :math:`\frac{\partial h}{\partial h} = 1`. def obj(free): + """Return h (always the last element in the free variables).""" return free[-1] def gradient(free): + """Return the gradient of the objective.""" grad = np.zeros_like(free) grad[-1] = 1.0 return grad +# %% +# Define Numerical Constants +# ========================== + # body segment inertia from https://nbviewer.org/github/pydy/pydy-tutorial-human-standing/blob/master/notebooks/n07_simulation.ipynb par_map = { Ar: 0.55, # m^2, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike CD: 1.15, # unitless, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike Cr: 0.006, # unitless, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike - G: 3.0, + G: 2.0, IAzz: 0.0, IBzz: 0.01, # guess, TODO ICzz: 0.101, # lower_leg_inertia [kg*m^2] IDzz: 0.282, # upper_leg_inertia [kg*m^2], - J: 2*0.1524, # from Browser Jason's thesis (rear wheel times 2) + J: 0.1524, # from Browser Jason's thesis (rear wheel) g: 9.81, lam: np.deg2rad(75.0), lc: 0.175, # crank length [m] @@ -332,14 +354,14 @@ def gradient(free): ll: 0.611, # lower_leg_length [m] ls: 0.8, # seat tube length [m] lu: 0.424, # upper_leg_length [m], - m: 175.0, # kg + m: 85.0, # kg #mA: 0.0, # not in eom mB: 1.0, # foot mass [kg] guess TODO mC: 6.769, # lower_leg_mass [kg] mD: 17.01, # upper_leg_mass [kg], - rho: 1.204, # kg/m^3 - rk: 0.04, # m - rw: 0.3, # m + rho: 1.204, # kg/m^3, air density + rk: 0.04, # m, knee radius + rw: 0.3, # m, wheel radius ankle_bot_mus.F_M_max: 100.0, ankle_bot_mus.l_M_opt: np.nan, ankle_bot_mus.l_T_slack: np.nan, @@ -357,8 +379,11 @@ def gradient(free): p = np.array(list(par_map.keys())) p_vals = np.array(list(par_map.values())) +# %% +# To get estimates of the tendon slack length, align the crank with the seat +# tube to maximally extend the knee and hold the foot perpendicular to the +# lower leg then calculate the muscle pathway lengths in this configuration. -# solve for maximal knee extension and flat foot q1_ext = -par_map[lam] # aligned with seat post q2_ext = 3*np.pi/2 # foot perpendicular to crank eval_holonomic = sm.lambdify((q, p), holonomic) @@ -367,8 +392,6 @@ def gradient(free): x0=np.deg2rad([-100.0, 20.0])) q_ext = np.array([q1_ext, q2_ext, q3_ext, q4_ext]) - - eval_ankle_top_len = sm.lambdify((q, p), ankle_top_pathway.length) eval_ankle_bot_len = sm.lambdify((q, p), ankle_bot_pathway.length) eval_knee_top_len = sm.lambdify((q, p), knee_top_pathway.length) @@ -383,22 +406,41 @@ def gradient(free): par_map[knee_top_mus.l_M_opt] = par_map[knee_top_mus.l_T_slack] + 0.01 par_map[knee_bot_mus.l_M_opt] = par_map[knee_bot_mus.l_T_slack] + 0.01 -# solve for initial configuration -q1_0 = 0.0 # horizontal and forward -q2_0 = np.pi # toe forward and foot aligned with the crank (horizontal) +p_vals = np.array(list(par_map.values())) + +# %% +# Instance Constraints +# ==================== +# +# The cyclist should start with no motion (stationary) and in an initial +# configuration with the crank forward and horizontal (q1=0 deg) and the toe +# forward and foot parallel to the crank (q2=180 deg). +q1_0 = 0.0 +q2_0 = np.pi eval_holonomic = sm.lambdify((q, p), holonomic) q3_0, q4_0 = fsolve(lambda x: eval_holonomic([q1_0, q2_0, x[0], x[1]], p_vals).squeeze(), - x0=np.deg2rad([-90.0, 90.0])) + x0=np.deg2rad([-90.0, 90.0]), xtol=1e-14) q_0 = np.array([q1_0, q2_0, q3_0, q4_0]) +# Crank revolutions are proportional to distance traveled so the race distance +# is defined by number of crank revolutions. +crank_revs = 10 +samples_per_rev = 50 +num_nodes = crank_revs*samples_per_rev + 1 + +h = sm.symbols('h', real=True) instance_constraints = ( - q1.replace(t, 0*h) - q1_0, # crank starts horizontal - q2.replace(t, 0*h) - q2_0, # foot/pedal aligned with crank + # set the initial configuration + q1.replace(t, 0*h) - q1_0, + q2.replace(t, 0*h) - q2_0, + # May not be necessary to set q3 and q4, since the holonomic constraint is + # enforced. q3.replace(t, 0*h) - q3_0, q4.replace(t, 0*h) - q4_0, - q1.replace(t, (num_nodes - 1)*h) + crank_revs*2*np.pi, # travel number of revolutions + # TODO : Not sure why we can't start from a standstill, constraint doesn't + # hold ever. #u1.replace(t, 0*h), # start stationary u2.replace(t, 0*h), # start stationary u3.replace(t, 0*h), # start stationary @@ -407,24 +449,32 @@ def gradient(free): knee_bot_mus.a.replace(t, 0*h), ankle_top_mus.a.replace(t, 0*h), ankle_bot_mus.a.replace(t, 0*h), + # at final time travel number of revolutions + q1.replace(t, (num_nodes - 1)*h) + crank_revs*2*np.pi, ) +# %% +# Only allow forward pedaling and limit the joint angle range of motion. All +# muscle excitations should be bound between 0 and 1. bounds = { - q1: (-crank_revs*2*np.pi, 0.0), # can only pedal forward - # ankle angle, q3=-np.pi/4: ankle maximally plantar flexed, q3=-3*pi/2: + q1: (-(crank_revs + 2)*2*np.pi, 0.0), # can only pedal forward + # ankle angle, q3=-130 deg: ankle maximally plantar flexed, q3=-20 deg: # ankle maximally dorsiflexed - q3: (-np.pi/2, -np.pi/4), + q3: (-np.deg2rad(100.0), -np.deg2rad(40.)), # knee angle, q4 = 0: upper and lower leg aligned, q4 = pi/2: knee is # flexed 90 degs q4: (0.0, 3*np.pi/2), - u1: (-20.0, 0.0), # about 200 rpm + u1: (-30.0, 0.0), # about 300 rpm ankle_bot_mus.e: (0.0, 1.0), ankle_top_mus.e: (0.0, 1.0), knee_bot_mus.e: (0.0, 1.0), knee_top_mus.e: (0.0, 1.0), - h: (0.0, 0.2), + h: (0.0, 0.1), } +# % +# Instantiate the Optimal Control Problem +# ======================================= problem = Problem( obj, gradient, @@ -438,6 +488,7 @@ def gradient(free): #integration_method='midpoint', ) problem.add_option('nlp_scaling_method', 'gradient-based') +problem.add_option('max_iter', 1000) # segmentation fault if I set initial guess to zero initial_guess = np.random.random(problem.num_free) @@ -452,18 +503,25 @@ def gradient(free): initial_guess[4*num_nodes:5*num_nodes] = u1_guess initial_guess[5*num_nodes:6*num_nodes] = u2_guess initial_guess[8*num_nodes:] = 0.5 # e -initial_guess[-1] = 0.1 +initial_guess[-1] = 0.01 + +problem.plot_trajectories(initial_guess) +# %% solution, info = problem.solve(initial_guess) +xs, us, ps, h_val= parse_free(solution, len(state_vars), 4, num_nodes, + variable_duration=True) +# %% problem.plot_objective_value() -problem.plot_constraint_violations(solution) -problem.plot_trajectories(solution) -xs, us, ps, h_val= parse_free(solution, len(state_vars), 4, num_nodes, - variable_duration=True) +# %% +problem.plot_constraint_violations(solution) +# %% +problem.plot_trajectories(solution) +# %% def plot_sim_compact(): fig, axes = plt.subplots(4, 1, sharex=True) time = np.linspace(0, num_nodes*h_val, num=num_nodes) @@ -479,6 +537,7 @@ def plot_sim_compact(): plot_sim_compact() +# %% plot_points = [P1, P2, P7, P3, P4, P6, P1] coordinates = P1.pos_from(P1).to_matrix(N) for Pi in plot_points[1:]: @@ -491,6 +550,8 @@ def plot_sim_compact(): mus_coordinates = mus_coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) eval_mus_coordinates = sm.lambdify((q, p), mus_coordinates) +title_template = 'Time = {:1.2f} s' + def plot_configuration(q_vals, p_vals, ax=None): if ax is None: @@ -498,6 +559,8 @@ def plot_configuration(q_vals, p_vals, ax=None): x, y, _ = eval_coordinates(q_vals, p_vals) xm, ym, _ = eval_mus_coordinates(q_vals, p_vals) + xm = np.hstack((xm[:4], np.nan, xm[4:])) + ym = np.hstack((ym[:4], np.nan, ym[4:])) leg_lines, = ax.plot(x, y, 'o-') mus_lines, = ax.plot(xm, ym, 'o-', color='red',) crank_circle = plt.Circle((0.0, 0.0), par_map[lc], fill=False, @@ -506,21 +569,28 @@ def plot_configuration(q_vals, p_vals, ax=None): fill=False) ax.add_patch(crank_circle) ax.add_patch(knee_circle) + title_text = ax.set_title(title_template.format(0.0)) ax.set_aspect('equal') - return ax, fig, leg_lines, mus_lines, knee_circle + return ax, fig, leg_lines, mus_lines, knee_circle, title_text plot_configuration(q_ext, p_vals) -ax, fig, leg_lines, mus_lines, knee_circle = plot_configuration(q_0, p_vals) + +# %% +ax, fig, leg_lines, mus_lines, knee_circle, title_text = \ + plot_configuration(q_0, p_vals) def animate(i): qi = xs[0:4, i] x, y, _ = eval_coordinates(qi, p_vals) xm, ym, _ = eval_mus_coordinates(qi, p_vals) + xm = np.hstack((xm[:4], np.nan, xm[4:])) + ym = np.hstack((ym[:4], np.nan, ym[4:])) leg_lines.set_data(x, y) mus_lines.set_data(xm, ym) knee_circle.set_center((x[4], y[4])) + title_text.set_text('Time = {:1.2f} s'.format(i*h_val)) ani = animation.FuncAnimation(fig, animate, num_nodes, diff --git a/opty/utils.py b/opty/utils.py index b76428d..d1ee953 100644 --- a/opty/utils.py +++ b/opty/utils.py @@ -780,3 +780,107 @@ def sum_of_sines(sigma, frequencies, time): sines_double_prime -= amplitude * w**2 * np.sin(w * time + p) return sines, sines_prime, sines_double_prime + + +class _ExtensorPathway(me.PathwayBase): + def __init__(self, origin, insertion, axis_point, axis, parent_axis, + child_axis, radius, coordinate): + """A custom pathway that wraps a circular arc around a pin joint. + This is intended to be used for extensor muscles. For example, a + triceps wrapping around the elbow joint to extend the upper arm at + the elbow. + + Parameters + ========== + origin : Point + Muscle origin point fixed on the parent body (A). + insertion : Point + Muscle insertion point fixed on the child body (B). + axis_point : Point + Pin joint location fixed in both the parent and child. + axis : Vector + Pin joint rotation axis. + parent_axis : Vector + Axis fixed in the parent frame (A) that is directed from the pin + joint point to the muscle origin point. + child_axis : Vector + Axis fixed in the child frame (B) that is directed from the pin + joint point to the muscle insertion point. + radius : sympyfiable + Radius of the arc that the muscle wraps around. + coordinate : sympfiable function of time + Joint angle, zero when parent and child frames align. Positive + rotation about the pin joint axis, B with respect to A. + + Notes + ===== + Only valid for coordinate >= 0. + + """ + super().__init__(origin, insertion) + self.origin = origin + self.insertion = insertion + self.axis_point = axis_point + self.axis = axis.normalize() + self.parent_axis = parent_axis.normalize() + self.child_axis = child_axis.normalize() + self.radius = radius + self.coordinate = coordinate + self.origin_distance = axis_point.pos_from(origin).magnitude() + self.insertion_distance = axis_point.pos_from(insertion).magnitude() + self.origin_angle = sm.asin(self.radius/self.origin_distance) + self.insertion_angle = sm.asin(self.radius/self.insertion_distance) + + @property + def length(self): + """Length of the pathway. + Length of two fixed length line segments and a changing arc length + of a circle. + """ + angle = self.origin_angle + self.coordinate + self.insertion_angle + arc_length = self.radius*angle + origin_segment_length = self.origin_distance*sm.cos(self.origin_angle) + insertion_segment_length = self.insertion_distance*sm.cos( + self.insertion_angle) + return origin_segment_length + arc_length + insertion_segment_length + + @property + def extension_velocity(self): + """Extension velocity of the pathway. + Arc length of circle is the only thing that changes when the elbow + flexes and extends. + """ + return self.radius*self.coordinate.diff(me.dynamicsymbols._t) + + def to_loads(self, force_magnitude): + """Loads in the correct format to be supplied to `KanesMethod`. + Forces applied to origin, insertion, and P from the muscle wrapped + over circular arc of radius r. + """ + parent_tangency_point = me.Point('Aw') # fixed in parent + child_tangency_point = me.Point('Bw') # fixed in child + parent_tangency_point.set_pos( + self.axis_point, + -self.radius*sm.cos(self.origin_angle)*self.parent_axis.cross( + self.axis) + + self.radius*sm.sin(self.origin_angle)*self.parent_axis, + ) + child_tangency_point.set_pos( + self.axis_point, + self.radius*sm.cos(self.insertion_angle)*self.child_axis.cross( + self.axis) + + self.radius*sm.sin(self.insertion_angle)*self.child_axis), + parent_force_direction_vector = self.origin.pos_from( + parent_tangency_point) + child_force_direction_vector = self.insertion.pos_from( + child_tangency_point) + force_on_parent = (force_magnitude* + parent_force_direction_vector.normalize()) + force_on_child = (force_magnitude* + child_force_direction_vector.normalize()) + loads = [ + me.Force(self.origin, force_on_parent), + me.Force(self.axis_point, -(force_on_parent + force_on_child)), + me.Force(self.insertion, force_on_child), + ] + return loads From d98954ed8726afeff07824d89fd1eaa9742f0d3a Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 06:23:32 +0200 Subject: [PATCH 16/40] Half the muscle slack and opt lengths, increase samples per revolution. --- examples-gallery/plot_one_legged_time_trial.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index bb990f4..98c38ee 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -397,10 +397,10 @@ def gradient(free): eval_knee_top_len = sm.lambdify((q, p), knee_top_pathway.length) eval_knee_bot_len = sm.lambdify((q, p), knee_bot_pathway.length) # length of muscle path when fully extended -par_map[ankle_top_mus.l_T_slack] = eval_ankle_top_len(q_ext, p_vals) -par_map[ankle_bot_mus.l_T_slack] = eval_ankle_bot_len(q_ext, p_vals) -par_map[knee_top_mus.l_T_slack] = eval_knee_top_len(q_ext, p_vals) -par_map[knee_bot_mus.l_T_slack] = eval_knee_bot_len(q_ext, p_vals) +par_map[ankle_top_mus.l_T_slack] = eval_ankle_top_len(q_ext, p_vals)/2 +par_map[ankle_bot_mus.l_T_slack] = eval_ankle_bot_len(q_ext, p_vals)/2 +par_map[knee_top_mus.l_T_slack] = eval_knee_top_len(q_ext, p_vals)/2 +par_map[knee_bot_mus.l_T_slack] = eval_knee_bot_len(q_ext, p_vals)/2 par_map[ankle_top_mus.l_M_opt] = par_map[ankle_top_mus.l_T_slack] + 0.01 par_map[ankle_bot_mus.l_M_opt] = par_map[ankle_bot_mus.l_T_slack] + 0.01 par_map[knee_top_mus.l_M_opt] = par_map[knee_top_mus.l_T_slack] + 0.01 @@ -426,7 +426,7 @@ def gradient(free): # Crank revolutions are proportional to distance traveled so the race distance # is defined by number of crank revolutions. crank_revs = 10 -samples_per_rev = 50 +samples_per_rev = 80 num_nodes = crank_revs*samples_per_rev + 1 h = sm.symbols('h', real=True) From 11dfd44e79efe7d585587a5319b33e31fa85ff0a Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 06:24:43 +0200 Subject: [PATCH 17/40] Move Extensor back into example file. --- .../plot_one_legged_time_trial.py | 107 +++++++++++++++++- opty/utils.py | 104 ----------------- 2 files changed, 106 insertions(+), 105 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 98c38ee..132e6c3 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -17,7 +17,6 @@ """ from opty.direct_collocation import Problem from opty.utils import parse_free -from opty.utils import _ExtensorPathway as ExtensorPathway from scipy.optimize import fsolve import matplotlib.animation as animation import matplotlib.pyplot as plt @@ -233,6 +232,112 @@ # We lump all the muscles that contribute to joint torques at the knee and # ankle into four simplified muscles. The quadriceps wrap over the knee. The # other muscles act on linear pathways. + + +class _ExtensorPathway(me.PathwayBase): + def __init__(self, origin, insertion, axis_point, axis, parent_axis, + child_axis, radius, coordinate): + """A custom pathway that wraps a circular arc around a pin joint. + This is intended to be used for extensor muscles. For example, a + triceps wrapping around the elbow joint to extend the upper arm at + the elbow. + + Parameters + ========== + origin : Point + Muscle origin point fixed on the parent body (A). + insertion : Point + Muscle insertion point fixed on the child body (B). + axis_point : Point + Pin joint location fixed in both the parent and child. + axis : Vector + Pin joint rotation axis. + parent_axis : Vector + Axis fixed in the parent frame (A) that is directed from the pin + joint point to the muscle origin point. + child_axis : Vector + Axis fixed in the child frame (B) that is directed from the pin + joint point to the muscle insertion point. + radius : sympyfiable + Radius of the arc that the muscle wraps around. + coordinate : sympfiable function of time + Joint angle, zero when parent and child frames align. Positive + rotation about the pin joint axis, B with respect to A. + + Notes + ===== + Only valid for coordinate >= 0. + + """ + super().__init__(origin, insertion) + self.origin = origin + self.insertion = insertion + self.axis_point = axis_point + self.axis = axis.normalize() + self.parent_axis = parent_axis.normalize() + self.child_axis = child_axis.normalize() + self.radius = radius + self.coordinate = coordinate + self.origin_distance = axis_point.pos_from(origin).magnitude() + self.insertion_distance = axis_point.pos_from(insertion).magnitude() + self.origin_angle = sm.asin(self.radius/self.origin_distance) + self.insertion_angle = sm.asin(self.radius/self.insertion_distance) + + @property + def length(self): + """Length of the pathway. + Length of two fixed length line segments and a changing arc length + of a circle. + """ + angle = self.origin_angle + self.coordinate + self.insertion_angle + arc_length = self.radius*angle + origin_segment_length = self.origin_distance*sm.cos(self.origin_angle) + insertion_segment_length = self.insertion_distance*sm.cos( + self.insertion_angle) + return origin_segment_length + arc_length + insertion_segment_length + + @property + def extension_velocity(self): + """Extension velocity of the pathway. + Arc length of circle is the only thing that changes when the elbow + flexes and extends. + """ + return self.radius*self.coordinate.diff(me.dynamicsymbols._t) + + def to_loads(self, force_magnitude): + """Loads in the correct format to be supplied to `KanesMethod`. + Forces applied to origin, insertion, and P from the muscle wrapped + over circular arc of radius r. + """ + parent_tangency_point = me.Point('Aw') # fixed in parent + child_tangency_point = me.Point('Bw') # fixed in child + parent_tangency_point.set_pos( + self.axis_point, + -self.radius*sm.cos(self.origin_angle)*self.parent_axis.cross( + self.axis) + + self.radius*sm.sin(self.origin_angle)*self.parent_axis, + ) + child_tangency_point.set_pos( + self.axis_point, + self.radius*sm.cos(self.insertion_angle)*self.child_axis.cross( + self.axis) + + self.radius*sm.sin(self.insertion_angle)*self.child_axis), + parent_force_direction_vector = self.origin.pos_from( + parent_tangency_point) + child_force_direction_vector = self.insertion.pos_from( + child_tangency_point) + force_on_parent = (force_magnitude* + parent_force_direction_vector.normalize()) + force_on_child = (force_magnitude* + child_force_direction_vector.normalize()) + loads = [ + me.Force(self.origin, force_on_parent), + me.Force(self.axis_point, -(force_on_parent + force_on_child)), + me.Force(self.insertion, force_on_child), + ] + return loads + + knee_top_pathway = ExtensorPathway(P9, P5, P4, C.z, -C.x, D.x, rk, q4) knee_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_top') knee_top_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_top', diff --git a/opty/utils.py b/opty/utils.py index d1ee953..b76428d 100644 --- a/opty/utils.py +++ b/opty/utils.py @@ -780,107 +780,3 @@ def sum_of_sines(sigma, frequencies, time): sines_double_prime -= amplitude * w**2 * np.sin(w * time + p) return sines, sines_prime, sines_double_prime - - -class _ExtensorPathway(me.PathwayBase): - def __init__(self, origin, insertion, axis_point, axis, parent_axis, - child_axis, radius, coordinate): - """A custom pathway that wraps a circular arc around a pin joint. - This is intended to be used for extensor muscles. For example, a - triceps wrapping around the elbow joint to extend the upper arm at - the elbow. - - Parameters - ========== - origin : Point - Muscle origin point fixed on the parent body (A). - insertion : Point - Muscle insertion point fixed on the child body (B). - axis_point : Point - Pin joint location fixed in both the parent and child. - axis : Vector - Pin joint rotation axis. - parent_axis : Vector - Axis fixed in the parent frame (A) that is directed from the pin - joint point to the muscle origin point. - child_axis : Vector - Axis fixed in the child frame (B) that is directed from the pin - joint point to the muscle insertion point. - radius : sympyfiable - Radius of the arc that the muscle wraps around. - coordinate : sympfiable function of time - Joint angle, zero when parent and child frames align. Positive - rotation about the pin joint axis, B with respect to A. - - Notes - ===== - Only valid for coordinate >= 0. - - """ - super().__init__(origin, insertion) - self.origin = origin - self.insertion = insertion - self.axis_point = axis_point - self.axis = axis.normalize() - self.parent_axis = parent_axis.normalize() - self.child_axis = child_axis.normalize() - self.radius = radius - self.coordinate = coordinate - self.origin_distance = axis_point.pos_from(origin).magnitude() - self.insertion_distance = axis_point.pos_from(insertion).magnitude() - self.origin_angle = sm.asin(self.radius/self.origin_distance) - self.insertion_angle = sm.asin(self.radius/self.insertion_distance) - - @property - def length(self): - """Length of the pathway. - Length of two fixed length line segments and a changing arc length - of a circle. - """ - angle = self.origin_angle + self.coordinate + self.insertion_angle - arc_length = self.radius*angle - origin_segment_length = self.origin_distance*sm.cos(self.origin_angle) - insertion_segment_length = self.insertion_distance*sm.cos( - self.insertion_angle) - return origin_segment_length + arc_length + insertion_segment_length - - @property - def extension_velocity(self): - """Extension velocity of the pathway. - Arc length of circle is the only thing that changes when the elbow - flexes and extends. - """ - return self.radius*self.coordinate.diff(me.dynamicsymbols._t) - - def to_loads(self, force_magnitude): - """Loads in the correct format to be supplied to `KanesMethod`. - Forces applied to origin, insertion, and P from the muscle wrapped - over circular arc of radius r. - """ - parent_tangency_point = me.Point('Aw') # fixed in parent - child_tangency_point = me.Point('Bw') # fixed in child - parent_tangency_point.set_pos( - self.axis_point, - -self.radius*sm.cos(self.origin_angle)*self.parent_axis.cross( - self.axis) - + self.radius*sm.sin(self.origin_angle)*self.parent_axis, - ) - child_tangency_point.set_pos( - self.axis_point, - self.radius*sm.cos(self.insertion_angle)*self.child_axis.cross( - self.axis) - + self.radius*sm.sin(self.insertion_angle)*self.child_axis), - parent_force_direction_vector = self.origin.pos_from( - parent_tangency_point) - child_force_direction_vector = self.insertion.pos_from( - child_tangency_point) - force_on_parent = (force_magnitude* - parent_force_direction_vector.normalize()) - force_on_child = (force_magnitude* - child_force_direction_vector.normalize()) - loads = [ - me.Force(self.origin, force_on_parent), - me.Force(self.axis_point, -(force_on_parent + force_on_child)), - me.Force(self.insertion, force_on_child), - ] - return loads From c9ab61ed07603a83427725240a445c216f26e7d2 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 06:56:50 +0200 Subject: [PATCH 18/40] Install SymPy dev in CI (remove later). --- .github/workflows/docs.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index f0d99fa..62dbf8f 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -31,5 +31,7 @@ jobs: python-version: ${{ matrix.python-version }} - name: Build Docs run: | + # TODO : Remove when SymPy 1.13 comes out. + python -m pip install -e https://github.com/sympy/sympy.git cd docs make html From b6236359806ef8a32b133680844bf3c70ce4aff1 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 07:50:58 +0200 Subject: [PATCH 19/40] Fix pip install. --- .github/workflows/docs.yml | 2 +- examples-gallery/plot_one_legged_time_trial.py | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index 62dbf8f..46b515e 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -32,6 +32,6 @@ jobs: - name: Build Docs run: | # TODO : Remove when SymPy 1.13 comes out. - python -m pip install -e https://github.com/sympy/sympy.git + python -m pip install -e git+https://github.com/sympy/sympy.git cd docs make html diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 132e6c3..2f6a223 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -234,7 +234,7 @@ # other muscles act on linear pathways. -class _ExtensorPathway(me.PathwayBase): +class ExtensorPathway(me.PathwayBase): def __init__(self, origin, insertion, axis_point, axis, parent_axis, child_axis, radius, coordinate): """A custom pathway that wraps a circular arc around a pin joint. @@ -593,7 +593,7 @@ def gradient(free): #integration_method='midpoint', ) problem.add_option('nlp_scaling_method', 'gradient-based') -problem.add_option('max_iter', 1000) +problem.add_option('max_iter', 400) # segmentation fault if I set initial guess to zero initial_guess = np.random.random(problem.num_free) @@ -627,6 +627,8 @@ def gradient(free): problem.plot_trajectories(solution) # %% + + def plot_sim_compact(): fig, axes = plt.subplots(4, 1, sharex=True) time = np.linspace(0, num_nodes*h_val, num=num_nodes) From 5c37152a1ad6fcdc9e14569501f0c85f1c8492b0 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 08:03:49 +0200 Subject: [PATCH 20/40] Set ankle limits to match estimates from my own ankle motion. --- examples-gallery/plot_one_legged_time_trial.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 2f6a223..310be59 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -563,9 +563,9 @@ def gradient(free): # muscle excitations should be bound between 0 and 1. bounds = { q1: (-(crank_revs + 2)*2*np.pi, 0.0), # can only pedal forward - # ankle angle, q3=-130 deg: ankle maximally plantar flexed, q3=-20 deg: + # ankle angle, q3=-105 deg: ankle maximally plantar flexed, q3=-30 deg: # ankle maximally dorsiflexed - q3: (-np.deg2rad(100.0), -np.deg2rad(40.)), + q3: (-np.deg2rad(105.0), -np.deg2rad(30.)), # knee angle, q4 = 0: upper and lower leg aligned, q4 = pi/2: knee is # flexed 90 degs q4: (0.0, 3*np.pi/2), From e12159a2a335129f3a114490761779a363ec0d85 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 08:04:16 +0200 Subject: [PATCH 21/40] Try another pip install git command. --- .github/workflows/docs.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index 46b515e..9fa31e8 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -32,6 +32,6 @@ jobs: - name: Build Docs run: | # TODO : Remove when SymPy 1.13 comes out. - python -m pip install -e git+https://github.com/sympy/sympy.git + python -m pip install --no-deps git+https://github.com/sympy/sympy.git cd docs make html From f600eedc79b2373c6994d41ac54a0eaf87e925b3 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 10:39:09 +0200 Subject: [PATCH 22/40] Fix u1 guesses. --- examples-gallery/plot_one_legged_time_trial.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 310be59..986364b 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -593,16 +593,16 @@ def gradient(free): #integration_method='midpoint', ) problem.add_option('nlp_scaling_method', 'gradient-based') -problem.add_option('max_iter', 400) +problem.add_option('max_iter', 100) # segmentation fault if I set initial guess to zero initial_guess = np.random.random(problem.num_free) q1_guess = np.linspace(0.0, -crank_revs*2*np.pi, num=num_nodes) q2_guess = np.linspace(0.0, crank_revs*2*np.pi, num=num_nodes) -u1_guess = np.linspace(0.0, -6.0, num=num_nodes) -u1_guess[num_nodes//2:] = -3.0 -u2_guess = np.linspace(0.0, 6.0, num=num_nodes) -u2_guess[num_nodes//2:] = 3.0 +u1_guess = np.linspace(0.0, -20.0, num=num_nodes) +u1_guess[num_nodes//2:] = -20.0 +u2_guess = np.linspace(0.0, 40.0, num=num_nodes) +u2_guess[num_nodes//2:] = 20.0 initial_guess[0*num_nodes:1*num_nodes] = q1_guess initial_guess[1*num_nodes:2*num_nodes] = q2_guess initial_guess[4*num_nodes:5*num_nodes] = u1_guess From b45c09f4f1130050e6630bcceea479a4ca06faeb Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 10:44:14 +0200 Subject: [PATCH 23/40] One more fix for the u1 guess. --- examples-gallery/plot_one_legged_time_trial.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 986364b..d1acbf0 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -597,12 +597,15 @@ def gradient(free): # segmentation fault if I set initial guess to zero initial_guess = np.random.random(problem.num_free) + q1_guess = np.linspace(0.0, -crank_revs*2*np.pi, num=num_nodes) q2_guess = np.linspace(0.0, crank_revs*2*np.pi, num=num_nodes) -u1_guess = np.linspace(0.0, -20.0, num=num_nodes) + +u1_guess = np.linspace(0.0, -40.0, num=num_nodes) u1_guess[num_nodes//2:] = -20.0 u2_guess = np.linspace(0.0, 40.0, num=num_nodes) u2_guess[num_nodes//2:] = 20.0 + initial_guess[0*num_nodes:1*num_nodes] = q1_guess initial_guess[1*num_nodes:2*num_nodes] = q2_guess initial_guess[4*num_nodes:5*num_nodes] = u1_guess From ecbc25ef649c1dd3812a3b08ad446172ee4e1d69 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 12:11:58 +0200 Subject: [PATCH 24/40] Fixed the solution! Needed higher max muscle forces. - Plotted forces and muscle pathway lengths and saw the ankle was drawing 800N which I had set the max to 100N for the ankle. - Raised all force maxes to 1000N and a smooth solution appears at 1000 iterations. - Still draws 1500N so more tuning on muscles needed. --- .../plot_one_legged_time_trial.py | 47 ++++++++++++++----- 1 file changed, 34 insertions(+), 13 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index d1acbf0..1693ad4 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -72,10 +72,11 @@ # - :math:`\rho`: density of air # - :math:`A_r`: frontal area of bicycle and cyclist ls, lc, lf, ll, lu = sm.symbols('ls, lc, lf, ll, lu', real=True, positive=True) -lam, g, rk = sm.symbols('lam, g, rk', real=True) -mA, mB, mC, mD = sm.symbols('mA, mB, mC, mD') -IAzz, IBzz, ICzz, IDzz = sm.symbols('IAzz, IBzz, ICzz, IDzz') -J, m, rw, G, Cr, CD, rho, Ar = sm.symbols('J, m, rw, G, Cr, CD, rho, Ar') +lam, g, rk = sm.symbols('lam, g, rk', real=True, nonnegative=True) +mA, mB, mC, mD = sm.symbols('mA, mB, mC, mD', nonnegative=True) +IAzz, IBzz, ICzz, IDzz = sm.symbols('IAzz, IBzz, ICzz, IDzz', nonnegative=True) +J, m, rw, G, Cr, CD, rho, Ar = sm.symbols('J, m, rw, G, Cr, CD, rho, Ar', + nonnegative=True) # %% # Reference Frames @@ -467,16 +468,16 @@ def gradient(free): rho: 1.204, # kg/m^3, air density rk: 0.04, # m, knee radius rw: 0.3, # m, wheel radius - ankle_bot_mus.F_M_max: 100.0, + ankle_bot_mus.F_M_max: 1000.0, ankle_bot_mus.l_M_opt: np.nan, ankle_bot_mus.l_T_slack: np.nan, - ankle_top_mus.F_M_max: 100.0, + ankle_top_mus.F_M_max: 1000.0, ankle_top_mus.l_M_opt: np.nan, ankle_top_mus.l_T_slack: np.nan, - knee_bot_mus.F_M_max: 300.0, + knee_bot_mus.F_M_max: 1000.0, knee_bot_mus.l_M_opt: np.nan, knee_bot_mus.l_T_slack: np.nan, - knee_top_mus.F_M_max: 600.0, + knee_top_mus.F_M_max: 1000.0, knee_top_mus.l_M_opt: np.nan, knee_top_mus.l_T_slack: np.nan, } @@ -497,6 +498,7 @@ def gradient(free): x0=np.deg2rad([-100.0, 20.0])) q_ext = np.array([q1_ext, q2_ext, q3_ext, q4_ext]) +# TODO : duplicated below eval_ankle_top_len = sm.lambdify((q, p), ankle_top_pathway.length) eval_ankle_bot_len = sm.lambdify((q, p), ankle_bot_pathway.length) eval_knee_top_len = sm.lambdify((q, p), knee_top_pathway.length) @@ -530,7 +532,7 @@ def gradient(free): # Crank revolutions are proportional to distance traveled so the race distance # is defined by number of crank revolutions. -crank_revs = 10 +crank_revs = 4 samples_per_rev = 80 num_nodes = crank_revs*samples_per_rev + 1 @@ -593,7 +595,7 @@ def gradient(free): #integration_method='midpoint', ) problem.add_option('nlp_scaling_method', 'gradient-based') -problem.add_option('max_iter', 100) +problem.add_option('max_iter', 1000) # segmentation fault if I set initial guess to zero initial_guess = np.random.random(problem.num_free) @@ -631,18 +633,37 @@ def gradient(free): # %% +eval_mus_forces = sm.lambdify((state_vars, p), + (ankle_bot_mus.force.doit().xreplace(qd_repl), + ankle_top_mus.force.doit().xreplace(qd_repl), + knee_bot_mus.force.doit().xreplace(qd_repl), + knee_top_mus.force.doit().xreplace(qd_repl)), + cse=True) +akb_force, akt_force, knb_force, knt_force = eval_mus_forces(xs, p_vals) + +eval_mus_lens = sm.lambdify((state_vars, p), + (ankle_bot_mus.pathway.length.doit().xreplace(qd_repl), + ankle_top_mus.pathway.length.doit().xreplace(qd_repl), + knee_bot_mus.pathway.length.doit().xreplace(qd_repl), + knee_top_mus.pathway.length.doit().xreplace(qd_repl)), + cse=True) +akb_len, akt_len, knb_len, knt_len = eval_mus_lens(xs, p_vals) + def plot_sim_compact(): - fig, axes = plt.subplots(4, 1, sharex=True) + fig, axes = plt.subplots(5, 1, sharex=True) time = np.linspace(0, num_nodes*h_val, num=num_nodes) - axes[0].plot(time, xs[0:4, :].T) - axes[0].legend(q) + axes[0].plot(time, akb_force, time, akt_force, time, knb_force, time, + knt_force) + axes[0].legend(['Ankle Bottom', 'Ankle Top', 'Knee Bottom', 'Knee Top']) axes[1].plot(time, -xs[4, :]*60/2/np.pi, time, xs[5, :]*60/2/np.pi) axes[1].legend(['Cadence', 'Pedal Cadence']) axes[2].plot(time, us[0:2, :].T) axes[2].legend(problem.collocator.unknown_input_trajectories[0:2]) axes[3].plot(time, us[2:4, :].T) axes[3].legend(problem.collocator.unknown_input_trajectories[2:4]) + axes[4].plot(time, akb_len, time, akt_len, time, knb_len, time, knt_len) + axes[4].legend(['Ankle Bottom', 'Ankle Top', 'Knee Bottom', 'Knee Top']) plot_sim_compact() From 7969b13a4d9c7f0d76ee790901a37bf6e12ae73d Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 12:22:25 +0200 Subject: [PATCH 25/40] First optimal solution found and it is from a standstill. --- examples-gallery/plot_one_legged_time_trial.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 1693ad4..ed50199 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -468,10 +468,10 @@ def gradient(free): rho: 1.204, # kg/m^3, air density rk: 0.04, # m, knee radius rw: 0.3, # m, wheel radius - ankle_bot_mus.F_M_max: 1000.0, + ankle_bot_mus.F_M_max: 1600.0, ankle_bot_mus.l_M_opt: np.nan, ankle_bot_mus.l_T_slack: np.nan, - ankle_top_mus.F_M_max: 1000.0, + ankle_top_mus.F_M_max: 1600.0, ankle_top_mus.l_M_opt: np.nan, ankle_top_mus.l_T_slack: np.nan, knee_bot_mus.F_M_max: 1000.0, @@ -548,7 +548,7 @@ def gradient(free): q4.replace(t, 0*h) - q4_0, # TODO : Not sure why we can't start from a standstill, constraint doesn't # hold ever. - #u1.replace(t, 0*h), # start stationary + u1.replace(t, 0*h), # start stationary u2.replace(t, 0*h), # start stationary u3.replace(t, 0*h), # start stationary u4.replace(t, 0*h), # start stationary @@ -595,7 +595,7 @@ def gradient(free): #integration_method='midpoint', ) problem.add_option('nlp_scaling_method', 'gradient-based') -problem.add_option('max_iter', 1000) +problem.add_option('max_iter', 3000) # segmentation fault if I set initial guess to zero initial_guess = np.random.random(problem.num_free) From fb4a08ae724f55927436426e1fffb4afe5f1e057 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 13:10:10 +0200 Subject: [PATCH 26/40] Reduce iteration for docs. --- examples-gallery/plot_one_legged_time_trial.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index ed50199..c21e718 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -442,7 +442,8 @@ def gradient(free): # Define Numerical Constants # ========================== -# body segment inertia from https://nbviewer.org/github/pydy/pydy-tutorial-human-standing/blob/master/notebooks/n07_simulation.ipynb +# body segment inertia from: +# https://nbviewer.org/github/pydy/pydy-tutorial-human-standing/blob/master/notebooks/n07_simulation.ipynb par_map = { Ar: 0.55, # m^2, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike CD: 1.15, # unitless, Tab 5.1, pg 188 Wilson 2004, Upright commuting bike @@ -533,7 +534,7 @@ def gradient(free): # Crank revolutions are proportional to distance traveled so the race distance # is defined by number of crank revolutions. crank_revs = 4 -samples_per_rev = 80 +samples_per_rev = 60 num_nodes = crank_revs*samples_per_rev + 1 h = sm.symbols('h', real=True) @@ -592,10 +593,9 @@ def gradient(free): known_parameter_map=par_map, instance_constraints=instance_constraints, bounds=bounds, - #integration_method='midpoint', ) problem.add_option('nlp_scaling_method', 'gradient-based') -problem.add_option('max_iter', 3000) +problem.add_option('max_iter', 1000) # segmentation fault if I set initial guess to zero initial_guess = np.random.random(problem.num_free) From 62f71bc95c3ae0a8d10c374d969dad1d902117ee Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 13:19:03 +0200 Subject: [PATCH 27/40] Set thumbnail plot and make layout better on plot. --- examples-gallery/plot_one_legged_time_trial.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index c21e718..d29cfc7 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -651,7 +651,7 @@ def gradient(free): def plot_sim_compact(): - fig, axes = plt.subplots(5, 1, sharex=True) + fig, axes = plt.subplots(5, 1, sharex=True, layout='constrained') time = np.linspace(0, num_nodes*h_val, num=num_nodes) axes[0].plot(time, akb_force, time, akt_force, time, knb_force, time, knt_force) @@ -705,6 +705,7 @@ def plot_configuration(q_vals, p_vals, ax=None): return ax, fig, leg_lines, mus_lines, knee_circle, title_text +# sphinx_gallery_thumbnail_number = 6 plot_configuration(q_ext, p_vals) # %% From 87763a65827eac51f3be339933b8fdd916fac3f1 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 15:23:52 +0200 Subject: [PATCH 28/40] Improvements to the plots. --- .../plot_one_legged_time_trial.py | 67 ++++++++++++------- 1 file changed, 44 insertions(+), 23 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index d29cfc7..593fea5 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -462,7 +462,7 @@ def gradient(free): ls: 0.8, # seat tube length [m] lu: 0.424, # upper_leg_length [m], m: 85.0, # kg - #mA: 0.0, # not in eom + # mA: 0.0, # not in eom mB: 1.0, # foot mass [kg] guess TODO mC: 6.769, # lower_leg_mass [kg] mD: 17.01, # upper_leg_mass [kg], @@ -534,7 +534,7 @@ def gradient(free): # Crank revolutions are proportional to distance traveled so the race distance # is defined by number of crank revolutions. crank_revs = 4 -samples_per_rev = 60 +samples_per_rev = 40 num_nodes = crank_revs*samples_per_rev + 1 h = sm.symbols('h', real=True) @@ -612,7 +612,7 @@ def gradient(free): initial_guess[1*num_nodes:2*num_nodes] = q2_guess initial_guess[4*num_nodes:5*num_nodes] = u1_guess initial_guess[5*num_nodes:6*num_nodes] = u2_guess -initial_guess[8*num_nodes:] = 0.5 # e +initial_guess[-4*num_nodes - 1:] = 0.5 # e initial_guess[-1] = 0.01 problem.plot_trajectories(initial_guess) @@ -621,6 +621,8 @@ def gradient(free): solution, info = problem.solve(initial_guess) xs, us, ps, h_val= parse_free(solution, len(state_vars), 4, num_nodes, variable_duration=True) +print('Optimal value h:', solution[-1]) +print(info['status_msg']) # %% problem.plot_objective_value() @@ -632,7 +634,6 @@ def gradient(free): problem.plot_trajectories(solution) # %% - eval_mus_forces = sm.lambdify((state_vars, p), (ankle_bot_mus.force.doit().xreplace(qd_repl), ankle_top_mus.force.doit().xreplace(qd_repl), @@ -642,31 +643,50 @@ def gradient(free): akb_force, akt_force, knb_force, knt_force = eval_mus_forces(xs, p_vals) eval_mus_lens = sm.lambdify((state_vars, p), - (ankle_bot_mus.pathway.length.doit().xreplace(qd_repl), - ankle_top_mus.pathway.length.doit().xreplace(qd_repl), - knee_bot_mus.pathway.length.doit().xreplace(qd_repl), - knee_top_mus.pathway.length.doit().xreplace(qd_repl)), + (ankle_bot_mus.pathway.length.xreplace(qd_repl), + ankle_top_mus.pathway.length.xreplace(qd_repl), + knee_bot_mus.pathway.length.xreplace(qd_repl), + knee_top_mus.pathway.length.xreplace(qd_repl)), cse=True) akb_len, akt_len, knb_len, knt_len = eval_mus_lens(xs, p_vals) def plot_sim_compact(): - fig, axes = plt.subplots(5, 1, sharex=True, layout='constrained') + time = np.linspace(0, num_nodes*h_val, num=num_nodes) - axes[0].plot(time, akb_force, time, akt_force, time, knb_force, time, - knt_force) - axes[0].legend(['Ankle Bottom', 'Ankle Top', 'Knee Bottom', 'Knee Top']) - axes[1].plot(time, -xs[4, :]*60/2/np.pi, time, xs[5, :]*60/2/np.pi) - axes[1].legend(['Cadence', 'Pedal Cadence']) - axes[2].plot(time, us[0:2, :].T) - axes[2].legend(problem.collocator.unknown_input_trajectories[0:2]) - axes[3].plot(time, us[2:4, :].T) - axes[3].legend(problem.collocator.unknown_input_trajectories[2:4]) - axes[4].plot(time, akb_len, time, akt_len, time, knb_len, time, knt_len) - axes[4].legend(['Ankle Bottom', 'Ankle Top', 'Knee Bottom', 'Knee Top']) + + fig, axes = plt.subplots(5, 1, sharex=True, layout='constrained') + + axes[0].set_title('Finish time = {:1.3f}'.format(time[-1])) + axes[0].plot(time, akb_force, + time, akt_force, + time, knb_force, + time, knt_force) + axes[0].set_ylabel('Force\n[N]') + # axes[0].legend(['Ankle Bottom', 'Ankle Top', 'Knee Bottom', 'Knee Top']) + + axes[1].plot(time, akb_len, time, akt_len, time, knb_len, time, knt_len) + axes[1].legend(['Ankle Bottom', 'Ankle Top', 'Knee Bottom', 'Knee Top']) + axes[1].set_ylabel('Length\n[M]') + + axes[2].plot(time, -xs[4, :]*60/2/np.pi, time, xs[5, :]*60/2/np.pi) + axes[2].set_ylabel('Cadence\n[RPM]') + axes[2].legend(['Cadence', 'Pedal Cadence']) + + axes[3].plot(time, us[0:2, :].T) + axes[3].legend(problem.collocator.unknown_input_trajectories[0:2]) + axes[3].set_ylabel('Excitation') + + axes[4].plot(time, us[2:4, :].T) + axes[4].legend(problem.collocator.unknown_input_trajectories[2:4]) + axes[4].set_ylabel('Excitation') + + axes[-1].set_xlabel('Time [s]') + + return axes -plot_sim_compact() +_ = plot_sim_compact() # %% plot_points = [P1, P2, P7, P3, P4, P6, P1] @@ -706,7 +726,7 @@ def plot_configuration(q_vals, p_vals, ax=None): # sphinx_gallery_thumbnail_number = 6 -plot_configuration(q_ext, p_vals) +_ = plot_configuration(q_ext, p_vals) # %% ax, fig, leg_lines, mus_lines, knee_circle, title_text = \ @@ -728,4 +748,5 @@ def animate(i): ani = animation.FuncAnimation(fig, animate, num_nodes, interval=int(h_val*1000)) -plt.show() +if __name__ == "__main__": + plt.show() From fb9cf4c31675a6095d288effd1e1b09288aa199b Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sun, 16 Jun 2024 17:08:30 +0200 Subject: [PATCH 29/40] Removed comments, added a couple of headers. --- examples-gallery/plot_one_legged_time_trial.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 593fea5..4e37339 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -543,12 +543,8 @@ def gradient(free): # set the initial configuration q1.replace(t, 0*h) - q1_0, q2.replace(t, 0*h) - q2_0, - # May not be necessary to set q3 and q4, since the holonomic constraint is - # enforced. q3.replace(t, 0*h) - q3_0, q4.replace(t, 0*h) - q4_0, - # TODO : Not sure why we can't start from a standstill, constraint doesn't - # hold ever. u1.replace(t, 0*h), # start stationary u2.replace(t, 0*h), # start stationary u3.replace(t, 0*h), # start stationary @@ -597,7 +593,6 @@ def gradient(free): problem.add_option('nlp_scaling_method', 'gradient-based') problem.add_option('max_iter', 1000) -# segmentation fault if I set initial guess to zero initial_guess = np.random.random(problem.num_free) q1_guess = np.linspace(0.0, -crank_revs*2*np.pi, num=num_nodes) @@ -689,6 +684,8 @@ def plot_sim_compact(): _ = plot_sim_compact() # %% +# Plot Configuration +# ================== plot_points = [P1, P2, P7, P3, P4, P6, P1] coordinates = P1.pos_from(P1).to_matrix(N) for Pi in plot_points[1:]: @@ -729,6 +726,8 @@ def plot_configuration(q_vals, p_vals, ax=None): _ = plot_configuration(q_ext, p_vals) # %% +# Animation +# ========= ax, fig, leg_lines, mus_lines, knee_circle, title_text = \ plot_configuration(q_0, p_vals) From a20622286e518972c5c3c20e5ae52dd9007a587b Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Thu, 20 Jun 2024 06:42:40 +0200 Subject: [PATCH 30/40] Fixed changes. --- examples-gallery/plot_one_legged_time_trial.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 4e37339..e785c60 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -359,12 +359,14 @@ def to_loads(self, force_magnitude): ankle_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('ankle_bot', ankle_bot_pathway, ankle_bot_act) +ankle_damping_B = me.Torque(B, 0.0*u3*B.z) +ankle_damping_C = me.Torque(C, -0.0*u3*B.z) loads = ( knee_top_mus.to_loads() + knee_bot_mus.to_loads() + ankle_top_mus.to_loads() + ankle_bot_mus.to_loads() + - [resistance, gravB, gravC, gravD] + [ankle_damping_B, ankle_damping_C, resistance, gravB, gravC, gravD] ) # %% @@ -534,7 +536,7 @@ def gradient(free): # Crank revolutions are proportional to distance traveled so the race distance # is defined by number of crank revolutions. crank_revs = 4 -samples_per_rev = 40 +samples_per_rev = 80 num_nodes = crank_revs*samples_per_rev + 1 h = sm.symbols('h', real=True) From 27f1d1822ac95a38d24a821ba6389d34f969e378 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Thu, 20 Jun 2024 08:37:51 +0200 Subject: [PATCH 31/40] Added some ankle damping and a better way to skip points in the plot lines. --- examples-gallery/plot_one_legged_time_trial.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index e785c60..5578956 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -359,8 +359,8 @@ def to_loads(self, force_magnitude): ankle_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('ankle_bot', ankle_bot_pathway, ankle_bot_act) -ankle_damping_B = me.Torque(B, 0.0*u3*B.z) -ankle_damping_C = me.Torque(C, -0.0*u3*B.z) +ankle_damping_B = me.Torque(B, 20.0*u3*B.z) +ankle_damping_C = me.Torque(C, -20.0*u3*B.z) loads = ( knee_top_mus.to_loads() + knee_bot_mus.to_loads() + @@ -536,7 +536,7 @@ def gradient(free): # Crank revolutions are proportional to distance traveled so the race distance # is defined by number of crank revolutions. crank_revs = 4 -samples_per_rev = 80 +samples_per_rev = 120 num_nodes = crank_revs*samples_per_rev + 1 h = sm.symbols('h', real=True) @@ -694,10 +694,14 @@ def plot_sim_compact(): coordinates = coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) eval_coordinates = sm.lambdify((q, p), coordinates) -mus_points = [P7, P8, P2, P8, P9, P6] +mus_points = [P7, P8, P2, P8, 'nan', P9, P6] mus_coordinates = P7.pos_from(P1).to_matrix(N) for Pi in mus_points[1:]: - mus_coordinates = mus_coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) + if Pi == 'nan': + pi_coord = sm.Matrix([sm.nan, sm.nan, sm.nan]) + else: + pi_coord = Pi.pos_from(P1).to_matrix(N) + mus_coordinates = mus_coordinates.row_join(pi_coord) eval_mus_coordinates = sm.lambdify((q, p), mus_coordinates) title_template = 'Time = {:1.2f} s' @@ -709,8 +713,6 @@ def plot_configuration(q_vals, p_vals, ax=None): x, y, _ = eval_coordinates(q_vals, p_vals) xm, ym, _ = eval_mus_coordinates(q_vals, p_vals) - xm = np.hstack((xm[:4], np.nan, xm[4:])) - ym = np.hstack((ym[:4], np.nan, ym[4:])) leg_lines, = ax.plot(x, y, 'o-') mus_lines, = ax.plot(xm, ym, 'o-', color='red',) crank_circle = plt.Circle((0.0, 0.0), par_map[lc], fill=False, From d3133c1f8e387fdd2b255ffa0c321bbb02f80371 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 29 Jun 2024 16:11:59 +0200 Subject: [PATCH 32/40] Corrected some variables in the text and lowered ankle flexiion max force. --- .../plot_one_legged_time_trial.py | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 5578956..a2fb065 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -36,7 +36,7 @@ # - :math:`u_1`: crank angular rate (cadence) # - :math:`u_2`: pedal angular rate # - :math:`u_3`: ankle angular rate -# - :math:`u_4`: knee anglular rate +# - :math:`u_4`: knee angular rate t = me.dynamicsymbols._t q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4', real=True) u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4', real=True) @@ -59,12 +59,12 @@ # - :math:`m_B`: mass of foot and pedal # - :math:`m_C`: mass of lower leg # - :math:`m_D`: mass of upper leg -# - :math:`I_{Axx}`: moment of inertia of crank -# - :math:`I_{Bxx}`: moment of inertia of foot and pedal -# - :math:`I_{Cxx}`: moment of inertia of lower leg -# - :math:`I_{Dxx}`: moment of inertia of upper leg -# - :math:`J`: rotational moment of inertia of the bicycle wheel -# - :math:`m`: mass of the bicycle and cyclist +# - :math:`I_{Azz}`: moment of inertia of crank +# - :math:`I_{Bzz}`: moment of inertia of foot and pedal +# - :math:`I_{Czz}`: moment of inertia of lower leg +# - :math:`I_{Dzz}`: moment of inertia of upper leg +# - :math:`J`: rotational moment of inertia of a single bicycle wheel +# - :math:`m`: combined mass of the bicycle and cyclist # - :math:`r_w`: wheel radius # - :math:`G`: gear ratio between crank and wheel # - :math:`C_r`: coefficient of rolling resistance @@ -90,7 +90,7 @@ N, A, B, C, D = sm.symbols('N, A, B, C, D', cls=me.ReferenceFrame) A.orient_axis(N, N.z, q1) # crank angle -B.orient_axis(A, A.z, q2) # pedal/foot angle +B.orient_axis(A, A.z, q2) # pedal angle C.orient_axis(B, B.z, q3) # ankle angle D.orient_axis(C, C.z, q4) # knee angle @@ -111,6 +111,10 @@ # - :math:`P_7` : heel # - :math:`P_8` : knee muscle lower leg insertion point # - :math:`P_9` : ankle muscle lower leg insertion point +# - :math:`A_o` : mass center of the crank +# - :math:`B_o` : mass center of the pedal and foot +# - :math:`C_o` : mass center of the lower leg +# - :math:`D_o` : mass center of the upper leg P1, P2, P3, P4, P5, P6, P7, P8, P9 = sm.symbols( 'P1, P2, P3, P4, P5, P6, P7, P8, P9', cls=me.Point) Ao, Bo, Co, Do = sm.symbols('Ao, Bo, Co, Do', cls=me.Point) @@ -120,10 +124,13 @@ Bo.set_pos(P2, lf/2*B.x) # foot mass center P3.set_pos(P2, lf*B.x) # ankle P7.set_pos(P2, 3*lf/2*B.x) # heel + Co.set_pos(P3, ll/2*C.x) # lower leg mass center P4.set_pos(P3, ll*C.x) # knee + Do.set_pos(P4, lu/2*D.x) # upper leg mass center P5.set_pos(P4, lu*D.x) # hip + P6.set_pos(P1, -ls*sm.cos(lam)*N.x + ls*sm.sin(lam)*N.y) # seat P8.set_pos(P3, ll/6*C.x) P9.set_pos(P4, -2*rk*C.x) @@ -474,7 +481,7 @@ def gradient(free): ankle_bot_mus.F_M_max: 1600.0, ankle_bot_mus.l_M_opt: np.nan, ankle_bot_mus.l_T_slack: np.nan, - ankle_top_mus.F_M_max: 1600.0, + ankle_top_mus.F_M_max: 600.0, ankle_top_mus.l_M_opt: np.nan, ankle_top_mus.l_T_slack: np.nan, knee_bot_mus.F_M_max: 1000.0, From 5b14fe422c429e297200f4bc6cf29a8970af938a Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Fri, 5 Jul 2024 18:45:00 +0200 Subject: [PATCH 33/40] Minor. --- examples-gallery/plot_one_legged_time_trial.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index a2fb065..29f35dd 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -194,7 +194,7 @@ # .. math:: # # (2J + m r_w^2)\dot{\omega} = -# -C_r m g r_w - \sgn \omega \frac{1}{2} \rho C_D A_r (\omega r_w)^2 + +# -C_r m g r_w - \sgn(\omega) \frac{1}{2} \rho C_D A_r (\omega r_w)^2 + # T_w # # The angular speed of the rear wheel is related to the crank cadence by the @@ -203,12 +203,13 @@ # .. math:: # # \omega = G u_1 \\ +# \dot{\omega} = G \dot{u}_1 \\ # G T_w = T_c # # .. math:: # # T_c = -# -(2J + m r_w^2)\dot{u}_1 +# -(2J + m r_w^2)G\dot{u}_1 # - C_r m g r_w G # - \sgn u_1 \frac{1}{2} \rho C_D A_r G (u_1 r_w)^2 # @@ -229,7 +230,8 @@ # resistance should be a posistive torque to resist the negative speed # NOTE : using sm.sign() will break the constraint Jacobian due taking the # derivative of sm.sign(). - (-(2*J + m*rw**2)*G**2*u1.diff() + Cr*m*g*rw*G + + (-(2*J + m*rw**2)*G**2*u1.diff() + + Cr*m*g*rw*G + rho*CD*Ar*G**3*rw**3*u1**2/2)*N.z, ) From b2e1983982f2ab9468f616e380b16127cb3b2062 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Fri, 5 Jul 2024 21:18:13 +0200 Subject: [PATCH 34/40] Only single underline. --- examples-gallery/plot_one_legged_time_trial.py | 1 - 1 file changed, 1 deletion(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 29f35dd..029ac0a 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -1,5 +1,4 @@ """ -============================= One-Legged Cycling Time Trial ============================= From a920817abde9865cbdc5de546c3a47864d0475d9 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 6 Jul 2024 12:31:19 +0200 Subject: [PATCH 35/40] Tidying. --- .../plot_one_legged_time_trial.py | 149 ++++++++++++------ 1 file changed, 98 insertions(+), 51 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 029ac0a..aeb9135 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -6,9 +6,8 @@ resistance mimic the torque felt if accelerating the whole bicycle with rider on flat ground. -The goal will be travel a specific distance in the shortest amount of time -given that the leg muscles have to coordinate and can only be excited from 0 to -1. +The objective is to travel a specific distance in the shortest amount of time +given that the leg muscles have to coordinate. Second goal will then be to solve for crank length and seat height that gives optimal performance. @@ -26,7 +25,7 @@ # %% # Coordinates -# =========== +# ----------- # # - :math:`q_1`: crank angle # - :math:`q_2`: pedal angle @@ -44,7 +43,7 @@ # %% # Constants -# ========= +# --------- # # - :math:`l_s`: seat tube length # - :math:`l_c`: crank length @@ -54,6 +53,7 @@ # - :math:`\lambda`: seat tube angle # - :math:`g`: acceleration due to gravity # - :math:`r_k`: knee wrapping radius +# - :math:`c`: torsional viscous damping coefficient # - :math:`m_A`: mass of crank # - :math:`m_B`: mass of foot and pedal # - :math:`m_C`: mass of lower leg @@ -71,7 +71,7 @@ # - :math:`\rho`: density of air # - :math:`A_r`: frontal area of bicycle and cyclist ls, lc, lf, ll, lu = sm.symbols('ls, lc, lf, ll, lu', real=True, positive=True) -lam, g, rk = sm.symbols('lam, g, rk', real=True, nonnegative=True) +lam, g, rk, c = sm.symbols('lam, g, rk, c', real=True, nonnegative=True) mA, mB, mC, mD = sm.symbols('mA, mB, mC, mD', nonnegative=True) IAzz, IBzz, ICzz, IDzz = sm.symbols('IAzz, IBzz, ICzz, IDzz', nonnegative=True) J, m, rw, G, Cr, CD, rho, Ar = sm.symbols('J, m, rw, G, Cr, CD, rho, Ar', @@ -79,7 +79,7 @@ # %% # Reference Frames -# ================ +# ---------------- # # - :math:`N`: inertial reference frame for leg dynamics # - :math:`A`: crank @@ -100,7 +100,8 @@ # %% # Point Kinematics -# ================ +# ---------------- +# # - :math:`P_1` : crank center # - :math:`P_2` : pedal center # - :math:`P_3` : ankle @@ -152,7 +153,7 @@ # %% # Holonomic Constraints -# ===================== +# --------------------- # # The leg forms a kinematic loop and two holonomic constraints arise from this # loop. @@ -163,6 +164,8 @@ mocon = me.msubs(holonomic.diff(t), qd_repl) # %% +# Inertia and Rigid Bodies +# ------------------------ IA = me.Inertia.from_inertia_scalars(Ao, A, 0, 0, IAzz) IB = me.Inertia.from_inertia_scalars(Bo, B, 0, 0, IBzz) IC = me.Inertia.from_inertia_scalars(Co, C, 0, 0, ICzz) @@ -177,14 +180,16 @@ # %% # Forces -# ====== +# ------ +# +# Gravity acts on each leg body segment. gravB = me.Force(Bo, -mB*g*N.y) gravC = me.Force(Co, -mC*g*N.y) gravD = me.Force(Do, -mD*g*N.y) # %% # Crank Resistance -# ---------------- +# ^^^^^^^^^^^^^^^^ # # We model the resistance torque at the crank to be that which you would feel # when accelerating the bicycle and cyclist along flat ground. The basic @@ -196,6 +201,8 @@ # -C_r m g r_w - \sgn(\omega) \frac{1}{2} \rho C_D A_r (\omega r_w)^2 + # T_w # +# where :math:`T_w` is the rear wheel driving torque. +# # The angular speed of the rear wheel is related to the crank cadence by the # gear ratio :math:`G`: # @@ -205,23 +212,26 @@ # \dot{\omega} = G \dot{u}_1 \\ # G T_w = T_c # +# The torque applied to the crank to drive the vehicle is then: +# # .. math:: # # T_c = -# -(2J + m r_w^2)G\dot{u}_1 -# - C_r m g r_w G -# - \sgn u_1 \frac{1}{2} \rho C_D A_r G (u_1 r_w)^2 +# (2J + m r_w^2)G^2\dot{u}_1 +# + C_r m g r_w G +# + \sgn(u_1) \frac{1}{2} \rho C_D A_r G^3 (u_1 r_w)^2 # -# :math:`u_1 < 0` give forward motion in the pedaling sign convention and we -# will restrict :math:`u_1 < 0` below, so the torque felt at the crank is: +# The :math:`\sgn` function that manages the sign of the drag force has a +# discontinuity and is not differentiable. Since we only want to solve this +# optimal control problem for forward motion we can make the assumption that +# :math:`u_1 < 0`. The torque felt at the crank is then: # # .. math:: # -# T_c = -# - (2J + m r_w^2)\dot{u}_1 +# -T_c = +# -(2J + m r_w^2)G^2\dot{u}_1 # - C_r m g r_w G -# + \frac{1}{2} \rho C_D A_r G (u_1 r_w)^2 -# TODO : Fix equations written above, they are not correct. +# + \frac{1}{2} \rho C_D A_r G^3 (u_1 r_w)^2 resistance = me.Torque( crank, @@ -229,27 +239,26 @@ # resistance should be a posistive torque to resist the negative speed # NOTE : using sm.sign() will break the constraint Jacobian due taking the # derivative of sm.sign(). - (-(2*J + m*rw**2)*G**2*u1.diff() + + (-(2*J + m*rw**2)*G**2*u1.diff() - Cr*m*g*rw*G + rho*CD*Ar*G**3*rw**3*u1**2/2)*N.z, ) # %% # Muscles -# ------- +# ^^^^^^^ # # We lump all the muscles that contribute to joint torques at the knee and # ankle into four simplified muscles. The quadriceps wrap over the knee. The -# other muscles act on linear pathways. +# other three muscle groups act on linear pathways. class ExtensorPathway(me.PathwayBase): def __init__(self, origin, insertion, axis_point, axis, parent_axis, child_axis, radius, coordinate): - """A custom pathway that wraps a circular arc around a pin joint. - This is intended to be used for extensor muscles. For example, a - triceps wrapping around the elbow joint to extend the upper arm at - the elbow. + """A custom pathway that wraps a circular arc around a pin joint. This + is intended to be used for extensor muscles. For example, a triceps + wrapping around the elbow joint to extend the upper arm at the elbow. Parameters ========== @@ -367,15 +376,13 @@ def to_loads(self, force_magnitude): ankle_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('ankle_bot', ankle_bot_pathway, ankle_bot_act) -ankle_damping_B = me.Torque(B, 20.0*u3*B.z) -ankle_damping_C = me.Torque(C, -20.0*u3*B.z) -loads = ( - knee_top_mus.to_loads() + - knee_bot_mus.to_loads() + - ankle_top_mus.to_loads() + - ankle_bot_mus.to_loads() + - [ankle_damping_B, ankle_damping_C, resistance, gravB, gravC, gravD] -) +# %% +# Joint Viscous Damping +# ^^^^^^^^^^^^^^^^^^^^^ +# The high stiffness in the ankle joint can be tamed some by adding some +# viscous damping in the ankle joint. +ankle_damping_B = me.Torque(B, c*u3*B.z) +ankle_damping_C = me.Torque(C, -c*u3*B.z) # %% # Form the Equations of Motion @@ -392,7 +399,17 @@ def to_loads(self, force_magnitude): u_dependent=(u3, u4), constraint_solver='CRAMER', ) + bodies = (crank, foot, lower_leg, upper_leg) + +loads = ( + knee_top_mus.to_loads() + + knee_bot_mus.to_loads() + + ankle_top_mus.to_loads() + + ankle_bot_mus.to_loads() + + [ankle_damping_B, ankle_damping_C, resistance, gravB, gravC, gravD] +) + Fr, Frs = kane.kanes_equations(bodies, loads) muscle_diff_eq = sm.Matrix([ @@ -402,6 +419,25 @@ def to_loads(self, force_magnitude): ankle_bot_mus.a.diff() - ankle_bot_mus.rhs()[0, 0], ]) +# %% +# +# The full equations of motion in implicit form are made up of the 4 +# kinematical differential equations, :math:`\mathbf{f}_k`, the 2 dynamical +# differential equations, :math:`\mathbf{f}_d`, the 4 musculo-tendon +# differential equations, :math:`\mathbf{f}_a`, and the 2 holonomic constraint +# equations, :math:`{f}_h`. +# +# .. math:: +# +# \begin{bmatrix} +# \mathbf{f}_k \\ +# \mathbf{f}_d \\ +# \mathbf{f}_a \\ +# \mathbf{f}_h +# \end{bmatrix} +# = +# 0 + eom = kindiff.col_join(Fr + Frs).col_join(muscle_diff_eq).col_join(holonomic) state_vars = ( @@ -479,6 +515,7 @@ def gradient(free): rho: 1.204, # kg/m^3, air density rk: 0.04, # m, knee radius rw: 0.3, # m, wheel radius + c: 30.0, # joint viscous damping [Nms] ankle_bot_mus.F_M_max: 1600.0, ankle_bot_mus.l_M_opt: np.nan, ankle_bot_mus.l_T_slack: np.nan, @@ -502,7 +539,7 @@ def gradient(free): # lower leg then calculate the muscle pathway lengths in this configuration. q1_ext = -par_map[lam] # aligned with seat post -q2_ext = 3*np.pi/2 # foot perpendicular to crank +q2_ext = 3.0*np.pi/2.0 # foot perpendicular to crank eval_holonomic = sm.lambdify((q, p), holonomic) q3_ext, q4_ext = fsolve(lambda x: eval_holonomic([q1_ext, q2_ext, x[0], x[1]], p_vals).squeeze(), @@ -531,8 +568,8 @@ def gradient(free): # ==================== # # The cyclist should start with no motion (stationary) and in an initial -# configuration with the crank forward and horizontal (q1=0 deg) and the toe -# forward and foot parallel to the crank (q2=180 deg). +# configuration with the crank forward and horizontal (:math:`q_1=0` deg) and +# the toe forward and foot parallel to the crank (:math:`q_2=180` deg). q1_0 = 0.0 q2_0 = np.pi eval_holonomic = sm.lambdify((q, p), holonomic) @@ -543,9 +580,10 @@ def gradient(free): # Crank revolutions are proportional to distance traveled so the race distance # is defined by number of crank revolutions. -crank_revs = 4 +distance = 10.0 # meters +crank_revs = distance/par_map[rw]/par_map[G]/2.0/np.pi # revolutions samples_per_rev = 120 -num_nodes = crank_revs*samples_per_rev + 1 +num_nodes = int(crank_revs*samples_per_rev) + 1 h = sm.symbols('h', real=True) @@ -555,10 +593,12 @@ def gradient(free): q2.replace(t, 0*h) - q2_0, q3.replace(t, 0*h) - q3_0, q4.replace(t, 0*h) - q4_0, - u1.replace(t, 0*h), # start stationary - u2.replace(t, 0*h), # start stationary - u3.replace(t, 0*h), # start stationary - u4.replace(t, 0*h), # start stationary + # start stationary + u1.replace(t, 0*h), + u2.replace(t, 0*h), + u3.replace(t, 0*h), + u4.replace(t, 0*h), + # start with no muscle activation knee_top_mus.a.replace(t, 0*h), knee_bot_mus.a.replace(t, 0*h), ankle_top_mus.a.replace(t, 0*h), @@ -623,6 +663,8 @@ def gradient(free): problem.plot_trajectories(initial_guess) # %% +# Solve the Optimal Control Problem +# ================================= solution, info = problem.solve(initial_guess) xs, us, ps, h_val= parse_free(solution, len(state_vars), 4, num_nodes, variable_duration=True) @@ -630,6 +672,8 @@ def gradient(free): print(info['status_msg']) # %% +# Plot the Solution +# ================= problem.plot_objective_value() # %% @@ -660,7 +704,7 @@ def plot_sim_compact(): time = np.linspace(0, num_nodes*h_val, num=num_nodes) - fig, axes = plt.subplots(5, 1, sharex=True, layout='constrained') + fig, axes = plt.subplots(6, 1, sharex=True, layout='constrained') axes[0].set_title('Finish time = {:1.3f}'.format(time[-1])) axes[0].plot(time, akb_force, @@ -678,14 +722,17 @@ def plot_sim_compact(): axes[2].set_ylabel('Cadence\n[RPM]') axes[2].legend(['Cadence', 'Pedal Cadence']) - axes[3].plot(time, us[0:2, :].T) - axes[3].legend(problem.collocator.unknown_input_trajectories[0:2]) - axes[3].set_ylabel('Excitation') + axes[3].plot(time, -xs[4, :]*par_map[G]*par_map[rw]*3.6) + axes[3].set_ylabel('Speed [km/h]') - axes[4].plot(time, us[2:4, :].T) - axes[4].legend(problem.collocator.unknown_input_trajectories[2:4]) + axes[4].plot(time, us[0:2, :].T) + axes[4].legend(problem.collocator.unknown_input_trajectories[0:2]) axes[4].set_ylabel('Excitation') + axes[5].plot(time, us[2:4, :].T) + axes[5].legend(problem.collocator.unknown_input_trajectories[2:4]) + axes[5].set_ylabel('Excitation') + axes[-1].set_xlabel('Time [s]') return axes From ddd147781a9905f47526dc6a0627f8f2567ed8f4 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 6 Jul 2024 15:26:12 +0200 Subject: [PATCH 36/40] Let sphinx gallery pick up svgs and have it remove the sphinx gallery comment configs. --- docs/conf.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/conf.py b/docs/conf.py index 94bfd9b..1395018 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -97,6 +97,8 @@ 'examples_dirs': os.path.join(REPO_DIR, 'examples-gallery'), 'gallery_dirs': 'examples', 'matplotlib_animations': True, + 'copyfile_regex': r'.*\.svg', + 'remove_config_comments': True, } # -- Options for HTML output ---------------------------------------------- From d39241510d9759d6da74ea142ce8547b7f1237d5 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 6 Jul 2024 15:26:52 +0200 Subject: [PATCH 37/40] Do not control figsize in the Problem plots. --- opty/direct_collocation.py | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/opty/direct_collocation.py b/opty/direct_collocation.py index 2a47ba9..02d53e5 100644 --- a/opty/direct_collocation.py +++ b/opty/direct_collocation.py @@ -403,7 +403,6 @@ def plot_trajectories(self, vector, axes=None): if axes is None: fig, axes = plt.subplots(num_axes, 1, sharex=True, - figsize=(6.4, max(4.8, 0.6*num_axes)), layout='compressed') for ax, traj, symbol in zip(axes, trajectories, traj_syms): @@ -416,7 +415,7 @@ def plot_trajectories(self, vector, axes=None): return axes @_optional_plt_dep - def plot_constraint_violations(self, vector): + def plot_constraint_violations(self, vector, axes=None): """Returns an axis with the state constraint violations plotted versus node number and the instance constraints as a bar graph. @@ -451,11 +450,9 @@ def plot_constraint_violations(self, vector): con_nodes = range(1, self.collocator.num_collocation_nodes) plot_inst_viols = self.collocator.instance_constraints is not None - fig, axes = plt.subplots(1 + plot_inst_viols, squeeze=False, - figsize=(max(6.4, - len(instance_violations)*0.4), - 4.8), - layout='compressed') + if axes is None: + fig, axes = plt.subplots(1 + plot_inst_viols, squeeze=False, + layout='compressed') axes = axes.ravel() axes[0].plot(con_nodes, state_violations.T) From 9c318c29d376bc2fa07dabdb4e472656452bf94e Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 6 Jul 2024 15:35:50 +0200 Subject: [PATCH 38/40] First complete draft of the one-legged time trial. --- .../plot_one_legged_time_trial.py | 362 ++++++++++-------- 1 file changed, 201 insertions(+), 161 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index aeb9135..92fe246 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -2,6 +2,8 @@ One-Legged Cycling Time Trial ============================= +.. image:: one-legged-time-trial.svg + Single human leg with four driving lumped muscles. The crank inertia and resistance mimic the torque felt if accelerating the whole bicycle with rider on flat ground. @@ -12,6 +14,10 @@ Second goal will then be to solve for crank length and seat height that gives optimal performance. +.. warning:: + + This example requires SymPy >= 1.13. + """ from opty.direct_collocation import Problem from opty.utils import parse_free @@ -40,6 +46,8 @@ u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4', real=True) q = sm.Matrix([q1, q2, q3, q4]) u = sm.Matrix([u1, u2, u3, u4]) +qd_repl = {q1.diff(t): u1, q2.diff(t): u2, + q3.diff(t): u3, q4.diff(t): u4} # %% # Constants @@ -70,10 +78,14 @@ # - :math:`C_D`: coefficient of drag # - :math:`\rho`: density of air # - :math:`A_r`: frontal area of bicycle and cyclist -ls, lc, lf, ll, lu = sm.symbols('ls, lc, lf, ll, lu', real=True, positive=True) -lam, g, rk, c = sm.symbols('lam, g, rk, c', real=True, nonnegative=True) -mA, mB, mC, mD = sm.symbols('mA, mB, mC, mD', nonnegative=True) -IAzz, IBzz, ICzz, IDzz = sm.symbols('IAzz, IBzz, ICzz, IDzz', nonnegative=True) +ls, lc, lf, ll, lu = sm.symbols('ls, lc, lf, ll, lu', + real=True, positive=True) +lam, g, rk, c = sm.symbols('lam, g, rk, c', + real=True, nonnegative=True) +mA, mB, mC, mD = sm.symbols('mA, mB, mC, mD', + nonnegative=True) +IAzz, IBzz, ICzz, IDzz = sm.symbols('IAzz, IBzz, ICzz, IDzz', + nonnegative=True) J, m, rw, G, Cr, CD, rho, Ar = sm.symbols('J, m, rw, G, Cr, CD, rho, Ar', nonnegative=True) @@ -149,7 +161,7 @@ Do.v2pt_theory(P4, N, D) P5.v2pt_theory(P4, N, D) -kindiff = sm.Matrix([ui - qi.diff() for ui, qi in zip(u, q)]) +kindiff = sm.Matrix([ui - qi.diff(t) for ui, qi in zip(u, q)]) # %% # Holonomic Constraints @@ -158,10 +170,8 @@ # The leg forms a kinematic loop and two holonomic constraints arise from this # loop. holonomic = (P5.pos_from(P1) - P6.pos_from(P1)).to_matrix(N)[:2, :] - -qd_repl = {q1.diff(): u1, q2.diff(): u2, q3.diff(): u3, q4.diff(): u4} - mocon = me.msubs(holonomic.diff(t), qd_repl) +sm.trigsimp(mocon) # %% # Inertia and Rigid Bodies @@ -171,12 +181,14 @@ IC = me.Inertia.from_inertia_scalars(Co, C, 0, 0, ICzz) ID = me.Inertia.from_inertia_scalars(Do, D, 0, 0, IDzz) -crank = me.RigidBody('crank', masscenter=Ao, frame=A, mass=mA, inertia=IA) -foot = me.RigidBody('foot', masscenter=Bo, frame=B, mass=mB, inertia=IB) -lower_leg = me.RigidBody('lower leg', masscenter=Co, frame=C, mass=mC, - inertia=IC) -upper_leg = me.RigidBody('upper leg', masscenter=Do, frame=D, mass=mD, - inertia=ID) +crank = me.RigidBody('crank', masscenter=Ao, frame=A, + mass=mA, inertia=IA) +foot = me.RigidBody('foot', masscenter=Bo, frame=B, + mass=mB, inertia=IB) +lower_leg = me.RigidBody('lower leg', masscenter=Co, frame=C, + mass=mC, inertia=IC) +upper_leg = me.RigidBody('upper leg', masscenter=Do, frame=D, + mass=mD, inertia=ID) # %% # Forces @@ -189,7 +201,7 @@ # %% # Crank Resistance -# ^^^^^^^^^^^^^^^^ +# ~~~~~~~~~~~~~~~~ # # We model the resistance torque at the crank to be that which you would feel # when accelerating the bicycle and cyclist along flat ground. The basic @@ -198,7 +210,8 @@ # .. math:: # # (2J + m r_w^2)\dot{\omega} = -# -C_r m g r_w - \sgn(\omega) \frac{1}{2} \rho C_D A_r (\omega r_w)^2 + +# -C_r m g r_w +# - \operatorname{sgn}(\omega) \frac{1}{2} \rho C_D A_r (\omega r_w)^2 + # T_w # # where :math:`T_w` is the rear wheel driving torque. @@ -219,12 +232,12 @@ # T_c = # (2J + m r_w^2)G^2\dot{u}_1 # + C_r m g r_w G -# + \sgn(u_1) \frac{1}{2} \rho C_D A_r G^3 (u_1 r_w)^2 +# + \operatorname{sgn}(u_1) \frac{1}{2} \rho C_D A_r G^3 (u_1 r_w)^2 # -# The :math:`\sgn` function that manages the sign of the drag force has a -# discontinuity and is not differentiable. Since we only want to solve this -# optimal control problem for forward motion we can make the assumption that -# :math:`u_1 < 0`. The torque felt at the crank is then: +# The :math:`\operatorname{sgn}` function that manages the sign of the drag +# force has a discontinuity and is not differentiable. Since we only want to +# solve this optimal control problem for forward motion we can make the +# assumption that :math:`u_1 < 0`. The torque felt at the crank is then: # # .. math:: # @@ -246,7 +259,7 @@ # %% # Muscles -# ^^^^^^^ +# ~~~~~~~ # # We lump all the muscles that contribute to joint torques at the knee and # ankle into four simplified muscles. The quadriceps wrap over the knee. The @@ -327,23 +340,23 @@ def to_loads(self, force_magnitude): Forces applied to origin, insertion, and P from the muscle wrapped over circular arc of radius r. """ - parent_tangency_point = me.Point('Aw') # fixed in parent - child_tangency_point = me.Point('Bw') # fixed in child - parent_tangency_point.set_pos( + self.parent_tangency_point = me.Point('Aw') # fixed in parent + self.child_tangency_point = me.Point('Bw') # fixed in child + self.parent_tangency_point.set_pos( self.axis_point, -self.radius*sm.cos(self.origin_angle)*self.parent_axis.cross( self.axis) + self.radius*sm.sin(self.origin_angle)*self.parent_axis, ) - child_tangency_point.set_pos( + self.child_tangency_point.set_pos( self.axis_point, self.radius*sm.cos(self.insertion_angle)*self.child_axis.cross( self.axis) + self.radius*sm.sin(self.insertion_angle)*self.child_axis), parent_force_direction_vector = self.origin.pos_from( - parent_tangency_point) + self.parent_tangency_point) child_force_direction_vector = self.insertion.pos_from( - child_tangency_point) + self.child_tangency_point) force_on_parent = (force_magnitude* parent_force_direction_vector.normalize()) force_on_child = (force_magnitude* @@ -357,28 +370,29 @@ def to_loads(self, force_magnitude): knee_top_pathway = ExtensorPathway(P9, P5, P4, C.z, -C.x, D.x, rk, q4) -knee_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_top') -knee_top_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_top', - knee_top_pathway, - knee_top_act) +knee_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults( + 'knee_top') +knee_top_mus = bm.MusculotendonDeGroote2016.with_defaults( + 'knee_top', knee_top_pathway, knee_top_act) knee_bot_pathway = me.LinearPathway(P9, P5) -knee_bot_act = bm.FirstOrderActivationDeGroote2016.with_defaults('knee_bot') -knee_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('knee_bot', - knee_bot_pathway, - knee_bot_act) +knee_bot_act = bm.FirstOrderActivationDeGroote2016.with_defaults( + 'knee_bot') +knee_bot_mus = bm.MusculotendonDeGroote2016.with_defaults( + 'knee_bot', knee_bot_pathway, knee_bot_act) ankle_top_pathway = me.LinearPathway(P8, P2) -ankle_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults('ankle_top') -ankle_top_mus = bm.MusculotendonDeGroote2016.with_defaults('ankle_top', - ankle_top_pathway, - ankle_top_act) +ankle_top_act = bm.FirstOrderActivationDeGroote2016.with_defaults( + 'ankle_top') +ankle_top_mus = bm.MusculotendonDeGroote2016.with_defaults( + 'ankle_top', ankle_top_pathway, ankle_top_act) ankle_bot_pathway = me.LinearPathway(P8, P7) -ankle_bot_act = bm.FirstOrderActivationDeGroote2016.with_defaults('ankle_bot') -ankle_bot_mus = bm.MusculotendonDeGroote2016.with_defaults('ankle_bot', - ankle_bot_pathway, - ankle_bot_act) +ankle_bot_act = bm.FirstOrderActivationDeGroote2016.with_defaults( + 'ankle_bot') +ankle_bot_mus = bm.MusculotendonDeGroote2016.with_defaults( + 'ankle_bot', ankle_bot_pathway, ankle_bot_act) + # %% # Joint Viscous Damping -# ^^^^^^^^^^^^^^^^^^^^^ +# ~~~~~~~~~~~~~~~~~~~~~ # The high stiffness in the ankle joint can be tamed some by adding some # viscous damping in the ankle joint. ankle_damping_B = me.Torque(B, c*u3*B.z) @@ -386,7 +400,7 @@ def to_loads(self, force_magnitude): # %% # Form the Equations of Motion -# ============================ +# ---------------------------- kane = me.KanesMethod( N, @@ -407,7 +421,8 @@ def to_loads(self, force_magnitude): knee_bot_mus.to_loads() + ankle_top_mus.to_loads() + ankle_bot_mus.to_loads() + - [ankle_damping_B, ankle_damping_C, resistance, gravB, gravC, gravD] + [ankle_damping_B, ankle_damping_C, resistance, + gravB, gravC, gravD] ) Fr, Frs = kane.kanes_equations(bodies, loads) @@ -438,7 +453,10 @@ def to_loads(self, force_magnitude): # = # 0 -eom = kindiff.col_join(Fr + Frs).col_join(muscle_diff_eq).col_join(holonomic) +eom = kindiff.col_join( + Fr + Frs).col_join( + muscle_diff_eq).col_join( + holonomic) state_vars = ( q1, q2, q3, q4, u1, u2, u3, u4, @@ -447,10 +465,11 @@ def to_loads(self, force_magnitude): ankle_top_mus.a, ankle_bot_mus.a, ) +state_vars # %% # Objective -# ========= +# --------- # # The objective is to cycle as fast as possible, so we need to find the minmal # time duration for a fixed distance (or more simply crank revolutions). This @@ -486,7 +505,7 @@ def gradient(free): # %% # Define Numerical Constants -# ========================== +# -------------------------- # body segment inertia from: # https://nbviewer.org/github/pydy/pydy-tutorial-human-standing/blob/master/notebooks/n07_simulation.ipynb @@ -516,21 +535,21 @@ def gradient(free): rk: 0.04, # m, knee radius rw: 0.3, # m, wheel radius c: 30.0, # joint viscous damping [Nms] - ankle_bot_mus.F_M_max: 1600.0, + ankle_bot_mus.F_M_max: 1000.0, ankle_bot_mus.l_M_opt: np.nan, ankle_bot_mus.l_T_slack: np.nan, - ankle_top_mus.F_M_max: 600.0, + ankle_top_mus.F_M_max: 400.0, ankle_top_mus.l_M_opt: np.nan, ankle_top_mus.l_T_slack: np.nan, - knee_bot_mus.F_M_max: 1000.0, + knee_bot_mus.F_M_max: 1200.0, knee_bot_mus.l_M_opt: np.nan, knee_bot_mus.l_T_slack: np.nan, - knee_top_mus.F_M_max: 1000.0, + knee_top_mus.F_M_max: 1400.0, knee_top_mus.l_M_opt: np.nan, knee_top_mus.l_T_slack: np.nan, } -p = np.array(list(par_map.keys())) +p = list(par_map.keys()) p_vals = np.array(list(par_map.values())) # %% @@ -546,26 +565,76 @@ def gradient(free): x0=np.deg2rad([-100.0, 20.0])) q_ext = np.array([q1_ext, q2_ext, q3_ext, q4_ext]) -# TODO : duplicated below -eval_ankle_top_len = sm.lambdify((q, p), ankle_top_pathway.length) -eval_ankle_bot_len = sm.lambdify((q, p), ankle_bot_pathway.length) -eval_knee_top_len = sm.lambdify((q, p), knee_top_pathway.length) -eval_knee_bot_len = sm.lambdify((q, p), knee_bot_pathway.length) +eval_mus_lens = sm.lambdify((q, p), + (ankle_bot_mus.pathway.length.xreplace(qd_repl), + ankle_top_mus.pathway.length.xreplace(qd_repl), + knee_bot_mus.pathway.length.xreplace(qd_repl), + knee_top_mus.pathway.length.xreplace(qd_repl)), + cse=True) +akb_len, akt_len, knb_len, knt_len = eval_mus_lens(q_ext, p_vals) # length of muscle path when fully extended -par_map[ankle_top_mus.l_T_slack] = eval_ankle_top_len(q_ext, p_vals)/2 -par_map[ankle_bot_mus.l_T_slack] = eval_ankle_bot_len(q_ext, p_vals)/2 -par_map[knee_top_mus.l_T_slack] = eval_knee_top_len(q_ext, p_vals)/2 -par_map[knee_bot_mus.l_T_slack] = eval_knee_bot_len(q_ext, p_vals)/2 -par_map[ankle_top_mus.l_M_opt] = par_map[ankle_top_mus.l_T_slack] + 0.01 -par_map[ankle_bot_mus.l_M_opt] = par_map[ankle_bot_mus.l_T_slack] + 0.01 -par_map[knee_top_mus.l_M_opt] = par_map[knee_top_mus.l_T_slack] + 0.01 -par_map[knee_bot_mus.l_M_opt] = par_map[knee_bot_mus.l_T_slack] + 0.01 +par_map[ankle_top_mus.l_T_slack] = akt_len/2 +par_map[ankle_bot_mus.l_T_slack] = akb_len/2 +par_map[knee_top_mus.l_T_slack] = knt_len/2 +par_map[knee_bot_mus.l_T_slack] = knb_len/2 +par_map[ankle_top_mus.l_M_opt] = akt_len/2 + 0.01 +par_map[ankle_bot_mus.l_M_opt] = akb_len/2 + 0.01 +par_map[knee_top_mus.l_M_opt] = knt_len/2 + 0.01 +par_map[knee_bot_mus.l_M_opt] = knb_len/2 + 0.01 p_vals = np.array(list(par_map.values())) +# %% +# Plot Extended Configuration +# --------------------------- +plot_points = [P6, P1, P2, P3, P7, P3, P4, P5] +coordinates = P6.pos_from(P1).to_matrix(N) +for Pi in plot_points[1:]: + coordinates = coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) +eval_coordinates = sm.lambdify((q, p), coordinates) + +mus_points = [P7, P8, P2, P8, None, P9, P6, + knee_top_pathway.child_tangency_point, None, + knee_top_pathway.parent_tangency_point, P9] +mus_coordinates = P7.pos_from(P1).to_matrix(N) +for Pi in mus_points[1:]: + if Pi is None: + pi_coord = sm.Matrix([sm.nan, sm.nan, sm.nan]) + else: + pi_coord = Pi.pos_from(P1).to_matrix(N) + mus_coordinates = mus_coordinates.row_join(pi_coord) +eval_mus_coordinates = sm.lambdify((q, p), mus_coordinates) + +title_template = 'Time = {:1.2f} s' + + +def plot_configuration(q_vals, p_vals, ax=None): + if ax is None: + fig, ax = plt.subplots(layout='constrained') + + x, y, _ = eval_coordinates(q_vals, p_vals) + xm, ym, _ = eval_mus_coordinates(q_vals, p_vals) + crank_circle = plt.Circle((0.0, 0.0), par_map[lc], fill=False, + linestyle='--') + bike_lines, = ax.plot(x[:3], y[:3], 'o-', linewidth=2, color='#3dcfc2ff') + leg_lines, = ax.plot(x[2:], y[2:], 'o-', linewidth=4, color='#ffd90fff') + mus_lines, = ax.plot(xm, ym, 'o-', color='#800080ff',) + knee_circle = plt.Circle((x[6], y[6]), par_map[rk], color='#800080ff', + fill=False) + ax.add_patch(crank_circle) + ax.add_patch(knee_circle) + title_text = ax.set_title(title_template.format(0.0)) + ax.set_aspect('equal') + return ax, fig, bike_lines, leg_lines, mus_lines, knee_circle, title_text + + +# sphinx_gallery_thumbnail_number = 1 +_ = plot_configuration(q_ext, p_vals) + + # %% # Instance Constraints -# ==================== +# -------------------- # # The cyclist should start with no motion (stationary) and in an initial # configuration with the crank forward and horizontal (:math:`q_1=0` deg) and @@ -578,6 +647,9 @@ def gradient(free): x0=np.deg2rad([-90.0, 90.0]), xtol=1e-14) q_0 = np.array([q1_0, q2_0, q3_0, q4_0]) +_ = plot_configuration(q_0, p_vals) + +# %% # Crank revolutions are proportional to distance traveled so the race distance # is defined by number of crank revolutions. distance = 10.0 # meters @@ -606,6 +678,7 @@ def gradient(free): # at final time travel number of revolutions q1.replace(t, (num_nodes - 1)*h) + crank_revs*2*np.pi, ) +sm.pprint(instance_constraints) # %% # Only allow forward pedaling and limit the joint angle range of motion. All @@ -625,10 +698,11 @@ def gradient(free): knee_top_mus.e: (0.0, 1.0), h: (0.0, 0.1), } +sm.pprint(bounds) -# % +# %% # Instantiate the Optimal Control Problem -# ======================================= +# --------------------------------------- problem = Problem( obj, gradient, @@ -638,12 +712,13 @@ def gradient(free): h, known_parameter_map=par_map, instance_constraints=instance_constraints, + time_symbol=t, bounds=bounds, ) problem.add_option('nlp_scaling_method', 'gradient-based') -problem.add_option('max_iter', 1000) +problem.add_option('max_iter', 3000) -initial_guess = np.random.random(problem.num_free) +initial_guess = 0.5*np.ones(problem.num_free) q1_guess = np.linspace(0.0, -crank_revs*2*np.pi, num=num_nodes) q2_guess = np.linspace(0.0, crank_revs*2*np.pi, num=num_nodes) @@ -657,68 +732,77 @@ def gradient(free): initial_guess[1*num_nodes:2*num_nodes] = q2_guess initial_guess[4*num_nodes:5*num_nodes] = u1_guess initial_guess[5*num_nodes:6*num_nodes] = u2_guess -initial_guess[-4*num_nodes - 1:] = 0.5 # e initial_guess[-1] = 0.01 -problem.plot_trajectories(initial_guess) +fig, axes = plt.subplots(15, 1, sharex=True, + figsize=(6.4, 0.8*15), + layout='compressed') +problem.plot_trajectories(initial_guess, axes=axes) # %% # Solve the Optimal Control Problem -# ================================= +# --------------------------------- solution, info = problem.solve(initial_guess) -xs, us, ps, h_val= parse_free(solution, len(state_vars), 4, num_nodes, +xs, us, ps, h_val= parse_free(solution, len(state_vars), + 4, num_nodes, variable_duration=True) print('Optimal value h:', solution[-1]) print(info['status_msg']) # %% # Plot the Solution -# ================= +# ----------------- problem.plot_objective_value() # %% -problem.plot_constraint_violations(solution) +fig, axes = plt.subplots(2, figsize=(12.8, 9.6), + layout='constrained') +problem.plot_constraint_violations(solution, axes=axes) # %% -problem.plot_trajectories(solution) +fig, axes = plt.subplots(15, 1, sharex=True, + figsize=(6.4, 0.8*15), + layout='compressed') +problem.plot_trajectories(solution, axes=axes) # %% -eval_mus_forces = sm.lambdify((state_vars, p), - (ankle_bot_mus.force.doit().xreplace(qd_repl), - ankle_top_mus.force.doit().xreplace(qd_repl), - knee_bot_mus.force.doit().xreplace(qd_repl), - knee_top_mus.force.doit().xreplace(qd_repl)), - cse=True) -akb_force, akt_force, knb_force, knt_force = eval_mus_forces(xs, p_vals) - -eval_mus_lens = sm.lambdify((state_vars, p), - (ankle_bot_mus.pathway.length.xreplace(qd_repl), - ankle_top_mus.pathway.length.xreplace(qd_repl), - knee_bot_mus.pathway.length.xreplace(qd_repl), - knee_top_mus.pathway.length.xreplace(qd_repl)), - cse=True) -akb_len, akt_len, knb_len, knt_len = eval_mus_lens(xs, p_vals) +eval_mus_forces = sm.lambdify( + (state_vars, p), + (ankle_bot_mus.force.doit().xreplace(qd_repl), + ankle_top_mus.force.doit().xreplace(qd_repl), + knee_bot_mus.force.doit().xreplace(qd_repl), + knee_top_mus.force.doit().xreplace(qd_repl)), + cse=True) +akb_force, akt_force, knb_force, knt_force = \ + eval_mus_forces(xs, p_vals) + +akb_len, akt_len, knb_len, knt_len = eval_mus_lens(xs[:4], p_vals) def plot_sim_compact(): time = np.linspace(0, num_nodes*h_val, num=num_nodes) - fig, axes = plt.subplots(6, 1, sharex=True, layout='constrained') - - axes[0].set_title('Finish time = {:1.3f}'.format(time[-1])) - axes[0].plot(time, akb_force, - time, akt_force, - time, knb_force, - time, knt_force) - axes[0].set_ylabel('Force\n[N]') - # axes[0].legend(['Ankle Bottom', 'Ankle Top', 'Knee Bottom', 'Knee Top']) - - axes[1].plot(time, akb_len, time, akt_len, time, knb_len, time, knt_len) - axes[1].legend(['Ankle Bottom', 'Ankle Top', 'Knee Bottom', 'Knee Top']) - axes[1].set_ylabel('Length\n[M]') - - axes[2].plot(time, -xs[4, :]*60/2/np.pi, time, xs[5, :]*60/2/np.pi) + fig, axes = plt.subplots(6, 1, sharex=True, layout='constrained', + figsize=(6.4, 9.6)) + + axes[0].set_title('Finish time = {:1.3f} s'.format(time[-1])) + axes[0].plot(time, -akb_force, + time, -akt_force, + time, -knb_force, + time, -knt_force) + axes[0].set_ylabel('Muscle Force\n[N]') + axes[0].legend(['Ankle Bottom', 'Ankle Top', + 'Knee Bottom', 'Knee Top']) + + axes[1].plot(time, akb_len, time, akt_len, + time, knb_len, time, knt_len) + axes[1].legend(['Ankle Bottom', 'Ankle Top', + 'Knee Bottom', 'Knee Top']) + axes[1].set_ylabel('Muscle-tendon\nLength\n[m]') + + axes[2].plot(time, -xs[4, :]*60/2/np.pi, + time, xs[5, :]*60/2/np.pi) axes[2].set_ylabel('Cadence\n[RPM]') axes[2].legend(['Cadence', 'Pedal Cadence']) @@ -740,54 +824,11 @@ def plot_sim_compact(): _ = plot_sim_compact() -# %% -# Plot Configuration -# ================== -plot_points = [P1, P2, P7, P3, P4, P6, P1] -coordinates = P1.pos_from(P1).to_matrix(N) -for Pi in plot_points[1:]: - coordinates = coordinates.row_join(Pi.pos_from(P1).to_matrix(N)) -eval_coordinates = sm.lambdify((q, p), coordinates) - -mus_points = [P7, P8, P2, P8, 'nan', P9, P6] -mus_coordinates = P7.pos_from(P1).to_matrix(N) -for Pi in mus_points[1:]: - if Pi == 'nan': - pi_coord = sm.Matrix([sm.nan, sm.nan, sm.nan]) - else: - pi_coord = Pi.pos_from(P1).to_matrix(N) - mus_coordinates = mus_coordinates.row_join(pi_coord) -eval_mus_coordinates = sm.lambdify((q, p), mus_coordinates) - -title_template = 'Time = {:1.2f} s' - - -def plot_configuration(q_vals, p_vals, ax=None): - if ax is None: - fig, ax = plt.subplots(layout='constrained') - - x, y, _ = eval_coordinates(q_vals, p_vals) - xm, ym, _ = eval_mus_coordinates(q_vals, p_vals) - leg_lines, = ax.plot(x, y, 'o-') - mus_lines, = ax.plot(xm, ym, 'o-', color='red',) - crank_circle = plt.Circle((0.0, 0.0), par_map[lc], fill=False, - linestyle='--') - knee_circle = plt.Circle((x[4], y[4]), par_map[rk], color='red', - fill=False) - ax.add_patch(crank_circle) - ax.add_patch(knee_circle) - title_text = ax.set_title(title_template.format(0.0)) - ax.set_aspect('equal') - return ax, fig, leg_lines, mus_lines, knee_circle, title_text - - -# sphinx_gallery_thumbnail_number = 6 -_ = plot_configuration(q_ext, p_vals) # %% # Animation -# ========= -ax, fig, leg_lines, mus_lines, knee_circle, title_text = \ +# --------- +ax, fig, bike_lines, leg_lines, mus_lines, knee_circle, title_text = \ plot_configuration(q_0, p_vals) @@ -795,16 +836,15 @@ def animate(i): qi = xs[0:4, i] x, y, _ = eval_coordinates(qi, p_vals) xm, ym, _ = eval_mus_coordinates(qi, p_vals) - xm = np.hstack((xm[:4], np.nan, xm[4:])) - ym = np.hstack((ym[:4], np.nan, ym[4:])) - leg_lines.set_data(x, y) + bike_lines.set_data(x[:3], y[:3]) + leg_lines.set_data(x[2:], y[2:]) mus_lines.set_data(xm, ym) - knee_circle.set_center((x[4], y[4])) + knee_circle.set_center((x[6], y[6])) title_text.set_text('Time = {:1.2f} s'.format(i*h_val)) -ani = animation.FuncAnimation(fig, animate, num_nodes, - interval=int(h_val*1000)) +ani = animation.FuncAnimation(fig, animate, range(0, num_nodes, 4), + interval=int(h_val*4000)) if __name__ == "__main__": plt.show() From 5350d5489ede4721fb68cbee4525ed84c603a8cf Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 6 Jul 2024 15:47:47 +0200 Subject: [PATCH 39/40] Correct size of drone plots. --- examples-gallery/plot_drone.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/examples-gallery/plot_drone.py b/examples-gallery/plot_drone.py index f1c9779..deb54db 100644 --- a/examples-gallery/plot_drone.py +++ b/examples-gallery/plot_drone.py @@ -211,7 +211,10 @@ initial_guess[2*num_nodes:3*num_nodes] = xyz_guess initial_guess[-4*num_nodes:] = 10.0 # constant thrust -prob.plot_trajectories(initial_guess) +fig, axes = plt.subplots(18, 1, sharex=True, + figsize=(6.4, 0.8*18), + layout='compressed') +prob.plot_trajectories(initial_guess, axes=axes) # %% # Find an optimal solution. @@ -221,11 +224,16 @@ # %% # Plot the optimal state and input trajectories. -prob.plot_trajectories(solution) +fig, axes = plt.subplots(18, 1, sharex=True, + figsize=(6.4, 0.8*18), + layout='compressed') +prob.plot_trajectories(solution, axes=axes) # %% # Plot the constraint violations. -prob.plot_constraint_violations(solution) +fig, axes = plt.subplots(2, figsize=(12.8, 9.6), + layout='constrained') +prob.plot_constraint_violations(solution, axes=axes) # %% # Plot the objective function as a function of optimizer iteration. From 657cdf2da35f499514a008d5bb1bdb35c2e9bad6 Mon Sep 17 00:00:00 2001 From: "Jason K. Moore" Date: Sat, 6 Jul 2024 16:25:44 +0200 Subject: [PATCH 40/40] Minor adjustments. --- examples-gallery/plot_one_legged_time_trial.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/examples-gallery/plot_one_legged_time_trial.py b/examples-gallery/plot_one_legged_time_trial.py index 92fe246..d081ffb 100644 --- a/examples-gallery/plot_one_legged_time_trial.py +++ b/examples-gallery/plot_one_legged_time_trial.py @@ -698,7 +698,6 @@ def plot_configuration(q_vals, p_vals, ax=None): knee_top_mus.e: (0.0, 1.0), h: (0.0, 0.1), } -sm.pprint(bounds) # %% # Instantiate the Optimal Control Problem @@ -746,7 +745,7 @@ def plot_configuration(q_vals, p_vals, ax=None): xs, us, ps, h_val= parse_free(solution, len(state_vars), 4, num_nodes, variable_duration=True) -print('Optimal value h:', solution[-1]) +print('Optimal value h:', h_val) print(info['status_msg']) # %% @@ -829,7 +828,7 @@ def plot_sim_compact(): # Animation # --------- ax, fig, bike_lines, leg_lines, mus_lines, knee_circle, title_text = \ - plot_configuration(q_0, p_vals) + plot_configuration(xs[:4, 0], p_vals) def animate(i):