simulation.py
import numpy as np
import sympy as sp
import system_model
from scipy.integrate import solve_ivp
from ackrep_core import ResultContainer
from ackrep_core.system_model_management import save_plot_in_dir
import matplotlib.pyplot as plt
import os
from assimulo.solvers import IDA
from assimulo.problem import Implicit_Problem
from ipydex import IPS, activate_ips_on_exception
# link to documentation with examples: https://ackrep-doc.readthedocs.io/en/latest/devdoc/contributing_data.html
def simulate():
"""
simulate model with DAEs, solved by IDA
return: list with results of the simulation
"""
# ---------start of edit section--------------------------------------
model = system_model.Model()
mod = model.get_mod()
print("Constraints:\n")
for i, eq in enumerate(mod.constraints):
print(eq)
print("\n")
print("ODEs:\n")
for i, eq in enumerate(mod.eqns):
print(eq)
print("\n")
dae_model_func = model.get_dae_model_func(mod)
# number of configuration coordinates
ntt = len(mod.tt)
# initial state values, calculated seperatly
(yy0, yyd0) = (
[0.3, 1.74961317, 0.50948621, 0.0, 0.0, 0.0, -0.27535424, 0.5455313],
[0.0, 0.0, 0.0, 23.53968609, 2.82766884, -14.48960943, -0.0, 0.0],
)
t0 = 0
problem_object = Implicit_Problem(dae_model_func, yy0, yyd0, t0)
sim = IDA(problem_object)
sim.verbosity = 0
tfinal = 11
ncp = 500
tt_sol, yy_sol, yyd_sol = sim.simulate(tfinal, ncp)
ttheta_sol = yy_sol[:, :ntt]
ttheta_d_sol = yy_sol[:, ntt : ntt * 2]
simulation_data = [tt_sol, yy_sol, yyd_sol, ttheta_sol, ttheta_d_sol]
# ---------end of edit section----------------------------------------
save_plot(simulation_data)
return simulation_data
def save_plot(simulation_data):
"""
plot your data and save the plot
:param simulation_data: simulation_data of system_model
:return: None
"""
# ---------start of edit section--------------------------------------
# plot of your data
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 9.6))
plt.sca(ax1)
ax1.plot(simulation_data[0], simulation_data[3])
ax1.set_title("angles")
ax2.plot(simulation_data[0], simulation_data[4])
ax2.set_title("angular velocities")
# ---------end of edit section----------------------------------------
plt.tight_layout()
save_plot_in_dir()
def evaluate_simulation(simulation_data):
"""
assert that the simulation results are as expected
:param simulation_data: simulation_data of system_model
:return: ResultContainer
"""
# ---------start of edit section--------------------------------------
# fill in final states of simulation to check your model
# simulation_data.y[i][-1]
expected_final_state = [11, 0.07655823245005482, -11.441290258055378, -4.092516225352934, 1.6614692473252495]
# [6, 0.9774883908572787, 0.9398128300737418, -4.229675196768807, -0.15115736529169507]
# ---------end of edit section----------------------------------------
rc = ResultContainer(score=1.0)
simulated_final_state = [
simulation_data[0][-1],
simulation_data[1][-1][-1],
simulation_data[2][-1][-1],
simulation_data[3][-1][-1],
simulation_data[4][-1][-1],
]
rc.final_state_errors = [
simulated_final_state[i] - expected_final_state[i] for i in np.arange(0, len(simulated_final_state))
]
rc.success = np.allclose(expected_final_state, simulated_final_state, rtol=0, atol=1e-2)
return rc
system_model.py
import sympy as sp
from sympy import sin, cos, pi
import symbtools as st
import importlib
import sys, os
# from ipydex import IPS, activate_ips_on_exception
import symbtools.modeltools as mt
from ackrep_core.system_model_management import GenericModel, import_parameters
# Import parameter_file
params = import_parameters()
# link to documentation with examples: https://ackrep-doc.readthedocs.io/en/latest/devdoc/contributing_data.html
class Model(GenericModel):
def initialize(self):
"""
this function is called by the constructor of GenericModel
:return: None
"""
# ---------start of edit section--------------------------------------
# Define number of inputs -- MODEL DEPENDENT
self.u_dim = 1
# Set "sys_dim" to constant value, if system dimension is constant
self.sys_dim = 8
# ---------end of edit section----------------------------------------
# check existence of params file
self.has_params = True
self.params = params
# ----------- SET DEFAULT INPUT FUNCTION ---------- #
# --------------- Only for non-autonomous Systems
def uu_default_func(self):
"""
define input function
:return:(function with 2 args - t, xx_nv) default input function
"""
T = 10
f1 = 1 * sp.sin(2 * sp.pi * self.t_symb / T)
u_num_func = st.expr_to_func(self.t_symb, f1)
# ---------start of edit section--------------------------------------
def uu_rhs(t, xx_nv):
"""
sequence of numerical input values
:param t:(scalar or vector) time
:param xx_nv:(vector or array of vectors) numeric state vector
:return:(list) numeric inputs
"""
u = u_num_func(t)
return [u]
# ---------end of edit section----------------------------------------
return uu_rhs
# ----------- SYMBOLIC RHS FUNCTION ---------- #
def get_mod(self):
"""
define symbolic model
return: object of class SymbolicModel from symbtools
"""
_mod = getattr(self, "_mod", None)
if _mod is not None:
return _mod
x1, x2, x3, xdot1, xdot2, xdot3, lambda_1, lambda_2 = sp.symbols(
"x1, x2, x3, xdot1, xdot2, xdot3, lambda_1, lambda_2"
)
xddot1, xddot2, xddot3 = sp.symbols("xddot1, xddot2, xddot3")
# s1, s2, s3, m1, m2, m3, J1, J2, J3, l1, l2, l3, l4, g = self.pp_symb
params = sp.symbols("s1, s2, s3, m1, m2, m3, J1, J2, J3, l1, l2, l3, l4, g")
s1, s2, s3, m1, m2, m3, J1, J2, J3, l1, l2, l3, l4, g = params
u1 = self.uu_symb[0] # inputs
mod = mt.SymbolicModel()
mod.constraints = sp.Matrix(
[[l1 * cos(x3) + l2 * cos(x1 + x3) - l3 * cos(x2) - l4], [l1 * sin(x3) + l2 * sin(x1 + x3) - l3 * sin(x2)]]
)
eqns1 = [
J2 * xddot1
+ J2 * xddot3
+ g * m2 * s2 * cos(x1 + x3)
+ l1 * m2 * xddot3 * s2 * cos(x1)
+ l1 * m2 * xdot3**2 * s2 * sin(x1)
+ l2 * lambda_1 * sin(x1 + x3)
- l2 * lambda_2 * cos(x1 + x3)
+ m2 * xddot1 * s2**2
+ m2 * xddot3 * s2**2
]
eqns2 = [
J3 * xddot2
+ g * m3 * s3 * cos(x2)
- l3 * lambda_1 * sin(x2)
+ l3 * lambda_2 * cos(x2)
+ m3 * xddot2 * s3**2
]
eqns3 = [
J1 * xddot3
+ J2 * xddot1
+ J2 * xddot3
+ g * l1 * m2 * cos(x3)
+ g * m1 * s1 * cos(x3)
+ g * m2 * s2 * cos(x1 + x3)
+ l1**2 * m2 * xddot3
+ l1 * lambda_1 * sin(x3)
- l1 * lambda_2 * cos(x3)
+ l1 * m2 * xddot1 * s2 * cos(x1)
- l1 * m2 * xdot1**2 * s2 * sin(x1)
- 2 * l1 * m2 * xdot1 * xdot3 * s2 * sin(x1)
+ 2 * l1 * m2 * xddot3 * s2 * cos(x1)
+ l2 * lambda_1 * sin(x1 + x3)
- l2 * lambda_2 * cos(x1 + x3)
+ m1 * xddot3 * s1**2
+ m2 * xddot1 * s2**2
+ m2 * xddot3 * s2**2
- u1
]
mod.eqns = sp.Matrix([eqns1, eqns2, eqns3])
mod.llmd = sp.Matrix([[lambda_1], [lambda_2]])
mod.tt = sp.Matrix([[x1], [x2], [x3]])
mod.ttd = sp.Matrix([[xdot1], [xdot2], [xdot3]])
mod.ttdd = sp.Matrix([[xddot1], [xddot2], [xddot3]])
mod.tau = sp.Matrix([[u1]])
# prevent unneccessary recalculation of the model
self._mod = mod
return mod
def get_rhs_func(self):
msg = "This DAE model has no rhs func like ODE models."
raise NotImplementedError(msg)
def get_dae_model_func(self, mod):
"""
generate dae system out of the symbolic model
return: function
"""
parameter_values = list(self.pp_str_dict.items())
dae = mod.calc_dae_eq(parameter_values)
dae.generate_eqns_funcs()
dae_mod_func = dae.model_func
return dae_mod_func
def get_rhs_symbolic(self):
"""This model is not represented by the standard rhs equations."""
return False
parameters.py
import sys
import os
import numpy as np
import sympy as sp
import tabulate as tab
# link to documentation with examples: https://ackrep-doc.readthedocs.io/en/latest/devdoc/contributing_data.html
# set model name
model_name = "four-bar linkage"
# ---------- create symbolic parameters
pp_symb = [s1, s2, s3, m1, m2, m3, J1, J2, J3, l1, l2, l3, l4, g] = sp.symbols(
"s1, s2, s3, m1, m2, m3, J1, J2, J3, l1, l2, l3, l4, g", real=True
)
# ---------- create symbolic parameter functions
# parameter values can be constant/fixed values OR set in relation to other parameters (for example: a = 2*b)
s1_sf = 1 / 2
s2_sf = 1 / 2
s3_sf = 1 / 2
m1_sf = 1
m2_sf = 1
m3_sf = 3
J1_sf = 1 / 12
J2_sf = 1 / 12
J3_sf = 1 / 12
l1_sf = 0.8
l2_sf = 1.5
l3_sf = 1.5
l4_sf = 2
g_sf = 9.81
# list of symbolic parameter functions
# trailing "_sf" stands for "symbolic parameter function"
pp_sf = [s1_sf, s2_sf, s3_sf, m1_sf, m2_sf, m3_sf, J1_sf, J2_sf, J3_sf, l1_sf, l2_sf, l3_sf, l4_sf, g_sf]
# ---------- list for substitution
# -- entries are tuples like: (independent symbolic parameter, numerical value)
pp_subs_list = []
# OPTONAL: Dictionary which defines how certain variables shall be written
# in the table - key: Symbolic Variable, Value: LaTeX Representation/Code
# useful for example for complex variables: {Z: r"\underline{Z}"}
latex_names = {}
# ---------- Define LaTeX table
# Define table header
# DON'T CHANGE FOLLOWING ENTRIES: "Symbol", "Value"
tabular_header = ["Parameter Name", "Symbol", "Value", "Unit"]
# Define column text alignments
col_alignment = ["left", "center", "left", "center"]
# Define Entries of all columns before the Symbol-Column
# --- Entries need to be latex code
col_1 = [
"distance from the joint to the center of gravity of link 1",
"distance from the joint to the center of gravity of link 2",
"distance from the joint to the center of gravity of link 3",
"mass of link 1",
"mass of link 2",
"mass of link 3",
"moment of inertia of link 1",
"moment of inertia of link 2",
"moment of inertia of link 3",
"length of link 1",
"length of link 2",
"length of link 3",
"length of link 4",
"acceleration due to gravity",
]
# contains all lists of the columns before the "Symbol" Column
# --- Empty list, if there are no columns before the "Symbol" Column
start_columns_list = [col_1]
# Define Entries of the columns after the Value-Column
# --- Entries need to be latex code
col_4 = [
"m",
"m",
"m",
"kg",
"kg",
"kg",
r"$kg \cdot m^2$",
r"$kg \cdot m^2$",
r"$kg \cdot m^2$",
"m",
"m",
"m",
"m",
r"$\frac{m}{s^2}$",
]
# contains all lists of columns after the FIX ENTRIES
# --- Empty list, if there are no columns after the "Value" column
end_columns_list = [col_4]