Question regarding plot_trajectories

[Peter230655]

I am trying to optimize a problem, where the time t appears explicitly in the EOMs.
So I copied the trick JM used in his example 1.2.3 Parameter identification.
my specified_sysmbols are t1, t2, t3 (torques acting on the ball)
my known_trajectory_map is {T(t): time} EXACTLY like in the example mentioned.

As per the description of plot_trajectories on page 57 I would have expected to get the plots of the state_symbols, followed by the specified_symbols.
However, the first plot after the state_symbols is labeled T(t) - but it seems to show t1.
(T(t) is a linear function, just like in the example.
Where am I wrong with this method?
Thanks for any help!

Below is the program

# %%
import sympy as sm
import sympy.physics.mechanics as me
import numpy as np
from collections import OrderedDict
from opty.direct_collocation import Problem
from opty.utils import parse_free, create_objective_function
from scipy.integrate import solve_ivp
from scipy.optimize import fsolve, minimize
import matplotlib.pyplot as plt
import matplotlib as mp
from mpl_toolkits.mplot3d import axes3d
from IPython.display import HTML
mp.rcParams['animation.embed_limit'] = 2**128
from matplotlib import animation

# %% [markdown]
# A uniform, solid ball with radius r and mass $m_b$ is rolling on a horizontal disc without slipping.\
# The disc starts at rest and speeds up like this: $u_3(t) = \Omega \cdot (1 - e^{-\alpha \cdot t})$, where $alpha > 0$ is a measure of the acceleration and $\Omega$ is the final rotational speed.\
# A torque is applied to the ball, and the goal is to get it to the center of the disc.
# 
# An observer, a particle of mass $m_o$, is attached to the surface the ball. 
# 
# **Constants**
# 
# - $m_b$: mass of the ball [kg]
# - r : radius of the ball [m]
# - $m_o$ : mass of the observer
# - $\Omega$: final rotational speed of the disc around the vertical axis [rad/sec]
# - $\alpha$: measure of the acceleration of the disc [1/sec]
# 
# **States**
# 
# - $q_1, q_2, q_3$: generalized coordinates of the ball w.r.t the disc
# - $u_1, u_2, u_3$: generalized angual velocities of the ball
# - $x, y$: position of the contact point w.r.t. the disc. Dependent states.
# - $ux, uy$: speeds of the contact point w.r.t the disc. Dependent states.
# 
# **Specifieds**
# 
# - $t_1, t_2, t_3$: Torque applied to the ball
# 
# 
# **Additional parameters**
# 
# - N: inertial frame
# - A1: frame fixed to the ball
# - A2: frame fixed to the disc
# 
# - $O$: point fixed in N
# - CP: contact point
# - Dmc: center of the ball
# - $o_{Dmc}$: position of observer
# 

# %%
t = me.dynamicsymbols._t
q1, q2, q3 = me.dynamicsymbols('q_1 q_2 q_3')
u1, u2, u3 = me.dynamicsymbols('u_1 u_2 u_3')
t1, t2, t3 = me.dynamicsymbols('t_1 t_2 t_3')
T = sm.symbols('T', cls=sm.Function)

Tdot, Tdotdot = sm.symbols('Tdot Tdotdot')

x, y = me.dynamicsymbols('x y')
ux, uy = me.dynamicsymbols('ux uy')

mb, mo, g, r = sm.symbols('mb, mo, g, r')
Omega, alpha = sm.symbols('Omega, alpha')

N, A1, A2 = sm.symbols('N, A1 A2', cls=me.ReferenceFrame)
O, CP, Dmc, m_Dmc = sm.symbols('O, CP, Dmc, m_Dmc', cls=me.Point) 
O.set_vel(N, 0)
udisc = Omega * (1 - sm.exp(-alpha * t))

# %% [markdown]
# **Determine the speed of (subsequent) contact points**
# 
# If the ball rotates around the x axis by an angle $q_1$ the contact point will be move by -$q_1 \cdot 2$ in the A2.y direction\
# If the ball rotates around the y axis by an angle $q_2$ the contact point will be move by $q_2 \cdot 2$ in the A2.x direction\
# Hence the coordinate constraints are;
# - $x = r \cdot q_2$
# - $y = -r \cdot q_1$
# 
# so the resulting speed constraints are:
# - $\frac{d}{dt} x = r \cdot \frac{d} {dt}q_2$
# - $\frac{d}{dt} y = -r \cdot \frac{d} {dt}q_1$ 

# %% [markdown]
# **Description of the system**
# 
# The time t appears explicitly in the EOMs.\
# I copy the trick from JM in the example1.2.3 Parameter identification.: declare a function T(t), and then\
# pass the time as known trajectory. My OEMs also contain $\frac{d}{dt} T(t)$ and $\frac{d^2}{dx^2} T(t)$\
# As $T(t) = const \cdot t$ I set these derivatives accordingly.
# 

# %%
udisc = Omega * (1 - sm.exp(-alpha * T(t)))
qdisc = Omega * T(t) + Omega * sm.exp(-alpha * T(t)) / alpha - Omega / alpha
A2.orient_axis(N, qdisc, N.z)
A2.set_ang_vel(N, udisc*N.z)

A1.orient_body_fixed(A2, (q1, q2, q3), '123')
rot = A1.ang_vel_in(N)
A1.set_ang_vel(A2, u1*A1.x + u2*A1.y + u3*A1.z)
rot1 = A1.ang_vel_in(N)

CP.set_pos(O, x*A2.x + y*A2.y)
CP.set_vel(A2, ux*A2.x + uy*A2.y) 

Dmc.set_pos(CP, r*N.z)
Dmc.set_vel(N, Dmc.pos_from(O).diff(t, N))

m_Dmc.set_pos(Dmc, r*A1.z)
m_Dmc.v2pt_theory(Dmc, N, A1)

iXX = 2./5. * mb * r**2
iYY = iXX
iZZ = iXX

I = me.inertia(A1, iXX, iYY, iZZ)
Ball = me.RigidBody('Ball', Dmc, A1, mb, (I, Dmc))
observer = me.Particle('observer', m_Dmc, mo)
BODY = [Ball, observer]

FL = [(Dmc, -mb*g*N.z), (m_Dmc, -mo*g*N.z), (A1, t1*A1.x + t2*A1.y + t3*A1.z)]
kd = sm.Matrix([ux - x.diff(t), uy - y.diff(t), *[(rot - rot1).dot(uv) for uv in N ]])
speed_constr = sm.Matrix([ux - r*u2, uy + r*u1])
hol_constr = sm.Matrix([x - r*q2, y + r*q1])

q_ind = [q1, q2, q3]
q_dep = [x, y]
u_ind = [u1, u2, u3]
u_dep = [ux, uy]

KM = me.KanesMethod(N, 
    q_ind=q_ind,
    q_dependent=q_dep,
    u_ind=u_ind,
    u_dependent=u_dep, 
    kd_eqs=kd,
    velocity_constraints=speed_constr,
    configuration_constraints=hol_constr,
    )

(fr, frstar) = KM.kanes_equations(BODY, FL)
EOM = kd.col_join(fr + frstar)
EOM = me.msubs((EOM.col_join(hol_constr)), {sm.Derivative(T(t), t): Tdot, sm.Derivative(T(t), (t,2)): Tdotdot})
#print('Shape of EOMs', EOM.shape)
#print(f'OEMs contain {sm.count_ops(EOM):,} operations, {sm.count_ops(sm.cse(EOM)):,} after common subexpression elimination')
#print('EOM DS: ', me.find_dynamicsymbols(EOM))
#print('EOM FS', EOM.free_symbols)

# %% [markdown]
# Set up the **optimization problem** and solve it.

# %%
state_symbols = tuple((*q_ind, *q_dep, *u_ind, *u_dep))
constant_symbols = (r, mb, mo, g, Omega, alpha, Tdot, Tdotdot)
specified_symbols = (t1, t2, t3)
unknown_symbols = []
methode = "backward euler"

duration = 10.0
num_nodes = 250

interval_value = duration / (num_nodes - 1)

time = np.linspace(0.0, duration, num_nodes)

# Specify the known system parameters.
par_map = OrderedDict()
par_map[mb]  = 5.0
par_map[mo]  = 1.0
par_map[r]   = 1.0
par_map[Omega] = 5.0
par_map[alpha] = 0.1
par_map[g]   = 9.81
par_map[Tdot] = interval_value
par_map[Tdotdot] = 0.0

objective = sm.Integral(t1**2 + t2**2 + t3**2, t)

# Specify the objective function and it's gradient.
obj, obj_grad = create_objective_function(
    objective, 
    state_symbols, 
    specified_symbols, 
    unknown_symbols,
    num_nodes, 
    interval_value, 
    methode)

# This portion is copied from Timo's code
t0, tf = 0.0, duration

# Opty want instance constraints, basically things like `q2(2.0) - 3.141` to specify that q2 should be 3.141 when t=2.0
# I personally like to rewrite it to initial state and final state constraints and then translate them into the set of equations opty wants.
initial_state_constraints = {
    q1: 0.0, 
    q2: 0.0,
    q3: 0.0,
    u1: 0.0,
    u2: 0.0,
    u3: 0.0,
    x: 5.0,
    y: 5.0,
    ux: 0.0,
    uy: 0.0
    }
final_state_constraints = {
    x: 0,
    y: 0,
    ux: 0,
    uy: 0
    }
instance_constraints = (
) + tuple(
    xi.subs({t: t0}) - xi_val for xi, xi_val in initial_state_constraints.items()
) + tuple(
    xi.subs({t: tf}) - xi_val for xi, xi_val in final_state_constraints.items()
)

grenze = 10.
bounds = {t1: (-grenze, grenze), t2: (-grenze, grenze), t3: (-grenze, grenze)}

map = {T(t): time}
 
# Create an optimization problem.
prob = Problem(
    obj, 
    obj_grad, 
    EOM, 
    state_symbols, 
    num_nodes, 
    interval_value,
    known_parameter_map=par_map,
    known_trajectory_map = {T(t): time},
    instance_constraints=instance_constraints,
    time_symbol=t,
    bounds=bounds,
    )

# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free) * 0.1

# set max number of iterations. Default is 3000.
prob.add_option('max_iter', 3000)

# Find the optimal solution.
for _ in range(1):
    solution, info = prob.solve(initial_guess)
    print('message from optimizer:', info['status_msg'])
    initial_guess = solution

prob.plot_objective_value()
prob.plot_constraint_violations(solution)
prob.plot_trajectories(solution)

[moorepants]

Its a bug, see: #111

[Peter230655]

I did not know this, #111 was initiated before I knew about opty.
Thanks!

[moorepants]

I double checked this and with the current master, it seems to give the expected plots:

[moorepants]

Double check is here: #187

[Peter230655]

YES, looks right!
How do I get the current master?
(I am not yet finished with my shell / git lectures. :-) )

Thanks!

[moorepants]

If you are using git you have to pull in the latest changes with git pull. But this depends on what you named the remote pointing to the main repo. Mine is set up so that I can do git pull upstream master.

[Peter230655]

My 'environenmnts' in in anaconda (I believe), and I am not yet very sure, what anaconda IS.
I better finish my lectures first. :-)