opty // reaction forces #189
Replies: 7 comments 12 replies
-
|
Any reaction forces can just be calculated after you solve the essential dynamics with opty. |
Beta Was this translation helpful? Give feedback.
-
|
This is what I did, and it works! The only 'issue' is that one has to set up the EOMs twice, no big deal, I agree. |
Beta Was this translation helpful? Give feedback.
-
|
Should I close the discussion? |
Beta Was this translation helpful? Give feedback.
-
|
A very simple example, like a single pendulum with the auxiliary equations to solve for the reaction force at the joint would be much easier to think about. I suspect opty can handle those equations and find the unknown forces but offhand I can't think about what would cause the issue. |
Beta Was this translation helpful? Give feedback.
-
I will do this later today and report back. |
Beta Was this translation helpful? Give feedback.
-
|
The problem might be this: if there are auxiliary speeds, KM.kanes_equations returns fr, frstar including fraux, frstaraux. |
Beta Was this translation helpful? Give feedback.
-
|
Below is a simple pendulum. I took the example from opty's examples-gallery. All I changed was this: |
Beta Was this translation helpful? Give feedback.
-
|
I tried to treat the reaction forces as additional specified symbols, for opty to minimize. """
Fixed Duration Pendulum Swing Up
================================
Given a compound pendulum that is driven by a torque about its joint axis,
swing the pendulum from hanging down to standing up in a fixed amount of time
using minimal input torque with a bounded torque magnitude.
"""
# %%
# Start with defining the fixed duration and number of nodes.
duration = 10.0 # seconds
num_nodes = 501
interval_value = duration/(num_nodes - 1)
#================
opty_crash = True
#================
# %%
# Specify the symbolic equations of motion.
import numpy as np
import sympy as sm
import sympy.physics.mechanics as me
from opty.direct_collocation import Problem
from opty.utils import create_objective_function
import matplotlib.pyplot as plt
import matplotlib.animation as animation
Izz, m, g, d, t = sm.symbols('Izz, m, g, d, t')
theta, omega, T = sm.symbols('theta, omega, T', cls=sm.Function)
N, A = sm.symbols('N, A', cls = me.ReferenceFrame)
O, P1, P2 = sm.symbols('O, P1, P2', cls=me.Point)
auxx,auxy, fx, fy = me.dynamicsymbols('auxx, auxy, fx, fy')
A.orient_axis(N, theta(t), N.z)
A.set_ang_vel(N, omega(t)*N.z)
O.set_vel(N, 0)
P1.set_pos(O, 0)
P1.set_vel(N, 0)
if opty_crash == True:
P1.set_vel(N, auxx*N.x + auxy*N.y)
else:
P1.set_vel(N, 0)
P2.set_pos(P1, -d*A.y)
P2.v2pt_theory(P1, N, A)
I = me.inertia(A, 0., 0., Izz)
body = me.RigidBody('body', P2, A, m, (I, P2))
BODY = [body]
if opty_crash == False:
FL = [(P2, -m*g*N.y), (A, T(t)*N.z)]
aux = []
else:
FL = [(P2, -m*g*N.y), (A, T(t)*N.z), (P1, fx*N.x + fy*N.y)]
aux = [auxx, auxy]
kd = sm.Matrix([theta(t).diff(t) - omega(t)])
KM = me.KanesMethod(N, q_ind=[theta(t)], u_ind=[omega(t)], u_auxiliary=aux, kd_eqs=kd )
(fr, frstar) = KM.kanes_equations(BODY, FL)
print(f'shape of fr + frstar auxiliary: {(fr+frstar).shape} virtual speeds present is {opty_crash} \n')
if opty_crash == True:
eom = kd.col_join(fr + frstar)
else:
eom = me.msubs(kd.col_join(fr + frstar), {fx: 0, fy: 0})
sm.pprint(eom)
state_symbols = (theta(t), omega(t))
constant_symbols = (Izz, m, g, d)
if opty_crash == True:
specified_symbols = (T(t), fx, fy)
else:
specified_symbols = (T(t),)
#eom1 = sm.Matrix([theta(t).diff() - omega(t),
# Izz*omega(t).diff() + m*g*d*sm.sin(theta(t)) - T(t)])
# %%
# Specify the known system parameters.
par_map = {
Izz: 1.0,
m: 1.0,
g: 9.81,
d: 1.0,
}
# %%
# Specify the objective function and it's gradient, in this case it calculates
# the area under the input torque curve over the simulation.
obj_func = sm.Integral(T(t)**2, t)
sm.pprint(obj_func)
obj, obj_grad = create_objective_function(obj_func, state_symbols,
specified_symbols, tuple(),
num_nodes,
node_time_interval=interval_value,
time_symbol=t)
# %%
# Specify the symbolic instance constraints, i.e. initial and end conditions,
# where the pendulum starts a zero degrees (hanging down) and ends at 180
# degrees (standing up).
target_angle = np.pi # radians
instance_constraints = (
theta(0.0),
theta(duration) - target_angle,
omega(0.0),
omega(duration),
)
# %%
# Limit the torque to a maximum magnitude.
bounds = {T(t): (-2.0, 2.0)}
# %%
# Create an optimization problem.
prob = Problem(obj, obj_grad, eom, state_symbols, num_nodes, interval_value,
known_parameter_map=par_map,
instance_constraints=instance_constraints,
bounds=bounds,
time_symbol=t)
# %%
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# %%
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
print(info['status_msg'])
print(info['obj_val'])
# %%
# Plot the optimal state and input trajectories.
prob.plot_trajectories(solution)
# %%
# Plot the constraint violations.
prob.plot_constraint_violations(solution)
# %%
# Plot the objective function as a function of optimizer iteration.
prob.plot_objective_value()
# %%
# Animate the pendulum swing up.
time = np.linspace(0.0, duration, num=num_nodes)
angle = solution[:num_nodes]
fig = plt.figure()
ax = fig.add_subplot(111, aspect='equal', autoscale_on=False,
xlim=(-2, 2), ylim=(-2, 2))
ax.grid()
line, = ax.plot([], [], 'o-', lw=2)
time_template = 'time = {:0.1f}s'
time_text = ax.text(0.05, 0.9, '', transform=ax.transAxes)
def init():
line.set_data([], [])
time_text.set_text('')
return line, time_text
def animate(i):
x = [0, par_map[d]*np.sin(angle[i])]
y = [0, -par_map[d]*np.cos(angle[i])]
line.set_data(x, y)
time_text.set_text(time_template.format(time[i]))
return line, time_text
ani = animation.FuncAnimation(fig, animate, range(num_nodes),
interval=int(interval_value*1000), blit=True,
init_func=init)
plt.show() |
Beta Was this translation helpful? Give feedback.
-
|
This program outputs: I don't see any unknown reaction forces in the last two equations, so how could opty solve for them if they are not present? |
Beta Was this translation helpful? Give feedback.
-
|
Did you use the second program I posted,, the one below? shape of fr + frstar auxiliary: (3, 1) virtual speeds present is True ⎡ d ⎤ |
Beta Was this translation helpful? Give feedback.
-
|
This program works: """
Fixed Duration Pendulum Swing Up
================================
Given a compound pendulum that is driven by a torque about its joint axis,
swing the pendulum from hanging down to standing up in a fixed amount of time
using minimal input torque with a bounded torque magnitude.
"""
# %%
# Start with defining the fixed duration and number of nodes.
duration = 10.0 # seconds
num_nodes = 501
interval_value = duration/(num_nodes - 1)
# %%
# Specify the symbolic equations of motion.
import numpy as np
import sympy as sm
import sympy.physics.mechanics as me
from opty.direct_collocation import Problem
from opty.utils import create_objective_function
import matplotlib.pyplot as plt
import matplotlib.animation as animation
Izz, m, g, d, t = sm.symbols('Izz, m, g, d, t')
theta, omega, T = sm.symbols('theta, omega, T', cls=sm.Function)
N, A = sm.symbols('N, A', cls = me.ReferenceFrame)
O, P1, P2 = sm.symbols('O, P1, P2', cls=me.Point)
auxx,auxy, fx, fy = me.dynamicsymbols('auxx, auxy, fx, fy')
A.orient_axis(N, theta(t), N.z)
A.set_ang_vel(N, omega(t)*N.z)
O.set_vel(N, 0)
P1.set_pos(O, 0)
P1.set_vel(N, auxx*N.x + auxy*N.y)
P2.set_pos(P1, -d*A.y)
P2.v2pt_theory(P1, N, A)
I = me.inertia(A, 0., 0., Izz)
body = me.RigidBody('body', P2, A, m, (I, P2))
BODY = [body]
FL = [(P2, -m*g*N.y), (A, T(t)*N.z), (P1, fx*N.x + fy*N.y)]
aux = [auxx, auxy]
kd = sm.Matrix([theta(t).diff(t) - omega(t)])
KM = me.KanesMethod(N, q_ind=[theta(t)], u_ind=[omega(t)], u_auxiliary=aux, kd_eqs=kd )
(fr, frstar) = KM.kanes_equations(BODY, FL)
eom = kd.col_join(fr + frstar)
sm.pprint(eom)
state_symbols = (theta(t), omega(t), fx, fy)
constant_symbols = (Izz, m, g, d)
specified_symbols = (T(t),) #, fx, fy)
# Specify the known system parameters.
par_map = {
Izz: 1.0,
m: 1.0,
g: 9.81,
d: 1.0,
}
# %%
# Specify the objective function and it's gradient, in this case it calculates
# the area under the input torque curve over the simulation.
obj_func = sm.Integral(T(t)**2, t)
sm.pprint(obj_func)
obj, obj_grad = create_objective_function(obj_func, state_symbols,
specified_symbols, tuple(),
num_nodes,
node_time_interval=interval_value,
time_symbol=t)
# %%
# Specify the symbolic instance constraints, i.e. initial and end conditions,
# where the pendulum starts a zero degrees (hanging down) and ends at 180
# degrees (standing up).
target_angle = np.pi # radians
instance_constraints = (
theta(0.0),
theta(duration) - target_angle,
omega(0.0),
omega(duration),
)
# %%
# Limit the torque to a maximum magnitude.
bounds = {T(t): (-2.0, 2.0)}
# %%
# Create an optimization problem.
prob = Problem(obj, obj_grad, eom, state_symbols, num_nodes, interval_value,
known_parameter_map=par_map,
instance_constraints=instance_constraints,
bounds=bounds,
time_symbol=t)
# %%
# Use a random positive initial guess.
initial_guess = np.random.randn(prob.num_free)
# %%
# Find the optimal solution.
solution, info = prob.solve(initial_guess)
print(info['status_msg'])
print(info['obj_val'])
Four equations needs four "states". |
Beta Was this translation helpful? Give feedback.
-
|
You made fx, fy to be state symbols, rather than specified symbols! |
Beta Was this translation helpful? Give feedback.
-
|
Beta Was this translation helpful? Give feedback.
-
It is only wrong because opty expects one state per dynamics equation |
Beta Was this translation helpful? Give feedback.
-
|
I works on my larger program (op_count(EOM) = 650,000 before cse), too!! |
Beta Was this translation helpful? Give feedback.
-
|
It seems to take vastely longer. Without reaction forces as state variables about 170 iterations were sufficient. Now 3 loops at 3000 iterations each do not reach a solution. Would the time needed increase exponentially with the state variables? (my gut feeling would say so - but it was wrong 45 min ago) Should I close the discussion? After all it was solved. |
Beta Was this translation helpful? Give feedback.
-
|
Adding the three reaction forces to the state variables of my 'larger' simulation (now 10 + 3 state variables) works in the sense opty does not throw an error. I could not find a solution in good time, maybe because I could not give a good initial guess for the reaction forces. |
Beta Was this translation helpful? Give feedback.
-
|
I suspect it will always be unnecessary extra work for the optimizer to add in new variables like this. |
Beta Was this translation helpful? Give feedback.
-
I totally agree. You recall, the whole thing started because opty threw an error which I did not understand. You explained it to me. |
Beta Was this translation helpful? Give feedback.
-
In an opty simulation, I try to get the reaction forces, too. I do it the standard way.
In the EOMs for use by opty, I eliminate the virtual speeds and the reaction forces, using me.msubs(EOM,.....)
When I try to run it, it gives the error below.
When I set up the EOMs without virtual speeds, opty runs fine and gives a nice result.
I set up the EOMs with virtual speeds again, after opty is finished to get the reaction forces for the solution of opty.
No problem at all.
I compared the sets of dynamic symbols and free symbols in the EOMs created with virtual speeds and without wirtual speeds:
They are the same.
This is the first time I tried this. Did anybody see this problem before?
What could be the reason?
Thanks for any help!
this is the error:
ERROR:cyipopt:b'Invalid number of indices returned from jacobian'
ERROR:cyipopt:b'Invalid number of indices returned from jacobian'
The Kernel crashed while executing code in the current cell or a previous cell.
Please review the code in the cell(s) to identify a possible cause of the failure.
Click here for more info.
View Jupyter log for further details.
this is the program. Near line 138 is a variable create_error, which allows to create the crash
Beta Was this translation helpful? Give feedback.
All reactions