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)