prob.plot_trajectories vs. prob.plot_constraint_violations

[Peter230655]

I am trying a simulation, and I get the error below.
It seems caused by prob.plot_trajectories(solution).
But prob.plot_constraint_violations(solution) works fine, with the same solution vector.
As per the documentation 25th May 2024 pages 56, 57 they taake the same shape of the argument vector.

NOTE: The optimization does not work (yet) is does not converge.
Could my problem be related to this?

Thanks for any help!

I appended the program below the error

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

the program

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]

You have to provide us with code we can run to help you debug.

[Peter230655]

You were too fast, the code is there now.
Thanks.
NB:
I am not yet worried about the code not running, just about the behaviour of the two plot functions, which I do not understand.
This might help me to get the optimization to run - I hope.

One more comment, no idea whether it helps:
I conventionally integrate the system, with solve_ivp to get good initial conditions.
As I have to replace abs and piecewise with exponential functions, I get the warning overflow encountered in exp, but solve_ivp gets the correct solution.
Could this overflow affect opty?

[moorepants]

It seems you have no unknown input trajectories and the plot fails. This is then a bug.

[Peter230655]

I have only one unkonwn parameter.
You mean a bug in my simulation or a bug in prob.plot_trajectories?
Thanks!

[moorepants]

bug in prob.plot_trajectories

[Peter230655]

Should in open an issue, since you say it is a bug in a method of opty?
If YES, close this?

[moorepants]

Yes, we need an issue opened about this.

[Peter230655]

I put this into issue # 181