from pymoskito import Simulator, PostProcessor, \
register_simulation_module, register_processing_module, register_visualizer, \
Model, Controller, Feedforward, PostProcessingModule
# import self defined simulation modules
from model import BallInTubeModel, BallInTubeSpringModel
from control import ExactInputOutputLinearisation, OpenLoop
from feedforward import BallInTubeFeedforward
from visualization import BallInTubeVisualizer
from processing import ErrorProcessor
if __name__ == "__main__":
# register Modules
register_simulation_module(Model, BallInTubeModel)
register_simulation_module(Model, BallInTubeSpringModel)
register_simulation_module(Controller, ExactInputOutputLinearisation)
register_simulation_module(Controller, OpenLoop)
register_simulation_module(Feedforward, BallInTubeFeedforward)
register_visualizer(BallInTubeVisualizer)
register_processing_module(PostProcessingModule, ErrorProcessor)
# create an Application instance (needed)
app = QtGui.QApplication([])
if 1:
# create gui
sim = Simulator()
"""
Calculations of system state changes
:param x: state
:param t: time
:type args: system input force on the cart
"""
x1, x2, x3, x4, x5 = x
v, w = 1, 1
return np.array([v * np.cos(x3),
v * np.sin(x3),
w,
v / self.l2 * np.sin(x3 - x4) - self.d1 / self.l2 * w * np.cos(x3 - x4),
v * (1 / self.l3 * np.sin(x3 - x5) - (self.d2 + self.l2) / (self.l2 * self.l3) * np.sin(
x3 - x4) * np.cos(x4 - x5)) \
+ w * (-self.d1 / self.l3 * np.cos(x3 - x5) + (self.d1 * (self.l2 + self.d2)) / (
self.l2 * self.l3) * np.cos(x3 - x4) * np.cos(x4 - x5))
])
def root_function(self, x):
return [False]
def check_consistency(self, x):
pass
def calc_output(self, input):
return input
pm.register_simulation_module(pm.Model, CarModel)
self.K = pm.place_siso(a_mat, b_mat, self._settings["poles"])
self.V = pm.calc_prefilter(a_mat, b_mat, c_mat, self.K)
self.h = self._settings["tick divider"] * self._settings["step width"]
def _control(self,
time,
trajectory_values=None,
feedforward_values=None,
input_values=None,
**kwargs):
# input abbreviations
x = input_values
yd = trajectory_values
e = self._error + self.h * (yd[0] - x[0])
# saturate error
if not abs(e) > self._settings["I Limit"]:
self._error = e
# calculate u
u = -self.K @ x + yd[0] * self.V + self._settings["Ki"] * self._error
return u
pm.register_simulation_module(pm.Controller, FController)
pm.register_simulation_module(pm.Controller, GController)
pm.register_simulation_module(pm.Controller, JController)
pm.register_simulation_module(pm.Controller, LSSController)
pm.register_simulation_module(pm.Controller, PIXController)
self.solver.set_integrator("dopri5",
method=self._settings["Method"],
rtol=self._settings["rTol"],
atol=self._settings["aTol"])
self.solver.set_initial_value(self._settings["initial state"])
self.nextObserver_output = self.output
def linear_state_function(self, t, q, args):
x1_o, x2_o, x3_o, x4_o = q
u = args[0]
y = args[1]
x_o = q
dx_o = (self.a_mat - self.L @ self.c_mat) @ x_o + self.b_mat @ u + self.L @ y
return dx_o
def _observe(self, time, system_input, system_output):
if system_input is not None:
self.solver.set_f_params((system_input, system_output))
self.output = self.solver.integrate(time)
return self.output
pm.register_simulation_module(pm.Observer, LuenbergerObserver)
pm.register_simulation_module(pm.Observer, LuenbergerObserverInt)
pm.register_simulation_module(pm.Observer, LuenbergerObserverReduced)
pm.register_simulation_module(pm.Observer, HighGainObserver)
pm.register_simulation_module(pm.Observer, BallBeamEKF)
pm.register_simulation_module(pm.Observer, BallBeamMHE)
# -*- coding: utf-8 -*-
import numpy as np
from collections import OrderedDict
import pymoskito as pm
class OpenLoop(pm.Controller):
"""
manual pwm input
"""
public_settings = OrderedDict([("pwm", 160),
("tick divider", 1)])
def __init__(self, settings):
# add specific "private" settings
settings.update(input_order=0)
settings.update(output_dim=1)
settings.update(input_type="system_state")
pm.Controller.__init__(self, settings)
def _control(self, time, trajectory_values=None, feedforward_values=None,
input_values=None, **kwargs):
u = self._settings["pwm"]
return np.array(u)
pm.register_simulation_module(pm.Controller, OpenLoop)
def __init__(self, settings):
super().__init__(settings, QuadcopterObserverModel())
self.trajectory = self._settings['trajectory']
self.ts, self.meas_accelerometer, self.meas_gyro, self.ref_ts, self.ref_pos, self.ref_angles = get_trajectory(self.trajectory)
def _observe(self, t, system_input: Array, system_output: Array) -> Array:
imu_index = time_to_index(t, self.ts)
ref_index = time_to_index(t, self.ref_ts)
ref_pos = self.ref_pos[ref_index]
ref_orientation = self.ref_angles[ref_index]
meas_gyro = self.meas_gyro[imu_index]
meas_accelerometer = self.meas_accelerometer[imu_index]
system_measurement = np.concatenate((meas_accelerometer, np.array([ref_orientation[2]])))
observer_out = super()._observe(t, system_input=meas_gyro, system_output=system_measurement)
obs_q = observer_out[:4]
obs_orientation = q_to_euler(obs_q)
obs_err = euler_difference(ref_orientation, obs_orientation)
output = np.concatenate((ref_pos, rad_to_deg(ref_orientation), rad_to_deg(obs_orientation), rad_to_deg(obs_err), observer_out[4:10], meas_gyro, meas_accelerometer, [norm(obs_q)]))
return output
QuadcopterEKF.add_public_settings()
pm.register_simulation_module(pm.Observer, QuadcopterEKF)
QuadcopterMHE.add_public_settings()
pm.register_simulation_module(pm.Observer, QuadcopterMHE)
import pymoskito as pm
class OpenLoop(pm.Controller):
"""
manual pwm input
"""
public_settings = OrderedDict([("pwm", 160), ("tick divider", 1)])
def __init__(self, settings):
# add specific "private" settings
settings.update(input_order=0)
settings.update(output_dim=1)
settings.update(input_type="system_state")
pm.Controller.__init__(self, settings)
def _control(self,
time,
trajectory_values=None,
feedforward_values=None,
input_values=None,
**kwargs):
u = self._settings["pwm"]
return np.array(u)
pm.register_simulation_module(pm.Controller, OpenLoop)
Calculations of system state changes
:param x: state
:param t: time
:type args: system input force on the cart
"""
x1, x2, x3, x4, x5 = x
v, w = 1, 1
return np.array([v * np.cos(x3),
v * np.sin(x3),
w,
v / self.l2 * np.sin(x3 - x4) - self.d1 / self.l2 * w * np.cos(x3 - x4),
v * (1 / self.l3 * np.sin(x3 - x5) - (self.d2 + self.l2) / (self.l2 * self.l3) * np.sin(
x3 - x4) * np.cos(x4 - x5)) \
+ w * (-self.d1 / self.l3 * np.cos(x3 - x5) + (self.d1 * (self.l2 + self.d2)) / (
self.l2 * self.l3) * np.cos(x3 - x4) * np.cos(x4 - x5))
])
def root_function(self, x):
return [False]
def check_consistency(self, x):
pass
def calc_output(self, input):
return input
pm.register_simulation_module(pm.Model, CarModel)
return np.array([dx1, dx2, dx3, dx4])
def root_function(self, x):
"""
is not used
:param x: state
:return:
"""
return [False]
def check_consistency(self, x):
"""
Check if the ball remains on the beam
:param x: state
"""
if abs(x[0]) > float(self._settings['beam length']) / 2:
raise pm.ModelException('Ball fell down.')
if abs(x[2]) > np.pi / 2:
raise pm.ModelException('Beam reached critical angle.')
def calc_output(self, input_vector):
"""
return ball position as output
:param input_vector: input values
:return: ball position
"""
return input_vector[0]
pm.register_simulation_module(pm.Model, BallBeamModel)
self.output = np.array(self._settings["initial state"], dtype=float)
self.solver = ode(self.linear_state_function)
self.solver.set_integrator("dopri5",
method=self._settings["Method"],
rtol=self._settings["rTol"],
atol=self._settings["aTol"])
self.solver.set_initial_value(self._settings["initial state"])
self.nextObserver_output = self.output
def linear_state_function(self, t, q, args):
x1_o, x2_o, x3_o, x4_o = q
u = args[0]
y = args[1]
x_o = q
dx_o = (self.a_mat - self.L @ self.c_mat) @ x_o + self.b_mat @ u + self.L @ y
return dx_o
def _observe(self, time, system_input, system_output):
if system_input is not None:
self.solver.set_f_params((system_input, system_output))
self.output = self.solver.integrate(time)
return self.output
pm.register_simulation_module(pm.Observer, LuenbergerObserver)
pm.register_simulation_module(pm.Observer, LuenbergerObserverInt)
pm.register_simulation_module(pm.Observer, LuenbergerObserverReduced)
pm.register_simulation_module(pm.Observer, HighGainObserver)
from chemical.model import ChemicalObserverModel
class ChemicalEKF(ExtendedKalmanFilterObserver):
@staticmethod
def add_public_settings():
ChemicalEKF.public_settings['zero clipping'] = False
def __init__(self, settings):
np.random.seed(0)
super().__init__(settings, ChemicalObserverModel())
self.zero_clipping = self._settings['zero clipping']
def _observe(self, time, system_input, system_output):
obs_result = super()._observe(time, system_input, system_output)
if self.zero_clipping:
self.filter_algorithm.x = np.fmax(self.filter_algorithm.x,
np.zeros(2, dtype=float))
return self.filter_algorithm.x
class ChemicalMHE(MovingHorizonEstimator):
def __init__(self, settings):
np.random.seed(0)
super().__init__(settings, ChemicalObserverModel())
ChemicalEKF.add_public_settings()
pm.register_simulation_module(pm.Observer, ChemicalEKF)
pm.register_simulation_module(pm.Observer, ChemicalMHE)
from pymoskito import Simulator, PostProcessor,\
register_simulation_module, register_processing_module, register_visualizer, \
PostProcessingModule, \
Model, Controller
from model import BallBeamModel
from control import FController
from visualization import BallBeamVisualizer
from postprocessing import EvalA1
__author__ = 'stefan'
if __name__ == '__main__':
# register own modules
register_simulation_module(Model, BallBeamModel)
register_simulation_module(Controller, FController)
register_processing_module(PostProcessingModule, EvalA1)
register_visualizer(BallBeamVisualizer)
# create an Application instance (needed)
app = QtGui.QApplication([])
if 0:
# create simulator
sim = Simulator()
# load default config
sim.load_regimes_from_file("default.sreg")
sim.apply_regime_by_name("test-nonlinear")
sim.start_simulation()
The flat output is given by the ball position (x3)
x1_flat and x2_flat are the system states expressed in flat coordinates.
"""
public_settings = OrderedDict([("tick divider", 1)])
def __init__(self, settings):
settings.update(input_order=4)
settings.update(output_dim=1)
pm.Feedforward.__init__(self, settings)
def _feedforward(self, time, trajectory_values):
yd = trajectory_values
x1_flat = (np.sqrt((yd[2] + st.g)*st.m*st.A_Sp**2/st.k_L)
+ st.A_B*yd[1])/st.k_V
x2_flat = (yd[3]*st.m*st.A_Sp**2/(2*st.k_V*st.k_L*(st.k_V*x1_flat
- st.A_B*yd[1]))
+ st.A_B*yd[2]/st.k_V)
# feed forward control
u = (st.T**2*(yd[4]*st.m*st.A_Sp**2 - 2*st.k_L*(st.k_V*x2_flat
- st.A_B*yd[2])**2)
/ (2*st.k_s*st.k_V*st.k_L*(st.k_V*x1_flat - st.A_B*yd[1]))
+ (st.T**2*st.A_B*yd[3])/(st.k_s*st.k_V)
+ (2*st.d*st.T*x2_flat)/st.k_s
+ x1_flat/st.k_s)*(255/st.Vcc)
return np.array([[u]], dtype=float)
pm.register_simulation_module(pm.Feedforward, BallInTubeFeedforward)
register_visualizer,
PostProcessingModule,
Model,
Controller,
)
# import custom simulation modules
from model import TwoPendulumModel, TwoPendulumRigidBodyModel, TwoPendulumModelParLin
from control import LinearStateFeedback, LinearStateFeedbackParLin, LjapunovController, SwingUpController
from visualization import TwoPendulumVisualizer
from processing import TwoPendulum
if __name__ == "__main__":
# register own modules
register_simulation_module(Model, TwoPendulumModel)
register_simulation_module(Model, TwoPendulumRigidBodyModel)
register_simulation_module(Model, TwoPendulumModelParLin)
register_simulation_module(Controller, LinearStateFeedback)
register_simulation_module(Controller, LinearStateFeedbackParLin)
register_simulation_module(Controller, LjapunovController)
register_simulation_module(Controller, SwingUpController)
register_visualizer(TwoPendulumVisualizer)
register_processing_module(PostProcessingModule, TwoPendulum)
# create an Application instance (needed)
app = QApplication([])
dx1 = x2
dx2 = u
dx3 = x4
dx4 = self.g*np.sin(x3)/self.l1 - (self.d1*x4)/(self.m1*self.l1**2) + np.cos(x3)*u/self.l1
dx5 = x6
dx6 = self.g*np.sin(x5)/self.l2 - (self.d2*x6)/(self.m2*self.l2**2) + np.cos(x5)*u/self.l2
dx = np.array([dx1, dx2, dx3, dx4, dx5, dx6])
return dx
def root_function(self, x):
return [False]
def check_consistency(self, x):
"""
Check something
"""
pass
def calc_output(self, input):
"""
return cart position as output
:param input: input values
:return: cart position
"""
return input[0]
pm.register_simulation_module(pm.Model, TwoPendulumModel)
pm.register_simulation_module(pm.Model, TwoPendulumModelParLin)
pm.register_simulation_module(pm.Model, TwoPendulumRigidBodyModel)
def output_func(self, x: Array) -> Array:
return self.out_lambda(x).ravel()
def derive_lambdas(self):
pA, pB, h = sp.symbols('p_A p_B h')
k = 0.16
x = sp.Matrix([pA, pB])
f = sp.Matrix([
pA / (2 * k * h * pA + 1),
pB + (k * h * pA**2) / (2 * k * h * pA + 1)
])
f_scalar = sp.lambdify((pA, pB, h), f)
f_lambda = lambda x_, u_, h_: f_scalar(*x_, h_)
f_jac_scalar = sp.lambdify((pA, pB, h), f.jacobian(x))
f_jac = lambda x_, u_, h_: f_jac_scalar(*x_, h_)
h_fun = sp.Matrix([pA + pB])
h_scalar = sp.lambdify((pA, pB), h_fun)
h_lambda = lambda x_: h_scalar(*x_)
h_jac_scalar = sp.lambdify((pA, pB), h_fun.jacobian(x))
h_jac = lambda x_: h_jac_scalar(*x_)
return f_lambda, h_lambda, f_jac, h_jac
pm.register_simulation_module(pm.Model, ChemicalModel)
("tick divider", 1)
])
def __init__(self, settings):
# add specific "private" settings
settings.update(input_order=4)
settings.update(output_dim=1)
pm.Feedforward.__init__(self, settings)
def _calc_theta_t(self, rdd):
theta_t = -rdd / (self._settings["B"] * self._settings["G"])
return theta_t
def _feedforward(self, t, yd):
theta = []
# need the second derivative of theta
for i in range(2 + 1):
theta.append(self._calc_theta_t(yd[i + 2]))
tau = ((self._settings['M'] * self._settings['r0'] ** 2
+ self._settings['J']
+ self._settings['Jb']) * theta[2]
+ self._settings['M'] * self._settings['G'] * yd[0])
return tau
pm.register_simulation_module(pm.Feedforward, LinearFeedforward)
:return:
"""
return [False]
#consistency
def check_consistency(self, x):
"""
Check if the ball remains on the beam
:param x: state
"""
if abs(x[0]) > float(self._settings['beam length']) / 2:
raise pm.ModelException('Ball fell down.')
if abs(x[2]) > np.pi / 2:
raise pm.ModelException('Beam reached critical angle.')
#output
def calc_output(self, input_vector):
"""
return ball position as output
:param input_vector: input values
:return: ball position
"""
return input_vector[0] + np.random.normal(0, np.sqrt(
self.output_noise))
#register
pm.register_simulation_module(pm.Model, BallBeamModel)
self._output = np.zeros((1, ))
# run pole placement
a_mat, b_mat, c_mat = linearise_system(self._settings["steady state"],
self._settings["steady tau"])
self.K = pm.place_siso(a_mat, b_mat, self._settings["poles"])
self.V = pm.calc_prefilter(a_mat, b_mat, c_mat, self.K)
self.h = self._settings["tick divider"] * self._settings["step width"]
def _control(self, time, trajectory_values=None, feedforward_values=None,
input_values=None, **kwargs):
# input abbreviations
x = input_values
yd = trajectory_values
e = self._error + self.h * (yd[0] - x[0])
# saturate error
if not abs(e) > self._settings["I Limit"]:
self._error = e
# calculate u
u = -self.K @ x + yd[0]*self.V + self._settings["Ki"] * self._error
return u
pm.register_simulation_module(pm.Controller, FController)
pm.register_simulation_module(pm.Controller, GController)
pm.register_simulation_module(pm.Controller, JController)
pm.register_simulation_module(pm.Controller, LSSController)
pm.register_simulation_module(pm.Controller, PIXController)
dx4 = self.g * np.sin(x3) / self.l1 - (self.d1 * x4) / (
self.m1 * self.l1**2) + np.cos(x3) * u / self.l1
dx5 = x6
dx6 = self.g * np.sin(x5) / self.l2 - (self.d2 * x6) / (
self.m2 * self.l2**2) + np.cos(x5) * u / self.l2
dx = np.array([dx1, dx2, dx3, dx4, dx5, dx6])
return dx
def root_function(self, x):
return [False]
def check_consistency(self, x):
"""
Check something
"""
pass
def calc_output(self, input):
"""
return cart position as output
:param input: input values
:return: cart position
"""
return input[0]
pm.register_simulation_module(pm.Model, TwoPendulumModel)
pm.register_simulation_module(pm.Model, TwoPendulumModelParLin)
pm.register_simulation_module(pm.Model, TwoPendulumRigidBodyModel)
flag = False
# fan speed
if x[0] <= 0:
x0[0] = 0
x0[1] = 0
flag = True
return flag, x0
def check_consistency(self, x):
"""
Checks if the model rules are violated
:param x: state
"""
if x[2] > (self._settings['tube_length']
): # + self._settings['tube_length']*0.2):
raise pm.ModelException('Ball flew out of the tube')
def calc_output(self, input_vector):
"""
return ball position as output
:param input_vector: input values
:return: ball position
"""
return input_vector[2]
pm.register_simulation_module(pm.Model, BallInTubeModel)
pm.register_simulation_module(pm.Model, BallInTubeSpringModel)
# -*- coding: utf-8 -*-
import pymoskito as pm
# import custom modules
import model
import controller
import visualizer_mpl
# import visualizer_vtk
if __name__ == '__main__':
# register model
pm.register_simulation_module(pm.Model, model.PendulumModel)
# register controller
pm.register_simulation_module(pm.Controller, controller.BasicController)
# register visualizer
pm.register_visualizer(visualizer_mpl.MplPendulumVisualizer)
# pm.register_visualizer(visualizer_vtk.VtkPendulumVisualizer)
# start the program
pm.run()
from collections import OrderedDict
import pymoskito as pm
import numpy as np
class Dummy(pm.Controller):
public_settings = OrderedDict([])
def __init__(self, settings):
settings.update(input_order=0)
settings.update(input_type='system_output')
super().__init__(settings)
def _control(self, time, trajectory_values=None, feedforward_values=None, input_values=None, **kwargs):
return np.zeros(1)
pm.register_simulation_module(pm.Controller, Dummy)
phi1_u = phi1
if phi1_u < 0:
phi1_u -= np.sign(state[2][0])*2*np.pi
# pendulum 2 (short) is at the bottom
phi2_u = phi2
if phi2_u < 0:
phi2_u -= np.sign(state[4][0])*2*np.pi
# calc small signal state
small_signal_state = np.zeros((6, 1))
for idx, val in enumerate(state):
small_signal_state[idx, 0] = val[0]
if settings["long pendulum"] == "o":
small_signal_state[2][0] = phi1_o - 0
if settings["long pendulum"] == "u":
small_signal_state[2][0] = phi1_u - np.pi
if settings["short pendulum"] == "o":
small_signal_state[4][0] = phi2_o - 0
if settings["short pendulum"] == "u":
small_signal_state[4][0] = phi2_u - np.pi
return small_signal_state
pm.register_simulation_module(pm.Controller, LinearStateFeedback)
pm.register_simulation_module(pm.Controller, LinearStateFeedbackParLin)
pm.register_simulation_module(pm.Controller, LinearQuadraticRegulator)
pm.register_simulation_module(pm.Controller, LjapunovController)
pm.register_simulation_module(pm.Controller, SwingUpController)
from PyQt4.QtGui import QApplication
from pymoskito import Simulator, PostProcessor,\
register_simulation_module, register_processing_module, register_visualizer, \
PostProcessingModule, \
Model, Controller
# import custom simulation modules
from model import TwoPendulumModel, TwoPendulumRigidBodyModel, TwoPendulumModelParLin
from control import LinearStateFeedback, LinearStateFeedbackParLin, LjapunovController, SwingUpController
from visualization import TwoPendulumVisualizer
from processing import TwoPendulum
if __name__ == '__main__':
# register own modules
register_simulation_module(Model, TwoPendulumModel)
register_simulation_module(Model, TwoPendulumRigidBodyModel)
register_simulation_module(Model, TwoPendulumModelParLin)
register_simulation_module(Controller, LinearStateFeedback)
register_simulation_module(Controller, LinearStateFeedbackParLin)
register_simulation_module(Controller, LjapunovController)
register_simulation_module(Controller, SwingUpController)
register_visualizer(TwoPendulumVisualizer)
register_processing_module(PostProcessingModule, TwoPendulum)
# create an Application instance (needed)
app = QApplication([])
public_settings = OrderedDict([("r0", 3), ("M", st.M), ("R", st.R),
("J", st.J), ("Jb", st.Jb), ("G", st.G),
("B", st.B), ("tick divider", 1)])
def __init__(self, settings):
# add specific "private" settings
settings.update(input_order=4)
settings.update(output_dim=1)
pm.Feedforward.__init__(self, settings)
def _calc_theta_t(self, rdd):
theta_t = -rdd / (self._settings["B"] * self._settings["G"])
return theta_t
def _feedforward(self, t, yd):
theta = []
# need the second derivative of theta
for i in range(2 + 1):
theta.append(self._calc_theta_t(yd[i + 2]))
tau = ((self._settings['M'] * self._settings['r0']**2 +
self._settings['J'] + self._settings['Jb']) * theta[2] +
self._settings['M'] * self._settings['G'] * yd[0])
return tau
pm.register_simulation_module(pm.Feedforward, LinearFeedforward)
g = -9.81 # g in inertial frame, positive means pointing in positive z direction
w_mat = sp.Matrix([[0, -wrealx, -wrealy, -wrealz],
[wrealx, 0, wrealz, -wrealy],
[wrealy, -wrealz, 0, wrealx],
[wrealz, wrealy, -wrealx, 0]])
q = sp.Matrix([[a, b, c, d]]).T
x = sp.Matrix([[a, b, c, d, bwx, bwy, bwz, bfx, bfy, bfz]]).T
dx = sp.Matrix([sp.S(1)/sp.S(2)*w_mat*q, sp.Matrix([0, 0, 0, 0, 0, 0])])
f = x + h * dx
f_scalar = sp.lambdify((a, b, c, d, bwx, bwy, bwz, bfx, bfy, bfz, wx, wy, wz, h), f)
f_lambda = lambda x_, u_, h_: f_scalar(*x_, *u_, h_)
f_jac_scalar = sp.lambdify((a, b, c, d, bwx, bwy, bwz, bfx, bfy, bfz, wx, wy, wz, h), f.jacobian(x))
f_jac = lambda x_, u_, h_: f_jac_scalar(*x_, *u_, h_)
h_fun = sp.Matrix([[-2*g*(b*d - a*c)+bfx],
[-2*g*(a*b + c*d)+bfy],
[-g*(a**2-b**2-c**2+d**2)+bfz],
[sp.atan2(2*(b*c+a*d), a**2+b**2-c**2-d**2)]])
h_scalar = sp.lambdify((a, b, c, d, bwx, bwy, bwz, bfx, bfy, bfz), h_fun)
h_lambda = lambda x_: h_scalar(*x_)
h_jac_scalar = sp.lambdify((a, b, c, d, bwx, bwy, bwz, bfx, bfy, bfz), h_fun.jacobian(x))
h_jac = lambda x_: h_jac_scalar(*x_)
return f_lambda, h_lambda, f_jac, h_jac
pm.register_simulation_module(pm.Model, Dummy)
