possible bug in Problem. plot_trajectories

[Peter230655]

I tried an optimization without any unknown trajectories, but only an unknown symbol to be minimized. I got an error from plot_trajectories, see below.

I appended the program below the error message.

ValueError                                Traceback (most recent call last)
Cell In[1], line 239
    237 print(solution.shape)
    238 prob.plot_constraint_violations(solution) 
--> 239 prob.plot_trajectories(solution)

File c:\Users\Peter\anaconda3\envs\math\Lib\site-packages\opty\utils.py:186, in _optional_plt_dep.<locals>.wrapper(*args, **kwargs)
    184     raise ImportError('Install matplotlib for plotting features.')
    185 else:
--> 186     func(*args, **kwargs)

File c:\Users\Peter\anaconda3\envs\math\Lib\site-packages\opty\direct_collocation.py:394, in Problem.plot_trajectories(self, vector, axes)
    390 num_axes = (self.collocator.num_states +
    391             self.collocator.num_input_trajectories)
    392 traj_syms = (self.collocator.state_symbols +
    393              self.collocator.input_trajectories)
--> 394 trajectories = np.vstack((state_traj, input_traj))
    396 if axes is None:
    397     fig, axes = plt.subplots(num_axes, 1, sharex=True,
    398                              figsize=(6.4, max(4.8, 0.6*num_axes)),
    399                              layout='compressed')

File <__array_function__ internals>:200, in vstack(*args, **kwargs)

File c:\Users\Peter\anaconda3\envs\math\Lib\site-packages\numpy\core\shape_base.py:296, in vstack(tup, dtype, casting)
    294 if not isinstance(arrs, list):
    295     arrs = [arrs]
--> 296 return _nx.concatenate(arrs, 0, dtype=dtype, casting=casting)

File <__array_function__ internals>:200, in concatenate(*args, **kwargs)

ValueError: all the input array dimensions except for the concatenation axis must match exactly, but along dimension 1, the array at index 0 has size 100 and the array at index 1 has size 1

this is the program: ( note, it does yet not work in the sense, that opty does not find a solution)

import sympy.physics.mechanics as me
from collections import OrderedDict
import time
import numpy as np
import sympy as sm
from scipy.interpolate import CubicSpline
from scipy.integrate import solve_ivp

import opty
from opty.direct_collocation import Problem
from opty.utils import create_objective_function
import matplotlib.pyplot as plt
import matplotlib
from IPython.display import HTML
matplotlib.rcParams['animation.embed_limit'] = 2**128
from matplotlib.animation import FuncAnimation
print('opty version: ', opty.__version__)
print('sympy version: ', sm.__version__)

N, AR, AW        = sm.symbols('N, AR, AW', cls = me.ReferenceFrame)
O, DmcR, DmcW, P = sm.symbols('O, DmcR, DmcW, P', cls=me.Point)

t = me.dynamicsymbols._t

qR, uR, qW, uW            = me.dynamicsymbols('qR, uR, qW, uW')
yR, uyR = me.dynamicsymbols('yR, uyR')

mR, mW, g, iZZR, iZZW, ro, d, max_kipp, kR, kW, reibung, korrekt = sm.symbols('mR, mW, g, iZZR, iZZW, ro, d, max_kipp, kR, kW, reibung, korrekt') 

# set up the geometry
O.set_vel(N, 0)

AR.orient_axis(N, qR, N.z)
AR.set_ang_vel(N, uR * N.z)
AW.orient_axis(AR, qW, AR.z)
rot = AW.ang_vel_in(N)
AW.set_ang_vel(AR, uW * AR.z)
rot1 = AW.ang_vel_in(N)

DmcR.set_pos(O, yR*N.y)
DmcR.set_vel(N, uyR*N.y)

P.set_pos(DmcR, ro*AR.x - ro/5.*AR.y)
P.v2pt_theory(DmcR, N, AR)

DmcW.set_pos(P, d*AW.x)
DmcW.v2pt_theory(P, N, AW)

iR = me.inertia(AR, 0., 0., iZZR)
iW = me.inertia(AW, 0., 0., iZZW)
Ring       = me.RigidBody('Ring', DmcR, AR, mR, (iR, DmcR)) 
Woodpecker = me.RigidBody('Woodpecker', DmcW, AW, mW, (iW, DmcW))
BODY       = [Ring, Woodpecker]

#================================================================================================
FG = [(DmcR, -mR*g*N.y), (DmcW, -mW*g*N.y)]         # gravity
FT1 = [(AW, -kW * (qW-korrekt) * N.z), (AR, kW * (qW-korrekt) * N.z)]    # torque if woodpecker is not 'in line' with the ring

def sprung_r(x, a):
    '''
    = 1 for x > a, = 0 for x < a
    '''
    return 1. / (1 + sm.exp(-50 * (x - a)))

def sprung_l(x, a):
    '''
    = 1 for x < a, = 0 for x > a
    '''
    return 1. / (1 + sm.exp(50 * (x - a))) 

def abs_replacer(x):
    '''
    = -x for x < 0, = x for x > 0
    '''
    return x * (1. - 2 / (sm.exp(50*x) + 1))

# When - max_kipp > qR or qR > max_kipp, the ring touches the peg 
#        faktor1 = sm.Piecewise((1., -max_kipp > qR), (1., qR >= max_kipp), (0., True))
faktor1 = sprung_l(qR, -max_kipp) + sprung_r(qR, max_kipp)

# the 'penetration' is qR - max_kipp if qR > max_kipp, and -(qR + max_kipp) if qR < -max_kipp
#        eindring = sm.Piecewise((qR - max_kipp, qR > max_kipp), (-qR - max_kipp, qR < -max_kipp), (0., True))
eindring = (qR - max_kipp) * sprung_r(qR, max_kipp) - (qR + max_kipp) * sprung_l(qR, -max_kipp)

# This penetration will create a torque on AR. I must determine the direction of this torque
#        faktor2 = sm.Piecewise((-1., max_kipp < qR), (1., True))
faktor2 = -sprung_r(qR, max_kipp) + sprung_l(qR, max_kipp)
kraft = kR * eindring * faktor1 * faktor2
FT2 = [(AR, kraft * N.z)]
# friction acting on DmcR to stop the sliding. I made it speed dependent to avoid that the ring might be driven upward.
kraft2 = abs_replacer(kraft)
FR = [(DmcR, -uyR * reibung * kraft2 * N.y)]

FL = FG + FT1 + FT2 + FR
hilfs = [kraft, -uyR * reibung*kraft2]

#====================================================================================
# set up the equations of motion
q_ind = [qR, qW, yR]
u_ind = [uR, uW, uyR]
kd    = sm.Matrix([i - j.diff(t) for i, j in zip(u_ind, q_ind)])

KM = me.KanesMethod(N, q_ind=q_ind, u_ind=u_ind, kd_eqs=kd) 
(fr, frstar) = KM.kanes_equations(BODY, FL)
MM = KM.mass_matrix_full
force = KM.forcing_full

EOM = kd.col_join(fr + frstar)

print('EOM DS', me.find_dynamicsymbols(EOM))
print('EOM free symbols', EOM.free_symbols)
print(f'EOM has {sm.count_ops(EOM)} operations, {sm.count_ops(sm.cse(EOM))} operations after cse', '\n')

state_symbols     = tuple(q_ind + u_ind)
laenge            = len(state_symbols)
constant_symbols  = tuple((mR, mW, g, iZZR, iZZW, ro, d, max_kipp, korrekt, kR, reibung))
specified_symbols = []
unknown_symbols   = [kW]

methode = 'backward euler'

duration  = 30.                                             # The duration of the simulation.
num_nodes = 100                                             # The number of nodes to use in the direct collocation problem.
t0, tf    = 0., duration                                    # The initial and final times
interval_value = duration / (num_nodes - 1)                 # The interval value between nodes.

WERT = [0., 0., 0., 0., 0., 0.]
initial_guess = np.array([wert for wert in WERT for _ in range(num_nodes)] + [2.e3])

# Specify the known system parameters.
par_map = OrderedDict()
par_map[mR]       = 1.0                                     # Mass of the ring
par_map[mW]       = 15                                     # Mass of the woodpecker
par_map[g]        = 9.81                                    # Acceleration due to gravity
par_map[iZZR]     = 1.                                     # Moment of inertia of the ring
par_map[iZZW]     = 10.                                     # Moment of inertia of the woodpecker
par_map[ro]       = 1.0                                     # Distance of the mass center of the ring from DmcR
par_map[d]        = 2.0                                     # Distance of the mass center of the woodpecker from P
par_map[max_kipp] = np.deg2rad(2.)                                     # The maximum angle the ring can be tilted
par_map[korrekt]  = np.deg2rad(10.)                                     # The angle the ring is tilted in the beginning
par_map[reibung]  = 0.75                                     # Friction coefficient
par_map[kR]       = 1.e6                                    # Spring constant of the ring

objective = sm.Integral(kW**2, t)   # minimal power

# 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)

#====================================================================================
initial_state_constraints = {
    qR: np.deg2rad(0),
    qW: np.deg2rad(-30),
    yR: 0.,
    uR: 0.,
    uW: 0.,
    uyR: 0.} 

final_state_constraints    = {yR: -12.8}

bounds = {kW: (8.e2, 1.e3)}

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()),
    )

#====================================================================================
# Integrate conventionally to get a good initial guess.
intervall = duration     # intervall in sec to be simulated
schritte  = num_nodes     # steps per sec to be returned

pL = [key for key in par_map.keys()] + unknown_symbols
pL_vals = [par_map[key] for key in pL[: -1]] + [2.e3]
qL = list(state_symbols)
print('pL: ', pL)
print('pL_vals: ', pL_vals)
print('qL: ', qL)

MM_lam = sm.lambdify(qL + pL, MM, cse=True)
force_lam = sm.lambdify(qL + pL, force, cse=True)

def gradient(t, y, args):
    sol = np.linalg.solve(MM_lam(*y, *args), force_lam(*y, *args))
    return np.array(sol).T[0]

times = np.linspace(0., intervall, schritte) #int(schritte*intervall))
t_span = (0., intervall)
y0 = [initial_state_constraints[key] for key in state_symbols]
print('y0: ', y0)
resultat1 = solve_ivp(gradient, t_span, y0, t_eval = times, args=(pL_vals,)) #, method='Radau') 
resultat = resultat1.y.T
print('Shape of result: ', resultat.shape)
event_dict = {-1: 'Integration failed', 0: 'Integration finished successfully', 1: 'some termination event'}
print(event_dict[resultat1.status], ' the message is: ', resultat1.message)

initial_guess = np.array(list(resultat1.y.flatten()) + [2.e3])
print('endpoint of woodpecker', initial_guess[3*num_nodes-1])
print(initial_guess.shape)
#====================================================================================

prob = Problem(
    obj, 
    obj_grad,
    EOM, 
    state_symbols, 
    num_nodes, 
    interval_value,
    known_parameter_map=par_map,
    instance_constraints=instance_constraints,
    bounds=bounds,
    integration_method=methode)

prob.add_option('max_iter', 3000)  # default is 3000
#prob.plot_trajectories(initial_guess)

# Find the optimal solution.
solution, info = prob.solve(initial_guess)
print('message from optimizer:', info['status_msg'])
print('Iterations needed',len(prob.obj_value))
print(f"objective value {info['obj_val']:.3e}")

print(f'kW: {solution[-1]:.3f}')
print(solution.shape)
prob.plot_constraint_violations(solution) 
prob.plot_trajectories(solution)

[moorepants]

Simpler example that raises the error:

from collections import OrderedDict

import numpy as np
import sympy as sym
import matplotlib.pyplot as plt
from opty.direct_collocation import Problem

duration = 1.0
num_nodes = 100
interval = duration / (num_nodes - 1)

mu, p, t = sym.symbols('mu, p, t')
y1, y2, T = sym.symbols('y1, y2, T', cls=sym.Function)

state_symbols = (y1(t), y2(t))
constant_symbols = (mu, p)

eom = sym.Matrix([y1(t).diff(t) - y2(t),
                  y2(t).diff(t) - mu**2 * y1(t) + (mu**2 + p**2) *
                  sym.sin(p * T(t))])

par_map = OrderedDict()
par_map[mu] = 60.0

time = np.linspace(0.0, 1.0, num_nodes)
y1_m = np.sin(np.pi * time) + np.random.normal(scale=0.05, size=len(time))
y2_m = np.pi * np.cos(np.pi * time) + np.random.normal(scale=0.05,
                                                       size=len(time))

def obj(free):
    return interval * np.sum((y1_m - free[:num_nodes])**2)

def obj_grad(free):
    grad = np.zeros_like(free)
    grad[:num_nodes] = 2.0 * interval * (free[:num_nodes] - y1_m)
    return grad

instance_constraints = (y1(0.0), y2(0.0) - np.pi)
prob = Problem(obj, obj_grad,
               eom, state_symbols,
               num_nodes, interval,
               known_parameter_map=par_map,
               known_trajectory_map={T(t): time},
               instance_constraints=instance_constraints,
               time_symbol=t,
               integration_method='midpoint')
initial_guess = np.random.randn(prob.num_free)
prob.plot_trajectories(initial_guess)