diff --git a/.gitignore b/.gitignore index be0750cb..7baef984 100644 --- a/.gitignore +++ b/.gitignore @@ -31,4 +31,6 @@ examples/logs/ .vscode/settings.json .vscode/launch.json plots/ -saved_plots/ \ No newline at end of file +saved_plots/ + +*/debug.ipynb \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md index e5cefb86..1b275656 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## Added - Automated testing of the existing examples - Automated testing of the electric motors +- 2x3 phase PMSM environment with tests and documentation ## Changed - Changed minimal required gymnasium version to 0.29.1. - updated the code of gem-control to be compatible with gymnasium v1.0.0 diff --git a/docs/parts/environments/environment.rst b/docs/parts/environments/environment.rst index 9ca95f28..6b8f923b 100644 --- a/docs/parts/environments/environment.rst +++ b/docs/parts/environments/environment.rst @@ -8,7 +8,7 @@ In general, all environment-ids are structured as follows: - The ``ControlType`` is in ``{Finite / Cont}`` for finite control set and continuous control set action spaces - The ``ControlTask`` is in ``{TC / SC / CC}`` (Torque / Speed / Current Control) -- The ``MotorType`` is in ``{PermExDc / ExtExDc / SeriesDc / ShuntDc / PMSM / SynRM / / EESM / DFIM / SCIM }`` +- The ``MotorType`` is in ``{PermExDc / ExtExDc / SeriesDc / ShuntDc / PMSM / SynRM / / EESM / DFIM / SCIM / SIXPMSM }`` =================================================================== ============================== @@ -96,6 +96,15 @@ Speed Control DFIM Environment ``'Cont-SC- Finite Current Control DFIM Environment ``'Finite-CC-DFIM-v0'`` Current Control DFIM Environment ``'Cont-CC-DFIM-v0'`` +**Six Phase Permanent Magnet Synchronous Motor (SIXPMSM) Environments** + +Finite Torque Control SIXPMSM Environment ``'Finite-TC-SIXPMSM-v0'`` +Torque Control SIXPMSM Environment ``'Cont-TC-SIXPMSM-v0'`` +Finite Speed Control SIXPMSM Environment ``'Finite-SC-SIXPMSM-v0'`` +Speed Control SIXPMSM Environment ``'Cont-SC-SIXPMSM-v0'`` +Finite Current Control SIXPMSM Environment ``'Finite-CC-SIXPMSM-v0'`` +Current Control SIXPMSM Environment ``'Cont-CC-SIXPMSM-v0'`` + =================================================================== ============================== .. toctree:: @@ -112,6 +121,7 @@ Current Control DFIM Environment ``'Cont-CC- synrm/synrm_envs scim/scim_envs dfim/dfim_envs + sixpmsm/sixpmsm_envs Electric Motor Base Environment diff --git a/docs/parts/environments/sixpmsm/cont_cc_sixpmsm.rst b/docs/parts/environments/sixpmsm/cont_cc_sixpmsm.rst new file mode 100644 index 00000000..4f9ff30f --- /dev/null +++ b/docs/parts/environments/sixpmsm/cont_cc_sixpmsm.rst @@ -0,0 +1,4 @@ +Current Control Six Phase Permanent Magnet Synchronous Motor Environment +***************************************************************************** +.. autoclass:: gym_electric_motor.envs.ContCurrentControlSixPhasePermanentMagnetSynchronousMotorEnv + :members: diff --git a/docs/parts/environments/sixpmsm/cont_sc_sixpmsm.rst b/docs/parts/environments/sixpmsm/cont_sc_sixpmsm.rst new file mode 100644 index 00000000..64813ec6 --- /dev/null +++ b/docs/parts/environments/sixpmsm/cont_sc_sixpmsm.rst @@ -0,0 +1,4 @@ +Speed Control Six Phase Permanent Magnet Synchronous Motor Environment +**************************************************************************** +.. autoclass:: gym_electric_motor.envs.ContSpeedControlSixPhasePermanentMagnetSynchronousMotorEnv + :members: diff --git a/docs/parts/environments/sixpmsm/cont_tc_sixpmsm.rst b/docs/parts/environments/sixpmsm/cont_tc_sixpmsm.rst new file mode 100644 index 00000000..1710ce87 --- /dev/null +++ b/docs/parts/environments/sixpmsm/cont_tc_sixpmsm.rst @@ -0,0 +1,4 @@ +Torque Control Six Phase Permanent Magnet Synchronous Motor Environment +****************************************************************************** +.. autoclass:: gym_electric_motor.envs.ContTorqueControlSixPhasePermanentMagnetSynchronousMotorEnv + :members: diff --git a/docs/parts/environments/sixpmsm/finite_cc_sixpmsm.rst b/docs/parts/environments/sixpmsm/finite_cc_sixpmsm.rst new file mode 100644 index 00000000..bf1d9f8b --- /dev/null +++ b/docs/parts/environments/sixpmsm/finite_cc_sixpmsm.rst @@ -0,0 +1,4 @@ +Finite Control Set Current Control Six Phase Permanent Magnet Synchronous Motor Environment +********************************************************************************** +.. autoclass:: gym_electric_motor.envs.FiniteCurrentControlSixPhasePermanentMagnetSynchronousMotorEnv + :members: diff --git a/docs/parts/environments/sixpmsm/finite_sc_sixpmsm.rst b/docs/parts/environments/sixpmsm/finite_sc_sixpmsm.rst new file mode 100644 index 00000000..0a55b5a4 --- /dev/null +++ b/docs/parts/environments/sixpmsm/finite_sc_sixpmsm.rst @@ -0,0 +1,4 @@ +Finite Control Set Speed Control Six Phase Permanent Magnet Synchronous Motor Environment +******************************************************************************** +.. autoclass:: gym_electric_motor.envs.FiniteSpeedControlSixPhasePermanentMagnetSynchronousMotorEnv + :members: diff --git a/docs/parts/environments/sixpmsm/finite_tc_sixpmsm.rst b/docs/parts/environments/sixpmsm/finite_tc_sixpmsm.rst new file mode 100644 index 00000000..ed47bfb7 --- /dev/null +++ b/docs/parts/environments/sixpmsm/finite_tc_sixpmsm.rst @@ -0,0 +1,4 @@ +Finite Control Set Torque Control Six Phase Permanent Magnet Synchronous Motor Environment +********************************************************************************* +.. autoclass:: gym_electric_motor.envs.FiniteTorqueControlSixPhasePermanentMagnetSynchronousMotorEnv + :members: diff --git a/docs/parts/environments/sixpmsm/sixpmsm_envs.rst b/docs/parts/environments/sixpmsm/sixpmsm_envs.rst new file mode 100644 index 00000000..262d76ac --- /dev/null +++ b/docs/parts/environments/sixpmsm/sixpmsm_envs.rst @@ -0,0 +1,12 @@ +Six Phase Permanent Magnet Synchronous Motor Environments +************************************************ + + +.. toctree:: + :maxdepth: 2 + :caption: Environments: + :glob: + + * + + diff --git a/docs/parts/physical_systems/electric_motors/electric_motor.rst b/docs/parts/physical_systems/electric_motors/electric_motor.rst index 18b0902f..ec0c0e76 100644 --- a/docs/parts/physical_systems/electric_motors/electric_motor.rst +++ b/docs/parts/physical_systems/electric_motors/electric_motor.rst @@ -18,6 +18,8 @@ Electric Motors three_phase_base synchronous_base induction_base + sixphase_base + sixphase_pmsm Electric Motor Base Class ************************************* diff --git a/docs/parts/physical_systems/electric_motors/sixphase_base.rst b/docs/parts/physical_systems/electric_motors/sixphase_base.rst new file mode 100644 index 00000000..5cb7d37d --- /dev/null +++ b/docs/parts/physical_systems/electric_motors/sixphase_base.rst @@ -0,0 +1,11 @@ +Base Six Phase Motor +############################ + + + +Code Documentation +****************** + +.. autoclass:: gym_electric_motor.physical_systems.electric_motors.SixPhaseMotor + :members: + :inherited-members: diff --git a/docs/parts/physical_systems/electric_motors/sixphase_pmsm.rst b/docs/parts/physical_systems/electric_motors/sixphase_pmsm.rst new file mode 100644 index 00000000..37546b34 --- /dev/null +++ b/docs/parts/physical_systems/electric_motors/sixphase_pmsm.rst @@ -0,0 +1,26 @@ +Six Phase Permanent Magnet Synchronous Motor +################################## + + +Electrical ODE +************** + +.. math:: + \frac{\mathrm{d} i_\mathrm{d}}{\mathrm{d} t}&= \frac{u_\mathrm{d} + \omega_\mathrm{el} L_\mathrm{q} i_\mathrm{q} - R_\mathrm{s} i_\mathrm{d}}{L_\mathrm{d}} \\ + \frac{\mathrm{d} i_\mathrm{q}}{\mathrm{d} t}&= \frac{u_\mathrm{q} - \omega_\mathrm{el} L_\mathrm{d} i_\mathrm{d} - R_\mathrm{s} i_\mathrm{q} - \omega_\mathrm{el} \psi_\mathrm{PM}}{L_\mathrm{q}} \\ + \frac{\mathrm{d} i_\mathrm{x}}{\mathrm{d} t}&= \frac{u_\mathrm{x} - \omega_\mathrm{el} L_\mathrm{y} i_\mathrm{y} - R_\mathrm{s} i_\mathrm{x}}{L_\mathrm{x}} \\ + \frac{\mathrm{d} i_\mathrm{y}}{\mathrm{d} t}&= \frac{u_\mathrm{y} + \omega_\mathrm{el} L_\mathrm{x} i_\mathrm{x} - R_\mathrm{s} i_\mathrm{y}}{L_\mathrm{y}} \\ + + + +Torque Equation +*************** + +.. math:: T=\frac{3}{2} p (\mathit{\Psi}_\mathrm{p} +(L_\mathrm{d}-L_\mathrm{q})i_\mathrm{sd}) i_\mathrm{sq} + +Code Documentation +****************** + +.. autoclass:: gym_electric_motor.physical_systems.electric_motors.SixPhasePMSM + :members: + :inherited-members: diff --git a/examples/classic_controllers/demoSixPhasePMSM.py b/examples/classic_controllers/demoSixPhasePMSM.py new file mode 100644 index 00000000..9f79447a --- /dev/null +++ b/examples/classic_controllers/demoSixPhasePMSM.py @@ -0,0 +1,41 @@ +import gym_electric_motor as gem +from gym_electric_motor.envs.motors import ActionType, ControlType, Motor, MotorType +from gym_electric_motor.reference_generators import LaplaceProcessReferenceGenerator +from gym_electric_motor.visualization import MotorDashboard +from gym_electric_motor.visualization.render_modes import RenderMode +from gym_electric_motor.visualization.console_printer import ConsolePrinter +from gym_electric_motor.physical_systems import ConstantSpeedLoad + + +my_changed_voltage_supply_args = {'u_nominal': 200.0} +motor_dashboard = MotorDashboard(state_plots=("i_a1","i_a2"),render_mode=RenderMode.Figure) +load = ConstantSpeedLoad(omega_fixed=200) +motor = Motor( + MotorType.SixPhasePMSM, + ControlType.CurrentControl, + ActionType.Continuous, + ) +env = gem.make( + motor.env_id(), + supply = my_changed_voltage_supply_args, + load = load, + visualization = motor_dashboard) +terminated = True +for _ in range(1000): + if terminated: + state, reference = env.reset() + (state, reference), reward, terminated, truncated, _ = env.step(0 * env.action_space.sample()) + +motor_dashboard.initialize() +fig = motor_dashboard.figure() +fig.gca().set_ylim([-20,+20]) +fig.gca().set_xlim([0,0.02]) +fig.get_axes()[0].set_ylim([-20,+20]) +fig.gca().legend(loc='upper right') +fig.get_axes()[0].legend(loc='upper right') +fig.savefig("test") +motor_dashboard.save_to_file() + + +#console.render() +env.close() \ No newline at end of file diff --git a/src/gem_controllers/stages/operation_point_selection/eesm_ops.py b/src/gem_controllers/stages/operation_point_selection/eesm_ops.py index e31ab378..9c54ecf8 100644 --- a/src/gem_controllers/stages/operation_point_selection/eesm_ops.py +++ b/src/gem_controllers/stages/operation_point_selection/eesm_ops.py @@ -181,7 +181,7 @@ def _calculate_luts(self): self.i_e_opt = sp_interpolate.interp1d(best_params[:, 0], best_params[:, 4], kind="cubic") self.t_grid, self.psi_grid = np.mgrid[ - 0 : self.t_max : np.complex(0, self.t_grid_count), 0 : self.psi_max : np.complex(self.psi_grid_count) + 0 : self.t_max : np.complex128(0, self.t_grid_count), 0 : self.psi_max : np.complex128(self.psi_grid_count) ] self.i_d_inter = sp_interpolate.griddata( diff --git a/src/gem_controllers/stages/operation_point_selection/pmsm_ops.py b/src/gem_controllers/stages/operation_point_selection/pmsm_ops.py index 6a828b11..0925a897 100644 --- a/src/gem_controllers/stages/operation_point_selection/pmsm_ops.py +++ b/src/gem_controllers/stages/operation_point_selection/pmsm_ops.py @@ -236,8 +236,8 @@ def tune(self, env: gem.core.ElectricMotorEnvironment, env_id: str, current_safe # Define the grid for the torque and fluxes self.t_grid, self.psi_grid = np.mgrid[ - np.amin(t) : np.amax(t) : np.complex(0, self.t_count), - self.psi_min : self.psi_max : np.complex(self.psi_count), + np.amin(t) : np.amax(t) : np.complex128(0, self.t_count), + self.psi_min : self.psi_max : np.complex128(self.psi_count), ] # Interpolate the functions diff --git a/src/gym_electric_motor/__init__.py b/src/gym_electric_motor/__init__.py index 828f1163..e9f36731 100644 --- a/src/gym_electric_motor/__init__.py +++ b/src/gym_electric_motor/__init__.py @@ -281,3 +281,23 @@ register( id="Cont-SC-DFIM-v0", entry_point=envs_path + "ContSpeedControlDoublyFedInductionMotorEnv", **registration_kwargs ) + +#sixphase machines +register( + id="Cont-CC-SIXPMSM-v0", entry_point=envs_path + "ContCurrentControlSixPhasePermanentMagnetSynchronousMotorEnv", **registration_kwargs +) +register( + id="Cont-SC-SIXPMSM-v0", entry_point=envs_path + "ContSpeedControlSixPhasePermanentMagnetSynchronousMotorEnv", **registration_kwargs +) +register( + id="Cont-TC-SIXPMSM-v0", entry_point=envs_path + "ContTorqueControlSixPhasePermanentMagnetSynchronousMotorEnv", **registration_kwargs +) +register( + id="Finite-CC-SIXPMSM-v0", entry_point=envs_path + "FiniteCurrentControlSixPhasePermanentMagnetSynchronousMotorEnv", **registration_kwargs +) +register( + id="Finite-SC-SIXPMSM-v0", entry_point=envs_path + "FiniteSpeedControlSixPhasePermanentMagnetSynchronousMotorEnv", **registration_kwargs +) +register( + id="Finite-TC-SIXPMSM-v0", entry_point=envs_path + "FiniteTorqueControlSixPhasePermanentMagnetSynchronousMotorEnv", **registration_kwargs +) diff --git a/src/gym_electric_motor/envs/__init__.py b/src/gym_electric_motor/envs/__init__.py index 475000ff..e8e38a31 100644 --- a/src/gym_electric_motor/envs/__init__.py +++ b/src/gym_electric_motor/envs/__init__.py @@ -50,6 +50,26 @@ from .gym_eesm.finite_tc_eesm_env import ( FiniteTorqueControlExternallyExcitedSynchronousMotorEnv, ) +from .gym_sixphase_pmsm.cont_cc_sixpmsm_env import ( + ContCurrentControlSixPhasePermanentMagnetSynchronousMotorEnv, + +) +from .gym_sixphase_pmsm.cont_sc_sixpmsm_env import ( + ContSpeedControlSixPhasePermanentMagnetSynchronousMotorEnv, +) +from .gym_sixphase_pmsm.cont_tc_sixpmsm_env import ( + ContTorqueControlSixPhasePermanentMagnetSynchronousMotorEnv, +) +from .gym_sixphase_pmsm.finite_cc_sixpmsm_env import ( + FiniteCurrentControlSixPhasePermanentMagnetSynchronousMotorEnv, +) +from .gym_sixphase_pmsm.finite_sc_sixpmsm_env import ( + FiniteSpeedControlSixPhasePermanentMagnetSynchronousMotorEnv, +) +from .gym_sixphase_pmsm.finite_tc_sixpmsm_env import ( + FiniteTorqueControlSixPhasePermanentMagnetSynchronousMotorEnv, +) + from .gym_im import ( ContCurrentControlDoublyFedInductionMotorEnv, ContCurrentControlSquirrelCageInductionMotorEnv, diff --git a/src/gym_electric_motor/envs/gym_sixphase_pmsm/__init__.py b/src/gym_electric_motor/envs/gym_sixphase_pmsm/__init__.py new file mode 100644 index 00000000..6a1636e3 --- /dev/null +++ b/src/gym_electric_motor/envs/gym_sixphase_pmsm/__init__.py @@ -0,0 +1,6 @@ +from .cont_cc_sixpmsm_env import ContCurrentControlSixPhasePermanentMagnetSynchronousMotorEnv +from .cont_sc_sixpmsm_env import ContSpeedControlSixPhasePermanentMagnetSynchronousMotorEnv +from .cont_tc_sixpmsm_env import ContTorqueControlSixPhasePermanentMagnetSynchronousMotorEnv +from .finite_cc_sixpmsm_env import FiniteCurrentControlSixPhasePermanentMagnetSynchronousMotorEnv +from .finite_sc_sixpmsm_env import FiniteSpeedControlSixPhasePermanentMagnetSynchronousMotorEnv +from .finite_tc_sixpmsm_env import FiniteTorqueControlSixPhasePermanentMagnetSynchronousMotorEnv \ No newline at end of file diff --git a/src/gym_electric_motor/envs/gym_sixphase_pmsm/cont_cc_sixpmsm_env.py b/src/gym_electric_motor/envs/gym_sixphase_pmsm/cont_cc_sixpmsm_env.py new file mode 100644 index 00000000..68852f1b --- /dev/null +++ b/src/gym_electric_motor/envs/gym_sixphase_pmsm/cont_cc_sixpmsm_env.py @@ -0,0 +1,193 @@ + +from gym_electric_motor import physical_systems as ps +from gym_electric_motor.constraints import SquaredConstraint +from gym_electric_motor.core import ( + ElectricMotorEnvironment, + ElectricMotorVisualization, + ReferenceGenerator, + RewardFunction, +) +from gym_electric_motor.physical_systems.physical_systems import SixPhasePMSM +from gym_electric_motor.reference_generators import ( + MultipleReferenceGenerator, + WienerProcessReferenceGenerator, +) +from gym_electric_motor.reward_functions import WeightedSumOfErrors +from gym_electric_motor.utils import initialize +from gym_electric_motor.visualization import MotorDashboard + + +class ContCurrentControlSixPhasePermanentMagnetSynchronousMotorEnv(ElectricMotorEnvironment): + """ + Description: + Environment to simulate an abc-domain continuous control set current controlled six phase permanent magnet synchr. motor. + + Key: + ``'Cont-CC-SIXPMSM-v0'`` + + Default Components: + - Supply: :py:class:`.IdealVoltageSupply` + - Converter: :py:class:`.ContB6BridgeConverter` + - Motor: :py:class:`.SixPhasePMSM` + - Load: :py:class:`.ConstantSpeedLoad` + - Ode-Solver: :py:class:`.ScipyOdeSolver` + + - Reference Generator: :py:class:`.WienerProcessReferenceGenerator` *Reference Quantity:* ``'i_sd', 'i_sq', 'i_sx', 'i_sy'`` + + - Reward Function: :py:class:`.WeightedSumOfErrors` reward_weights: ``'i_sd' = 0.5, 'i_sq' = 0.5, 'i_sx' = 0.5, 'i_sy' = 0.5`` + + - Visualization: :py:class:`.MotorDashboard` current and action plots + + - Constraints: :py:class:`.SquaredConstraint` on the currents ``'i_sd', 'i_sq', 'i_sx', 'i_sy'`` + + State Variables: + ``['omega' , 'torque', 'i_a1', 'i_b1', 'i_c1', 'i_a2', 'i_b2', 'i_c2', 'i_sd', 'i_sq', 'i_sx', 'i_sy', 'u_a1', 'u_b1', 'u_c1', 'u_a2', 'u_b2', 'u_c2', 'u_sd', 'u_sq', 'u_sx', 'u_sy', 'epsilon', 'u_sup']`` + + Reference Variables: + ``['i_sd', 'i_sq', 'i_sx', 'i_sy']`` + + Control Cycle Time: + tau = 1e-4 seconds + + Observation Space: + Type: Tuple(State_Space, Reference_Space) + + State Space: + Box(low=23 * [-1], high=23 * [1]) + + Reference Space: + Box(low=[-1, -1], high=[1, 1]) + + Action Space: + Box(low=[-1] * 6, high=[1] * 6) + + Initial State: + Zeros on all state variables. + + Example: + >>> import gym_electric_motor as gem + >>> from gym_electric_motor.reference_generators import LaplaceProcessReferenceGenerator + >>> # Update the default arguments to the voltage supply by passing a parameter dict + >>> my_changed_voltage_supply_args = {'u_nominal': 400.0} + >>> + >>> # Replace the reference generator by passing a new instance + >>> my_new_ref_gen_instance = LaplaceProcessReferenceGenerator( + ... reference_state='i_sq', + ... sigma_range=(1e-3, 1e-2) + ... ) + >>> env = gem.make( + ... "Cont-CC-SIXPMSM-v0", + ... voltage_supply=my_changed_voltage_supply_args, + ... reference_generator=my_new_ref_gen_instance + ... ) + >>> terminated = True + >>> for _ in range(1000): + >>> if terminated: + >>> state, reference = env.reset() + >>> (state, reference), reward, terminated, truncated, _ = env.step(env.action_space.sample()) + """ + + def __init__( + self, + supply=None, + converter=None, + motor=None, + load=None, + ode_solver=None, + reward_function=None, + reference_generator=None, + visualization=None, + state_filter=None, + callbacks=(), + constraints=(SquaredConstraint(("i_sd", "i_sq", "i_sx", "i_sy")),), + calc_jacobian=True, + tau=1e-4, + physical_system_wrappers=(), + **kwargs, + ): + """ + Args: + supply(env-arg): Specification of the :py:class:`.VoltageSupply` for the environment + converter(env-arg): Specification of the :py:class:`.PowerElectronicConverter` for the environment + motor(env-arg): Specification of the :py:class:`.ElectricMotor` for the environment + load(env-arg): Specification of the :py:class:`.MechanicalLoad` for the environment + ode_solver(env-arg): Specification of the :py:class:`.OdeSolver` for the environment + reward_function(env-arg): Specification of the :py:class:`.RewardFunction` for the environment + reference_generator(env-arg): Specification of the :py:class:`.ReferenceGenerator` for the environment + visualization(env-arg): Specification of the :py:class:`.ElectricMotorVisualization` for the environment + constraints(iterable(str/Constraint)): All Constraints of the environment. \n + - str: A LimitConstraints for states (episode terminates, if the quantity exceeds the limit) + can be directly specified by passing the state name here (e.g. 'i', 'omega') \n + - instance of Constraint: More complex constraints (e.g. the SquaredConstraint can be initialized and + passed to the environment. + calc_jacobian(bool): Flag, if the jacobian of the environment shall be taken into account during the + simulation. This may lead to speed improvements. Default: True + tau(float): Duration of one control step in seconds. Default: 1e-4. + state_filter(list(str)): List of states that shall be returned to the agent. Default: None (no filter) + callbacks(list(Callback)): Callbacks for user interaction. Default: () + physical_system_wrappers(list(PhysicalSystemWrapper)): List of Physical System Wrappers to modify the + actions to and states from the physical system before they are used in the environment. Default: () + + Note on the env-arg type: + All parameters of type env-arg can be selected as one of the following types: + + **instance:** Pass an already instantiated object derived from the corresponding base class + (e.g. ``reward_function=MyRewardFunction()``). This is directly used in the environment. + + **dict:** Pass a dict to update the default parameters of the default type. + (e.g. ``visualization=dict(state_plots=('omega', 'u'))``) + + **str:** Pass a string out of the registered classes to select a different class for the component. + This class is then initialized with its default parameters. + The available strings can be looked up in the documentation. (e.g. ``converter='Finite-2QC'``) + """ + + default_subgenerators = ( + WienerProcessReferenceGenerator(reference_state="i_sd"), + WienerProcessReferenceGenerator(reference_state="i_sq"), + WienerProcessReferenceGenerator(reference_state="i_sx"), + WienerProcessReferenceGenerator(reference_state="i_sy"), + ) + default_sub_converters = ( + ps.ContB6BridgeConverter(), + ps.ContB6BridgeConverter(), + ) + + physical_system = SixPhasePMSM( + supply=initialize(ps.VoltageSupply, supply, ps.IdealVoltageSupply, dict(u_nominal=300.0)), + converter=initialize(ps.PowerElectronicConverter,converter,ps.ContMultiConverter,dict(subconverters=default_sub_converters),), + motor=initialize(ps.ElectricMotor, motor, ps.electric_motors.SixPhasePMSM, dict()), + load=initialize(ps.MechanicalLoad, load, ps.ConstantSpeedLoad, dict(omega_fixed=100.0)), + ode_solver=initialize(ps.OdeSolver, ode_solver, ps.ScipyOdeSolver, dict()), + calc_jacobian=calc_jacobian, + tau=tau, + ) + reference_generator = initialize( + ReferenceGenerator, + reference_generator, + MultipleReferenceGenerator, + dict(sub_generators=default_subgenerators), + ) + reward_function = initialize( + RewardFunction, + reward_function, + WeightedSumOfErrors, + dict(reward_weights=dict(i_sd=0.5, i_sq=0.5, i_sx=0.5, i_sy=0.5,)), + ) + visualization = initialize( + ElectricMotorVisualization, + visualization, + MotorDashboard, + dict(state_plots=("i_sd", "i_sq"), action_plots="all"), + ) + super().__init__( + physical_system=physical_system, + reference_generator=reference_generator, + reward_function=reward_function, + constraints=constraints, + visualization=visualization, + state_filter=state_filter, + callbacks=callbacks, + physical_system_wrappers=physical_system_wrappers, + **kwargs, + ) \ No newline at end of file diff --git a/src/gym_electric_motor/envs/gym_sixphase_pmsm/cont_sc_sixpmsm_env.py b/src/gym_electric_motor/envs/gym_sixphase_pmsm/cont_sc_sixpmsm_env.py new file mode 100644 index 00000000..61b17c21 --- /dev/null +++ b/src/gym_electric_motor/envs/gym_sixphase_pmsm/cont_sc_sixpmsm_env.py @@ -0,0 +1,185 @@ + +from gym_electric_motor import physical_systems as ps +from gym_electric_motor.constraints import SquaredConstraint +from gym_electric_motor.core import ( + ElectricMotorEnvironment, + ElectricMotorVisualization, + ReferenceGenerator, + RewardFunction, +) +from gym_electric_motor.physical_systems.physical_systems import SixPhasePMSM +from gym_electric_motor.reference_generators import ( + WienerProcessReferenceGenerator, +) +from gym_electric_motor.reward_functions import WeightedSumOfErrors +from gym_electric_motor.utils import initialize +from gym_electric_motor.visualization import MotorDashboard + + +class ContSpeedControlSixPhasePermanentMagnetSynchronousMotorEnv(ElectricMotorEnvironment): + """ + Description: + Environment to simulate an abc-domain continuous control set speed controlled six phase permanent magnet synchr. motor. + + Key: + ``'Cont-SC-SIXPMSM-v0'`` + + Default Components: + - Supply: :py:class:`.IdealVoltageSupply` + - Converter: :py:class:`.ContB6BridgeConverter` + - Motor: :py:class:`.SixPhasePMSM` + - Load: :py:class:`.PolynomialStaticLoad` + - Ode-Solver: :py:class:`.ScipyOdeSolver` + + - Reference Generator: :py:class:`.WienerProcessReferenceGenerator` *Reference Quantity:* ``'omega'`` + + - Reward Function: :py:class:`.WeightedSumOfErrors` reward_weights: ``'omega' = 1.0`` + + - Visualization: :py:class:`.MotorDashboard` current and action plots + + - Constraints: :py:class:`.SquaredConstraint` on the currents ``'i_sd', 'i_sq', 'i_sx', 'i_sy'`` + + State Variables: + ``['omega' , 'torque', 'i_a1', 'i_b1', 'i_c1', 'i_a2', 'i_b2', 'i_c2', 'i_sd', 'i_sq', 'i_sx', 'i_sy', 'u_a1', 'u_b1', 'u_c1', 'u_a2', 'u_b2', 'u_c2', 'u_sd', 'u_sq', 'u_sx', 'u_sy', 'epsilon', 'u_sup']`` + + Reference Variables: + ``['omega']`` + + Control Cycle Time: + tau = 1e-4 seconds + + Observation Space: + Type: Tuple(State_Space, Reference_Space) + + State Space: + Box(low=23 * [-1], high=23 * [1]) + + Reference Space: + Box(low=[-1, -1], high=[1, 1]) + + Action Space: + Box(low=[-1] * 6, high=[1] * 6) + + Initial State: + Zeros on all state variables. + + Example: + >>> import gym_electric_motor as gem + >>> from gym_electric_motor.reference_generators import LaplaceProcessReferenceGenerator + >>> # Update the default arguments to the voltage supply by passing a parameter dict + >>> my_changed_voltage_supply_args = {'u_nominal': 400.0} + >>> + >>> # Replace the reference generator by passing a new instance + >>> my_new_ref_gen_instance = LaplaceProcessReferenceGenerator( + ... reference_state='i_sq', + ... sigma_range=(1e-3, 1e-2) + ... ) + >>> env = gem.make( + ... "Cont-SC-SIXPMSM-v0", + ... voltage_supply=my_changed_voltage_supply_args, + ... reference_generator=my_new_ref_gen_instance + ... ) + >>> terminated = True + >>> for _ in range(1000): + >>> if terminated: + >>> state, reference = env.reset() + >>> (state, reference), reward, terminated, truncated, _ = env.step(env.action_space.sample()) + """ + def __init__( + self, + supply=None, + converter=None, + motor=None, + load=None, + ode_solver=None, + reward_function=None, + reference_generator=None, + visualization=None, + state_filter=None, + callbacks=(), + constraints=(SquaredConstraint(("i_sq", "i_sd", "i_sx", "i_sy")),), + calc_jacobian=True, + tau=1e-4, + physical_system_wrappers=(), + **kwargs, + ): + """ + Args: + supply(env-arg): Specification of the :py:class:`.VoltageSupply` for the environment + converter(env-arg): Specification of the :py:class:`.PowerElectronicConverter` for the environment + motor(env-arg): Specification of the :py:class:`.ElectricMotor` for the environment + load(env-arg): Specification of the :py:class:`.MechanicalLoad` for the environment + ode_solver(env-arg): Specification of the :py:class:`.OdeSolver` for the environment + reward_function(env-arg): Specification of the :py:class:`.RewardFunction` for the environment + reference_generator(env-arg): Specification of the :py:class:`.ReferenceGenerator` for the environment + visualization(env-arg): Specification of the :py:class:`.ElectricMotorVisualization` for the environment + constraints(iterable(str/Constraint)): All Constraints of the environment. \n + - str: A LimitConstraints for states (episode terminates, if the quantity exceeds the limit) + can be directly specified by passing the state name here (e.g. 'i', 'omega') \n + - instance of Constraint: More complex constraints (e.g. the SquaredConstraint can be initialized and + passed to the environment. + calc_jacobian(bool): Flag, if the jacobian of the environment shall be taken into account during the + simulation. This may lead to speed improvements. Default: True + tau(float): Duration of one control step in seconds. Default: 1e-4. + state_filter(list(str)): List of states that shall be returned to the agent. Default: None (no filter) + callbacks(list(Callback)): Callbacks for user interaction. Default: () + physical_system_wrappers(list(PhysicalSystemWrapper)): List of Physical System Wrappers to modify the + actions to and states from the physical system before they are used in the environment. Default: () + + Note on the env-arg type: + All parameters of type env-arg can be selected as one of the following types: + + **instance:** Pass an already instantiated object derived from the corresponding base class + (e.g. ``reward_function=MyRewardFunction()``). This is directly used in the environment. + + **dict:** Pass a dict to update the default parameters of the default type. + (e.g. ``visualization=dict(state_plots=('omega', 'u'))``) + + **str:** Pass a string out of the registered classes to select a different class for the component. + This class is then initialized with its default parameters. + The available strings can be looked up in the documentation. (e.g. ``converter='Finite-2QC'``) + """ + + default_sub_converters = ( + ps.ContB6BridgeConverter(), + ps.ContB6BridgeConverter(), + ) + + physical_system = SixPhasePMSM( + supply=initialize(ps.VoltageSupply, supply, ps.IdealVoltageSupply, dict(u_nominal=300.0)), + converter=initialize(ps.PowerElectronicConverter,converter,ps.ContMultiConverter,dict(subconverters=default_sub_converters),), + motor=initialize(ps.ElectricMotor, motor, ps.electric_motors.SixPhasePMSM, dict()), + load=initialize(ps.MechanicalLoad, load, ps.PolynomialStaticLoad, dict(load_parameter=dict(a=0.01, b=0.01, c=0.0))), + ode_solver=initialize(ps.OdeSolver, ode_solver, ps.ScipyOdeSolver, dict()), + calc_jacobian=calc_jacobian, + tau=tau, + ) + reference_generator = initialize( + ReferenceGenerator, + reference_generator, + WienerProcessReferenceGenerator, + dict(reference_state="omega"), + ) + reward_function = initialize( + RewardFunction, + reward_function, + WeightedSumOfErrors, + dict(reward_weights=dict(omega=1.0)), + ) + visualization = initialize( + ElectricMotorVisualization, + visualization, + MotorDashboard, + dict(state_plots=("omega",), action_plots="all"), + ) + super().__init__( + physical_system=physical_system, + reference_generator=reference_generator, + reward_function=reward_function, + constraints=constraints, + visualization=visualization, + state_filter=state_filter, + callbacks=callbacks, + physical_system_wrappers=physical_system_wrappers, + **kwargs, + ) \ No newline at end of file diff --git a/src/gym_electric_motor/envs/gym_sixphase_pmsm/cont_tc_sixpmsm_env.py b/src/gym_electric_motor/envs/gym_sixphase_pmsm/cont_tc_sixpmsm_env.py new file mode 100644 index 00000000..e6a561c6 --- /dev/null +++ b/src/gym_electric_motor/envs/gym_sixphase_pmsm/cont_tc_sixpmsm_env.py @@ -0,0 +1,186 @@ + +from gym_electric_motor import physical_systems as ps +from gym_electric_motor.constraints import SquaredConstraint +from gym_electric_motor.core import ( + ElectricMotorEnvironment, + ElectricMotorVisualization, + ReferenceGenerator, + RewardFunction, +) +from gym_electric_motor.physical_systems.physical_systems import SixPhasePMSM +from gym_electric_motor.reference_generators import ( + WienerProcessReferenceGenerator, +) +from gym_electric_motor.reward_functions import WeightedSumOfErrors +from gym_electric_motor.utils import initialize +from gym_electric_motor.visualization import MotorDashboard + + +class ContTorqueControlSixPhasePermanentMagnetSynchronousMotorEnv(ElectricMotorEnvironment): + """ + Description: + Environment to simulate an abc-domain continuous control set torque controlled six phase permanent magnet synchr. motor. + + Key: + ``'Cont-TC-SIXPMSM-v0'`` + + Default Components: + - Supply: :py:class:`.IdealVoltageSupply` + - Converter: :py:class:`.ContB6BridgeConverter` + - Motor: :py:class:`.SixPhasePMSM` + - Load: :py:class:`.ConstantSpeedLoad` + - Ode-Solver: :py:class:`.ScipyOdeSolver` + + - Reference Generator: :py:class:`.WienerProcessReferenceGenerator` *Reference Quantity:* ``'torque'`` + + - Reward Function: :py:class:`.WeightedSumOfErrors` reward_weights: ``'torque' = 1.0`` + + - Visualization: :py:class:`.MotorDashboard` current and action plots + + - Constraints: :py:class:`.SquaredConstraint` on the currents ``'i_sd', 'i_sq', 'i_sx', 'i_sy'`` + + State Variables: + ``['omega' , 'torque', 'i_a1', 'i_b1', 'i_c1', 'i_a2', 'i_b2', 'i_c2', 'i_sd', 'i_sq', 'i_sx', 'i_sy', 'u_a1', 'u_b1', 'u_c1', 'u_a2', 'u_b2', 'u_c2', 'u_sd', 'u_sq', 'u_sx', 'u_sy', 'epsilon', 'u_sup']`` + + Reference Variables: + ``['torque']`` + + Control Cycle Time: + tau = 1e-4 seconds + + Observation Space: + Type: Tuple(State_Space, Reference_Space) + + State Space: + Box(low=23 * [-1], high=23 * [1]) + + Reference Space: + Box(low=[-1, -1], high=[1, 1]) + + Action Space: + Box(low=[-1] * 6, high=[1] * 6) + + Initial State: + Zeros on all state variables. + + Example: + >>> import gym_electric_motor as gem + >>> from gym_electric_motor.reference_generators import LaplaceProcessReferenceGenerator + >>> # Update the default arguments to the voltage supply by passing a parameter dict + >>> my_changed_voltage_supply_args = {'u_nominal': 400.0} + >>> + >>> # Replace the reference generator by passing a new instance + >>> my_new_ref_gen_instance = LaplaceProcessReferenceGenerator( + ... reference_state='i_sq', + ... sigma_range=(1e-3, 1e-2) + ... ) + >>> env = gem.make( + ... "Cont-TC-SIXPMSM-v0", + ... voltage_supply=my_changed_voltage_supply_args, + ... reference_generator=my_new_ref_gen_instance + ... ) + >>> terminated = True + >>> for _ in range(1000): + >>> if terminated: + >>> state, reference = env.reset() + >>> (state, reference), reward, terminated, truncated, _ = env.step(env.action_space.sample()) + """ + + def __init__( + self, + supply=None, + converter=None, + motor=None, + load=None, + ode_solver=None, + reward_function=None, + reference_generator=None, + visualization=None, + state_filter=None, + callbacks=(), + constraints=(SquaredConstraint(("i_sq", "i_sd", "i_sx", "i_sy")),), + calc_jacobian=True, + tau=1e-4, + physical_system_wrappers=(), + **kwargs, + ): + """ + Args: + supply(env-arg): Specification of the :py:class:`.VoltageSupply` for the environment + converter(env-arg): Specification of the :py:class:`.PowerElectronicConverter` for the environment + motor(env-arg): Specification of the :py:class:`.ElectricMotor` for the environment + load(env-arg): Specification of the :py:class:`.MechanicalLoad` for the environment + ode_solver(env-arg): Specification of the :py:class:`.OdeSolver` for the environment + reward_function(env-arg): Specification of the :py:class:`.RewardFunction` for the environment + reference_generator(env-arg): Specification of the :py:class:`.ReferenceGenerator` for the environment + visualization(env-arg): Specification of the :py:class:`.ElectricMotorVisualization` for the environment + constraints(iterable(str/Constraint)): All Constraints of the environment. \n + - str: A LimitConstraints for states (episode terminates, if the quantity exceeds the limit) + can be directly specified by passing the state name here (e.g. 'i', 'omega') \n + - instance of Constraint: More complex constraints (e.g. the SquaredConstraint can be initialized and + passed to the environment. + calc_jacobian(bool): Flag, if the jacobian of the environment shall be taken into account during the + simulation. This may lead to speed improvements. Default: True + tau(float): Duration of one control step in seconds. Default: 1e-4. + state_filter(list(str)): List of states that shall be returned to the agent. Default: None (no filter) + callbacks(list(Callback)): Callbacks for user interaction. Default: () + physical_system_wrappers(list(PhysicalSystemWrapper)): List of Physical System Wrappers to modify the + actions to and states from the physical system before they are used in the environment. Default: () + + Note on the env-arg type: + All parameters of type env-arg can be selected as one of the following types: + + **instance:** Pass an already instantiated object derived from the corresponding base class + (e.g. ``reward_function=MyRewardFunction()``). This is directly used in the environment. + + **dict:** Pass a dict to update the default parameters of the default type. + (e.g. ``visualization=dict(state_plots=('omega', 'u'))``) + + **str:** Pass a string out of the registered classes to select a different class for the component. + This class is then initialized with its default parameters. + The available strings can be looked up in the documentation. (e.g. ``converter='Finite-2QC'``) + """ + + default_sub_converters = ( + ps.ContB6BridgeConverter(), + ps.ContB6BridgeConverter(), + ) + + physical_system = SixPhasePMSM( + supply=initialize(ps.VoltageSupply, supply, ps.IdealVoltageSupply, dict(u_nominal=300.0)), + converter=initialize(ps.PowerElectronicConverter,converter,ps.ContMultiConverter,dict(subconverters=default_sub_converters),), + motor=initialize(ps.ElectricMotor, motor, ps.electric_motors.SixPhasePMSM, dict()), + load=initialize(ps.MechanicalLoad, load, ps.ConstantSpeedLoad, dict(omega_fixed=100.0)), + ode_solver=initialize(ps.OdeSolver, ode_solver, ps.ScipyOdeSolver, dict()), + calc_jacobian=calc_jacobian, + tau=tau, + ) + reference_generator = initialize( + ReferenceGenerator, + reference_generator, + WienerProcessReferenceGenerator, + dict(reference_state="torque"), + ) + reward_function = initialize( + RewardFunction, + reward_function, + WeightedSumOfErrors, + dict(reward_weights=dict(torque=1.0)), + ) + visualization = initialize( + ElectricMotorVisualization, + visualization, + MotorDashboard, + dict(state_plots=("torque",), action_plots="all"), + ) + super().__init__( + physical_system=physical_system, + reference_generator=reference_generator, + reward_function=reward_function, + constraints=constraints, + visualization=visualization, + state_filter=state_filter, + callbacks=callbacks, + physical_system_wrappers=physical_system_wrappers, + **kwargs, + ) \ No newline at end of file diff --git a/src/gym_electric_motor/envs/gym_sixphase_pmsm/finite_cc_sixpmsm_env.py b/src/gym_electric_motor/envs/gym_sixphase_pmsm/finite_cc_sixpmsm_env.py new file mode 100644 index 00000000..9efd89a3 --- /dev/null +++ b/src/gym_electric_motor/envs/gym_sixphase_pmsm/finite_cc_sixpmsm_env.py @@ -0,0 +1,193 @@ + +from gym_electric_motor import physical_systems as ps +from gym_electric_motor.constraints import SquaredConstraint +from gym_electric_motor.core import ( + ElectricMotorEnvironment, + ElectricMotorVisualization, + ReferenceGenerator, + RewardFunction, +) +from gym_electric_motor.physical_systems.physical_systems import SixPhasePMSM +from gym_electric_motor.reference_generators import ( + MultipleReferenceGenerator, + WienerProcessReferenceGenerator, +) +from gym_electric_motor.reward_functions import WeightedSumOfErrors +from gym_electric_motor.utils import initialize +from gym_electric_motor.visualization import MotorDashboard + + +class FiniteCurrentControlSixPhasePermanentMagnetSynchronousMotorEnv(ElectricMotorEnvironment): + """ + Description: + Environment to simulate finite control set current controlled six phase permanent magnet synchr. motor. + + Key: + ``'Finite-CC-SIXPMSM-v0'`` + + Default Components: + - Supply: :py:class:`.IdealVoltageSupply` + - Converter: :py:class:`.FiniteB6BridgeConverter` + - Motor: :py:class:`.SixPhasePMSM` + - Load: :py:class:`.ConstantSpeedLoad` + - Ode-Solver: :py:class:`.ScipyOdeSolver` + + - Reference Generator: :py:class:`.WienerProcessReferenceGenerator` *Reference Quantity:* ``'i_sd', 'i_sq', 'i_sx', 'i_sy'`` + + - Reward Function: :py:class:`.WeightedSumOfErrors` reward_weights: ``'i_sd' = 0.5, 'i_sq' = 0.5, 'i_sx' = 0.5, 'i_sy' = 0.5`` + + - Visualization: :py:class:`.MotorDashboard` current and action plots + + - Constraints: :py:class:`.SquaredConstraint` on the currents ``'i_sd', 'i_sq', 'i_sx', 'i_sy'`` + + State Variables: + ``['omega' , 'torque', 'i_a1', 'i_b1', 'i_c1', 'i_a2', 'i_b2', 'i_c2', 'i_sd', 'i_sq', 'i_sx', 'i_sy', 'u_a1', 'u_b1', 'u_c1', 'u_a2', 'u_b2', 'u_c2', 'u_sd', 'u_sq', 'u_sx', 'u_sy', 'epsilon', 'u_sup']`` + + Reference Variables: + ``['i_sd', 'i_sq', 'i_sx', 'i_sy']`` + + Control Cycle Time: + tau = 1e-4 seconds + + Observation Space: + Type: Tuple(State_Space, Reference_Space) + + State Space: + Box(low=23 * [-1], high=23 * [1]) + + Reference Space: + Box(low=[-1, -1], high=[1, 1]) + + Action Space: + + + Initial State: + Zeros on all state variables. + + Example: + >>> import gym_electric_motor as gem + >>> from gym_electric_motor.reference_generators import LaplaceProcessReferenceGenerator + >>> # Update the default arguments to the voltage supply by passing a parameter dict + >>> my_changed_voltage_supply_args = {'u_nominal': 400.0} + >>> + >>> # Replace the reference generator by passing a new instance + >>> my_new_ref_gen_instance = LaplaceProcessReferenceGenerator( + ... reference_state='i_sq', + ... sigma_range=(1e-3, 1e-2) + ... ) + >>> env = gem.make( + ... "Finite-CC-SIXPMSM-v0", + ... voltage_supply=my_changed_voltage_supply_args, + ... reference_generator=my_new_ref_gen_instance + ... ) + >>> terminated = True + >>> for _ in range(1000): + >>> if terminated: + >>> state, reference = env.reset() + >>> (state, reference), reward, terminated, truncated, _ = env.step(env.action_space.sample()) + """ + + def __init__( + self, + supply=None, + converter=None, + motor=None, + load=None, + ode_solver=None, + reward_function=None, + reference_generator=None, + visualization=None, + state_filter=None, + callbacks=(), + constraints=(SquaredConstraint(("i_sq", "i_sd", "i_sx", "i_sy")),), + calc_jacobian=True, + tau=1e-4, + physical_system_wrappers=(), + **kwargs, + ): + """ + Args: + supply(env-arg): Specification of the :py:class:`.VoltageSupply` for the environment + converter(env-arg): Specification of the :py:class:`.PowerElectronicConverter` for the environment + motor(env-arg): Specification of the :py:class:`.ElectricMotor` for the environment + load(env-arg): Specification of the :py:class:`.MechanicalLoad` for the environment + ode_solver(env-arg): Specification of the :py:class:`.OdeSolver` for the environment + reward_function(env-arg): Specification of the :py:class:`.RewardFunction` for the environment + reference_generator(env-arg): Specification of the :py:class:`.ReferenceGenerator` for the environment + visualization(env-arg): Specification of the :py:class:`.ElectricMotorVisualization` for the environment + constraints(iterable(str/Constraint)): All Constraints of the environment. \n + - str: A LimitConstraints for states (episode terminates, if the quantity exceeds the limit) + can be directly specified by passing the state name here (e.g. 'i', 'omega') \n + - instance of Constraint: More complex constraints (e.g. the SquaredConstraint can be initialized and + passed to the environment. + calc_jacobian(bool): Flag, if the jacobian of the environment shall be taken into account during the + simulation. This may lead to speed improvements. Default: True + tau(float): Duration of one control step in seconds. Default: 1e-4. + state_filter(list(str)): List of states that shall be returned to the agent. Default: None (no filter) + callbacks(list(Callback)): Callbacks for user interaction. Default: () + physical_system_wrappers(list(PhysicalSystemWrapper)): List of Physical System Wrappers to modify the + actions to and states from the physical system before they are used in the environment. Default: () + + Note on the env-arg type: + All parameters of type env-arg can be selected as one of the following types: + + **instance:** Pass an already instantiated object derived from the corresponding base class + (e.g. ``reward_function=MyRewardFunction()``). This is directly used in the environment. + + **dict:** Pass a dict to update the default parameters of the default type. + (e.g. ``visualization=dict(state_plots=('omega', 'u'))``) + + **str:** Pass a string out of the registered classes to select a different class for the component. + This class is then initialized with its default parameters. + The available strings can be looked up in the documentation. (e.g. ``converter='Finite-2QC'``) + """ + + default_subgenerators = ( + WienerProcessReferenceGenerator(reference_state="i_sd"), + WienerProcessReferenceGenerator(reference_state="i_sq"), + WienerProcessReferenceGenerator(reference_state="i_sx"), + WienerProcessReferenceGenerator(reference_state="i_sy"), + ) + default_sub_converters = ( + ps.FiniteB6BridgeConverter(), + ps.FiniteB6BridgeConverter(), + ) + + physical_system = SixPhasePMSM( + supply=initialize(ps.VoltageSupply, supply, ps.IdealVoltageSupply, dict(u_nominal=300.0)), + converter=initialize(ps.PowerElectronicConverter,converter,ps.FiniteMultiConverter,dict(subconverters=default_sub_converters),), + motor=initialize(ps.ElectricMotor, motor, ps.electric_motors.SixPhasePMSM, dict()), + load=initialize(ps.MechanicalLoad, load, ps.ConstantSpeedLoad, dict(omega_fixed=100.0)), + ode_solver=initialize(ps.OdeSolver, ode_solver, ps.ScipyOdeSolver, dict()), + calc_jacobian=calc_jacobian, + tau=tau, + ) + reference_generator = initialize( + ReferenceGenerator, + reference_generator, + MultipleReferenceGenerator, + dict(sub_generators=default_subgenerators), + ) + reward_function = initialize( + RewardFunction, + reward_function, + WeightedSumOfErrors, + dict(reward_weights=dict(i_sd=0.5, i_sq=0.5, i_sx=0.5, i_sy=0.5,)), + ) + visualization = initialize( + ElectricMotorVisualization, + visualization, + MotorDashboard, + dict(state_plots=("i_sd", "i_sq"), action_plots="all"), + ) + super().__init__( + physical_system=physical_system, + reference_generator=reference_generator, + reward_function=reward_function, + constraints=constraints, + visualization=visualization, + state_filter=state_filter, + callbacks=callbacks, + physical_system_wrappers=physical_system_wrappers, + **kwargs, + ) \ No newline at end of file diff --git a/src/gym_electric_motor/envs/gym_sixphase_pmsm/finite_sc_sixpmsm_env.py b/src/gym_electric_motor/envs/gym_sixphase_pmsm/finite_sc_sixpmsm_env.py new file mode 100644 index 00000000..e7ba6904 --- /dev/null +++ b/src/gym_electric_motor/envs/gym_sixphase_pmsm/finite_sc_sixpmsm_env.py @@ -0,0 +1,186 @@ + +from gym_electric_motor import physical_systems as ps +from gym_electric_motor.constraints import SquaredConstraint +from gym_electric_motor.core import ( + ElectricMotorEnvironment, + ElectricMotorVisualization, + ReferenceGenerator, + RewardFunction, +) +from gym_electric_motor.physical_systems.physical_systems import SixPhasePMSM +from gym_electric_motor.reference_generators import ( + WienerProcessReferenceGenerator, +) +from gym_electric_motor.reward_functions import WeightedSumOfErrors +from gym_electric_motor.utils import initialize +from gym_electric_motor.visualization import MotorDashboard + + +class FiniteSpeedControlSixPhasePermanentMagnetSynchronousMotorEnv(ElectricMotorEnvironment): + """ + Description: + Environment to simulate Finite control set speed controlled six phase permanent magnet synchr. motor. + + Key: + ``'Finite-SC-SIXPMSM-v0'`` + + Default Components: + - Supply: :py:class:`.IdealVoltageSupply` + - Converter: :py:class:`.FiniteB6BridgeConverter` + - Motor: :py:class:`.SixPhasePMSM` + - Load: :py:class:`.PolynomialStaticLoad` + - Ode-Solver: :py:class:`.ScipyOdeSolver` + + - Reference Generator: :py:class:`.WienerProcessReferenceGenerator` *Reference Quantity:* ``'omega'`` + + - Reward Function: :py:class:`.WeightedSumOfErrors` reward_weights: ``'omega' = 1.0`` + + - Visualization: :py:class:`.MotorDashboard` current and action plots + + - Constraints: :py:class:`.SquaredConstraint` on the currents ``'i_sd', 'i_sq', 'i_sx', 'i_sy'`` + + State Variables: + ``['omega' , 'torque', 'i_a1', 'i_b1', 'i_c1', 'i_a2', 'i_b2', 'i_c2', 'i_sd', 'i_sq', 'i_sx', 'i_sy', 'u_a1', 'u_b1', 'u_c1', 'u_a2', 'u_b2', 'u_c2', 'u_sd', 'u_sq', 'u_sx', 'u_sy', 'epsilon', 'u_sup']`` + + Reference Variables: + ``['omega']`` + + Control Cycle Time: + tau = 1e-4 seconds + + Observation Space: + Type: Tuple(State_Space, Reference_Space) + + State Space: + Box(low=23 * [-1], high=23 * [1]) + + Reference Space: + Box(low=[-1, -1], high=[1, 1]) + + Action Space: + + + Initial State: + Zeros on all state variables. + + Example: + >>> import gym_electric_motor as gem + >>> from gym_electric_motor.reference_generators import LaplaceProcessReferenceGenerator + >>> # Update the default arguments to the voltage supply by passing a parameter dict + >>> my_changed_voltage_supply_args = {'u_nominal': 400.0} + >>> + >>> # Replace the reference generator by passing a new instance + >>> my_new_ref_gen_instance = LaplaceProcessReferenceGenerator( + ... reference_state='i_sq', + ... sigma_range=(1e-3, 1e-2) + ... ) + >>> env = gem.make( + ... "Finite-SC-SIXPMSM-v0", + ... voltage_supply=my_changed_voltage_supply_args, + ... reference_generator=my_new_ref_gen_instance + ... ) + >>> terminated = True + >>> for _ in range(1000): + >>> if terminated: + >>> state, reference = env.reset() + >>> (state, reference), reward, terminated, truncated, _ = env.step(env.action_space.sample()) + """ + + def __init__( + self, + supply=None, + converter=None, + motor=None, + load=None, + ode_solver=None, + reward_function=None, + reference_generator=None, + visualization=None, + state_filter=None, + callbacks=(), + constraints=(SquaredConstraint(("i_sq", "i_sd", "i_sx", "i_sy")),), + calc_jacobian=True, + tau=1e-4, + physical_system_wrappers=(), + **kwargs, + ): + """ + Args: + supply(env-arg): Specification of the :py:class:`.VoltageSupply` for the environment + converter(env-arg): Specification of the :py:class:`.PowerElectronicConverter` for the environment + motor(env-arg): Specification of the :py:class:`.ElectricMotor` for the environment + load(env-arg): Specification of the :py:class:`.MechanicalLoad` for the environment + ode_solver(env-arg): Specification of the :py:class:`.OdeSolver` for the environment + reward_function(env-arg): Specification of the :py:class:`.RewardFunction` for the environment + reference_generator(env-arg): Specification of the :py:class:`.ReferenceGenerator` for the environment + visualization(env-arg): Specification of the :py:class:`.ElectricMotorVisualization` for the environment + constraints(iterable(str/Constraint)): All Constraints of the environment. \n + - str: A LimitConstraints for states (episode terminates, if the quantity exceeds the limit) + can be directly specified by passing the state name here (e.g. 'i', 'omega') \n + - instance of Constraint: More complex constraints (e.g. the SquaredConstraint can be initialized and + passed to the environment. + calc_jacobian(bool): Flag, if the jacobian of the environment shall be taken into account during the + simulation. This may lead to speed improvements. Default: True + tau(float): Duration of one control step in seconds. Default: 1e-4. + state_filter(list(str)): List of states that shall be returned to the agent. Default: None (no filter) + callbacks(list(Callback)): Callbacks for user interaction. Default: () + physical_system_wrappers(list(PhysicalSystemWrapper)): List of Physical System Wrappers to modify the + actions to and states from the physical system before they are used in the environment. Default: () + + Note on the env-arg type: + All parameters of type env-arg can be selected as one of the following types: + + **instance:** Pass an already instantiated object derived from the corresponding base class + (e.g. ``reward_function=MyRewardFunction()``). This is directly used in the environment. + + **dict:** Pass a dict to update the default parameters of the default type. + (e.g. ``visualization=dict(state_plots=('omega', 'u'))``) + + **str:** Pass a string out of the registered classes to select a different class for the component. + This class is then initialized with its default parameters. + The available strings can be looked up in the documentation. (e.g. ``converter='Finite-2QC'``) + """ + + default_sub_converters = ( + ps.FiniteB6BridgeConverter(), + ps.FiniteB6BridgeConverter(), + ) + + physical_system = SixPhasePMSM( + supply=initialize(ps.VoltageSupply, supply, ps.IdealVoltageSupply, dict(u_nominal=300.0)), + converter=initialize(ps.PowerElectronicConverter,converter,ps.FiniteMultiConverter,dict(subconverters=default_sub_converters),), + motor=initialize(ps.ElectricMotor, motor, ps.electric_motors.SixPhasePMSM, dict()), + load=initialize(ps.MechanicalLoad, load, ps.PolynomialStaticLoad, dict(load_parameter=dict(a=0.01, b=0.01, c=0.0))), + ode_solver=initialize(ps.OdeSolver, ode_solver, ps.ScipyOdeSolver, dict()), + calc_jacobian=calc_jacobian, + tau=tau, + ) + reference_generator = initialize( + ReferenceGenerator, + reference_generator, + WienerProcessReferenceGenerator, + dict(reference_state="omega"), + ) + reward_function = initialize( + RewardFunction, + reward_function, + WeightedSumOfErrors, + dict(reward_weights=dict(omega=1.0)), + ) + visualization = initialize( + ElectricMotorVisualization, + visualization, + MotorDashboard, + dict(state_plots=("omega",), action_plots="all"), + ) + super().__init__( + physical_system=physical_system, + reference_generator=reference_generator, + reward_function=reward_function, + constraints=constraints, + visualization=visualization, + state_filter=state_filter, + callbacks=callbacks, + physical_system_wrappers=physical_system_wrappers, + **kwargs, + ) \ No newline at end of file diff --git a/src/gym_electric_motor/envs/gym_sixphase_pmsm/finite_tc_sixpmsm_env.py b/src/gym_electric_motor/envs/gym_sixphase_pmsm/finite_tc_sixpmsm_env.py new file mode 100644 index 00000000..79e83109 --- /dev/null +++ b/src/gym_electric_motor/envs/gym_sixphase_pmsm/finite_tc_sixpmsm_env.py @@ -0,0 +1,185 @@ + +from gym_electric_motor import physical_systems as ps +from gym_electric_motor.constraints import SquaredConstraint +from gym_electric_motor.core import ( + ElectricMotorEnvironment, + ElectricMotorVisualization, + ReferenceGenerator, + RewardFunction, +) +from gym_electric_motor.physical_systems.physical_systems import SixPhasePMSM +from gym_electric_motor.reference_generators import ( + WienerProcessReferenceGenerator, +) +from gym_electric_motor.reward_functions import WeightedSumOfErrors +from gym_electric_motor.utils import initialize +from gym_electric_motor.visualization import MotorDashboard + + +class FiniteTorqueControlSixPhasePermanentMagnetSynchronousMotorEnv(ElectricMotorEnvironment): + """ + Description: + Environment to simulate Finite control set torque controlled six phase permanent magnet synchr. motor. + + Key: + ``' Finite-TC-SIXPMSM-v0'`` + + Default Components: + - Supply: :py:class:`.IdealVoltageSupply` + - Converter: :py:class:`.FiniteB6BridgeConverter` + - Motor: :py:class:`.SixPhasePMSM` + - Load: :py:class:`.ConstantSpeedLoad` + - Ode-Solver: :py:class:`.ScipyOdeSolver` + + - Reference Generator: :py:class:`.WienerProcessReferenceGenerator` *Reference Quantity:* ``'torque'`` + + - Reward Function: :py:class:`.WeightedSumOfErrors` reward_weights: ``'torque' = 1.0`` + + - Visualization: :py:class:`.MotorDashboard` current and action plots + + - Constraints: :py:class:`.SquaredConstraint` on the currents ``'i_sd', 'i_sq', 'i_sx', 'i_sy'`` + + State Variables: + ``['omega' , 'torque', 'i_a1', 'i_b1', 'i_c1', 'i_a2', 'i_b2', 'i_c2', 'i_sd', 'i_sq', 'i_sx', 'i_sy', 'u_a1', 'u_b1', 'u_c1', 'u_a2', 'u_b2', 'u_c2', 'u_sd', 'u_sq', 'u_sx', 'u_sy', 'epsilon', 'u_sup']`` + + Reference Variables: + ``['torque']`` + + Control Cycle Time: + tau = 1e-4 seconds + + Observation Space: + Type: Tuple(State_Space, Reference_Space) + + State Space: + Box(low=23 * [-1], high=23 * [1]) + + Reference Space: + Box(low=[-1, -1], high=[1, 1]) + + Action Space: + + + Initial State: + Zeros on all state variables. + + Example: + >>> import gym_electric_motor as gem + >>> from gym_electric_motor.reference_generators import LaplaceProcessReferenceGenerator + >>> # Update the default arguments to the voltage supply by passing a parameter dict + >>> my_changed_voltage_supply_args = {'u_nominal': 400.0} + >>> + >>> # Replace the reference generator by passing a new instance + >>> my_new_ref_gen_instance = LaplaceProcessReferenceGenerator( + ... reference_state='i_sq', + ... sigma_range=(1e-3, 1e-2) + ... ) + >>> env = gem.make( + ... "Finite-TC-SIXPMSM-v0", + ... voltage_supply=my_changed_voltage_supply_args, + ... reference_generator=my_new_ref_gen_instance + ... ) + >>> terminated = True + >>> for _ in range(1000): + >>> if terminated: + >>> state, reference = env.reset() + >>> (state, reference), reward, terminated, truncated, _ = env.step(env.action_space.sample()) + """ + def __init__( + self, + supply=None, + converter=None, + motor=None, + load=None, + ode_solver=None, + reward_function=None, + reference_generator=None, + visualization=None, + state_filter=None, + callbacks=(), + constraints=(SquaredConstraint(("i_sq", "i_sd", "i_sx", "i_sy")),), + calc_jacobian=True, + tau=1e-4, + physical_system_wrappers=(), + **kwargs, + ): + """ + Args: + supply(env-arg): Specification of the :py:class:`.VoltageSupply` for the environment + converter(env-arg): Specification of the :py:class:`.PowerElectronicConverter` for the environment + motor(env-arg): Specification of the :py:class:`.ElectricMotor` for the environment + load(env-arg): Specification of the :py:class:`.MechanicalLoad` for the environment + ode_solver(env-arg): Specification of the :py:class:`.OdeSolver` for the environment + reward_function(env-arg): Specification of the :py:class:`.RewardFunction` for the environment + reference_generator(env-arg): Specification of the :py:class:`.ReferenceGenerator` for the environment + visualization(env-arg): Specification of the :py:class:`.ElectricMotorVisualization` for the environment + constraints(iterable(str/Constraint)): All Constraints of the environment. \n + - str: A LimitConstraints for states (episode terminates, if the quantity exceeds the limit) + can be directly specified by passing the state name here (e.g. 'i', 'omega') \n + - instance of Constraint: More complex constraints (e.g. the SquaredConstraint can be initialized and + passed to the environment. + calc_jacobian(bool): Flag, if the jacobian of the environment shall be taken into account during the + simulation. This may lead to speed improvements. Default: True + tau(float): Duration of one control step in seconds. Default: 1e-4. + state_filter(list(str)): List of states that shall be returned to the agent. Default: None (no filter) + callbacks(list(Callback)): Callbacks for user interaction. Default: () + physical_system_wrappers(list(PhysicalSystemWrapper)): List of Physical System Wrappers to modify the + actions to and states from the physical system before they are used in the environment. Default: () + + Note on the env-arg type: + All parameters of type env-arg can be selected as one of the following types: + + **instance:** Pass an already instantiated object derived from the corresponding base class + (e.g. ``reward_function=MyRewardFunction()``). This is directly used in the environment. + + **dict:** Pass a dict to update the default parameters of the default type. + (e.g. ``visualization=dict(state_plots=('omega', 'u'))``) + + **str:** Pass a string out of the registered classes to select a different class for the component. + This class is then initialized with its default parameters. + The available strings can be looked up in the documentation. (e.g. ``converter='Finite-2QC'``) + """ + + default_sub_converters = ( + ps.FiniteB6BridgeConverter(), + ps.FiniteB6BridgeConverter(), + ) + + physical_system = SixPhasePMSM( + supply=initialize(ps.VoltageSupply, supply, ps.IdealVoltageSupply, dict(u_nominal=300.0)), + converter=initialize(ps.PowerElectronicConverter,converter,ps.FiniteMultiConverter,dict(subconverters=default_sub_converters),), + motor=initialize(ps.ElectricMotor, motor, ps.electric_motors.SixPhasePMSM, dict()), + load=initialize(ps.MechanicalLoad, load, ps.ConstantSpeedLoad, dict(omega_fixed=100.0)), + ode_solver=initialize(ps.OdeSolver, ode_solver, ps.ScipyOdeSolver, dict()), + calc_jacobian=calc_jacobian, + tau=tau, + ) + reference_generator = initialize( + ReferenceGenerator, + reference_generator, + WienerProcessReferenceGenerator, + dict(reference_state="torque"), + ) + reward_function = initialize( + RewardFunction, + reward_function, + WeightedSumOfErrors, + dict(reward_weights=dict(torque=1.0)), + ) + visualization = initialize( + ElectricMotorVisualization, + visualization, + MotorDashboard, + dict(state_plots=("torque",), action_plots="all"), + ) + super().__init__( + physical_system=physical_system, + reference_generator=reference_generator, + reward_function=reward_function, + constraints=constraints, + visualization=visualization, + state_filter=state_filter, + callbacks=callbacks, + physical_system_wrappers=physical_system_wrappers, + **kwargs, + ) \ No newline at end of file diff --git a/src/gym_electric_motor/envs/motors.py b/src/gym_electric_motor/envs/motors.py index 01475cc7..1242d2aa 100644 --- a/src/gym_electric_motor/envs/motors.py +++ b/src/gym_electric_motor/envs/motors.py @@ -3,7 +3,7 @@ MotorType = Enum( "MotorType", - ["PermanentlyExcitedDcMotor", "ExternallyExcitedDcMotor", "SeriesDc", "ShuntDc", "ExternallyExcitedSynchronousMotor", "DoublyFedInductionMotor", "SquirrelCageInductionMotor", "PermanentMagnetSynchronousMotor", "SynchronousReluctanceMotor"], + ["PermanentlyExcitedDcMotor", "ExternallyExcitedDcMotor", "SeriesDc", "ShuntDc", "ExternallyExcitedSynchronousMotor", "DoublyFedInductionMotor", "SquirrelCageInductionMotor", "PermanentMagnetSynchronousMotor", "SynchronousReluctanceMotor", "SixPhasePMSM"], ) MotorType.PermanentlyExcitedDcMotor.states = ["omega", "torque", "i", "u"] MotorType.ExternallyExcitedDcMotor.states = [ @@ -37,6 +37,9 @@ MotorType.SynchronousReluctanceMotor.states = [ "omega" , "torque", "i_sd", "i_sq", "i_a", "i_b", "i_c", "u_sd", "u_sq", "u_a", "u_b", "u_c" ] +MotorType.SixPhasePMSM.states = ["omega" , "torque", "i_a1", "i_b1", "i_c1", "i_a2", "i_b2", "i_c2", "i_sd", "i_sq", + "i_sx", "i_sy", "u_a1", "u_b1", "u_c1", "u_a2", "u_b2", "u_c2", "u_sd", "u_sq", "u_sx", "u_sy","epsilon", +] # add env_id_tag if you dont want to use enum name as env_id @@ -47,6 +50,7 @@ MotorType.SquirrelCageInductionMotor.env_id_tag = "SCIM" MotorType.PermanentMagnetSynchronousMotor.env_id_tag = "PMSM" MotorType.SynchronousReluctanceMotor.env_id_tag = "SynRM" +MotorType.SixPhasePMSM.env_id_tag = "SIXPMSM" ControlType = Enum("ControlType", ["SpeedControl", "TorqueControl", "CurrentControl"]) ControlType.SpeedControl.env_id_tag = "SC" diff --git a/src/gym_electric_motor/physical_systems/electric_motors/__init__.py b/src/gym_electric_motor/physical_systems/electric_motors/__init__.py index e6a096dd..e4ede7de 100644 --- a/src/gym_electric_motor/physical_systems/electric_motors/__init__.py +++ b/src/gym_electric_motor/physical_systems/electric_motors/__init__.py @@ -21,3 +21,6 @@ # Three Phase Motors from .three_phase_motor import ThreePhaseMotor + +#six phase motors +from .six_phase_PMSM import SixPhasePMSM \ No newline at end of file diff --git a/src/gym_electric_motor/physical_systems/electric_motors/six_phase_PMSM.py b/src/gym_electric_motor/physical_systems/electric_motors/six_phase_PMSM.py new file mode 100644 index 00000000..b8084406 --- /dev/null +++ b/src/gym_electric_motor/physical_systems/electric_motors/six_phase_PMSM.py @@ -0,0 +1,267 @@ +import math + +import numpy as np + +from .six_phase_motor import SixPhaseMotor + + +class SixPhasePMSM(SixPhaseMotor): + """ + ===================== ========== ============= =========================================== + Motor Parameter Unit Default Value Description + ===================== ========== ============= =========================================== + r_s Ohm 64.3e-3 Stator resistance + l_d H 125e-6 Direct axis inductance + l_q H 126e-6 Quadrature axis inductance + l_x H 39e-6 x-axis inductance + l_y H 35e-6 y-axis inductance + p 1 5 Pole pair number + psi_PM Vs 4.7e-3 flux linkage of the permanent magnets + ===================== ========== ============= =========================================== + + The default parameters originate from: + L. Broghammer et al., "Reinforcement Learning Control of Six-Phase Permanent Magnet Synchronous Machines," + 2023 13th International Electric Drives Production Conference (EDPC), Regensburg, Germany, 2023,pp. 1-8, + doi: 10.1109/EDPC60603.2023.10372153. + + =============== ====== ============================================= + Motor Currents Unit Description + =============== ====== ============================================= + i_sd A Direct axis current + i_sq A Quadrature axis current + i_sx A Stator current in the counter-rotating system + i_sy A Stator current in the counter-rotating system + i_salpha A Stator current in alpha direction + i_sbeta A Stator current in beta direction + i_sX A Stator current in X direction + i_sY A Stator current in Y direction + i_sa1 A Stator current in phase a1 + i_sa2 A Stator current in phase a2 + i_sb1 A Stator current in phase b1 + i_sb2 A Stator current in phase b2 + i_sc1 A Stator current in phase c1 + i_sc2 A Stator current in phase c2 + + =============== ====== ============================================= + =============== ====== ============================================= + Motor Voltages Unit Description + =============== ====== ============================================= + u_sd V Direct axis voltage + u_sq V Quadrature axis voltage + u_sx V voltage in the counter-rotating system + u_sy V voltage in the counter-rotating system + u_a1 V Phase voltage in phase a1 + u_a2 V Phase voltage in phase a2 + u_b1 V Phase voltage in phase b1 + u_b2 V Phase voltage in phase b2 + u_c1 V Phase voltage in phase c1 + u_c2 V Phase voltage in phase c2 + =============== ====== ============================================= + + ======== =========================================================== + Limits / Nominal Value Dictionary Entries: + -------- ----------------------------------------------------------- + Entry Description + ======== =========================================================== + i General current limit / nominal value + ======== =========================================================== + + """ + I_SD_IDX = 0 + I_SQ_IDX = 1 + I_SX_IDX = 2 + I_SY_IDX = 3 + CURRENTS_IDX = [0, 1, 2, 3] + CURRENTS = ["i_sd", "i_sq", "i_sx", "i_sy"] + VOLTAGES = ["u_sd", "u_sq", "u_sx", "u_sy"] + + @property + def motor_parameter(self): + # Docstring of superclass + return self._motor_parameter + + @property + def initializer(self): + # Docstring of superclass + return self._initializer + + #### Parameters taken from https://ieeexplore.ieee.org/document/10372153 + _default_motor_parameter = {"p": 5, "l_d": 125e-6, "l_q": 126e-6, "l_x": 39e-6, "l_y": 35e-6, "r_s": 64.3e-3, "psi_PM": 4.7e-3, "j_rotor": 0.0110,} + HAS_JACOBIAN = True + _default_limits =dict(omega=4e3 * np.pi / 30, torque=10.0, i=400, epsilon=math.pi, u=300) + _default_nominal_values = dict(omega=3e3 * np.pi / 30, torque=0.0, i=240, epsilon=math.pi, u=300) + _default_initializer = { + "states": {"i_sd": 0.0, "i_sq": 0.0, "i_sx": 0.0, "i_sy": 0.0, "epsilon": 0.0}, + "interval": None, + "random_init": None, + "random_params": (None, None), + } + IO_VOLTAGES = ["u_a1", "u_b1", "u_c1", "u_a2", "u_b2", "u_c2", "u_sd", "u_sq", "u_sx", "u_sy"] + IO_CURRENTS = ["i_a1", "i_b1", "i_a2", "i_b2", "i_c2", "i_c1", "i_sd", "i_sq", "i_sx", "i_sy"] + _model_constants = None + + _initializer = None + + def __init__( + self, + motor_parameter=None, + nominal_values=None, + limit_values=None, + motor_initializer=None, + ): + # Docstring of superclass + nominal_values = nominal_values or {} + limit_values = limit_values or {} + super().__init__(motor_parameter, nominal_values, limit_values, motor_initializer) + self._update_model() + self._update_limits() + + def _update_model(self): + """Updates the motor's model parameters with the motor parameters. + + Called internally when the motor parameters are changed or the motor is initialized. + """ + mp = self._motor_parameter + self._model_constants = np.array([ + # omega, i_d, i_q, i_x, i_y, u_d, u_q, u_x, u_y, omega * i_d, omega * i_q, omega * i_x, omega * i_y + [ 0, -mp['r_s'], 0, 0, 0, 1, 0, 0, 0, 0, mp['l_q'], 0, 0], + [-mp['psi_PM'], 0, -mp['r_s'], 0, 0, 0, 1, 0, 0, -mp['l_d'], 0, 0, 0], + [ 0, 0, 0, -mp['r_s'], 0, 0, 0, 1, 0, 0, 0, 0, -mp['l_y']], + [ 0, 0, 0, 0, -mp['r_s'], 0, 0, 0, 1, 0, 0, mp['l_x'], 0], + [ mp['p'], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + + ]) + self._model_constants[self.I_SD_IDX] = self._model_constants[self.I_SD_IDX] / mp["l_d"] + self._model_constants[self.I_SQ_IDX] = self._model_constants[self.I_SQ_IDX] / mp["l_q"] + self._model_constants[self.I_SX_IDX] = self._model_constants[self.I_SX_IDX] / mp["l_x"] + self._model_constants[self.I_SY_IDX] = self._model_constants[self.I_SY_IDX] / mp["l_y"] + + + def electrical_ode(self, state, u_dqxy, omega, *_): + """ + The differential equation of the Six phase PMSM. + + Args: + state: The current state of the motor. [i_sd, i_sq, i_sx, i_sy, epsilon] + omega: electrical rotational speed + u_qdxy: The input voltages [u_sd, u_sq, u_sx, u_sy] + + Returns: + The derivatives of the state vector d/dt([i_sd, i_sq, i_sx, i_sy, epsilon]) + """ + return np.matmul( + self._model_constants, + np.array( + [ + omega, + state[self.I_SD_IDX], + state[self.I_SQ_IDX], + state[self.I_SX_IDX], + state[self.I_SY_IDX], + u_dqxy[0], + u_dqxy[1], + u_dqxy[2], + u_dqxy[3], + omega * state[self.I_SD_IDX], + omega * state[self.I_SQ_IDX], + omega * state[self.I_SX_IDX], + omega * state[self.I_SY_IDX], + ] + ), + ) + def electrical_jacobian(self, state, u_in, omega, *args): + mp = self._motor_parameter + return ( + np.array( + [ # dx'/dx + # i_sd i_sq i_sx i_sy epsilon + [ + -mp["r_s"] / mp["l_d"], + mp["l_q"] / mp["l_d"] * omega, + 0, + 0, + 0 + ], + [ + -mp["l_d"] / mp["l_q"] * omega, + -mp["r_s"] / mp["l_q"], + 0, + 0, + 0 + ], + [ + 0, + 0, + -mp["r_s"] / mp["l_x"], + -mp["l_y"] / mp["l_x"] * omega, + 0 + ], + [ + 0, + 0, + mp["l_x"] / mp["l_y"] * omega, + -mp["r_s"] / mp["l_y"], + 0 + ], + [0, 0, 0, 0, 0], + ] + ), + np.array( + [ # dx'/dw + mp["l_q"] / mp["l_d"], + -mp["l_d"] / mp["l_q"], + -mp["l_y"] / mp["l_x"], + mp["l_x"] / mp["l_y"], + mp["p"], + ] + ), + np.array( + [ # dT/dx + 1.5 * mp["p"] * (mp["l_d"] - mp["l_q"]) * state[self.I_SQ_IDX], + 1.5 * mp["p"] * (mp["psi_PM"] + (mp["l_d"] - mp["l_q"]) * state[self.I_SD_IDX]), + 0, + 0, + 0 + ] + ), + ) + + def i_in(self, state): + # Docstring of superclass + return state[self.CURRENTS_IDX] + + def reset(self, state_space, state_positions, **__): + # Docstring of superclass + if self._initializer and self._initializer["states"]: + self.initialize(state_space, state_positions) + return np.asarray(list(self._initial_states.values())) + else: + return np.zeros(len(self.CURRENTS) + 1) + + + #from pmsm + def torque(self, currents): + # Docstring of superclass + mp = self._motor_parameter + return ( + 1.5 * mp["p"] * (mp["psi_PM"] + (mp["l_d"] - mp["l_q"]) * currents[self.I_SD_IDX]) * currents[self.I_SQ_IDX] + ) + + #torque limit ? + + def _update_limits(self): + # Docstring of superclass + + voltage_limit = 0.5 * self._limits["u"] + voltage_nominal = 0.5 * self._nominal_values["u"] + + limits_agenda = {} + nominal_agenda = {} + for u, i in zip(self.IO_VOLTAGES, self.IO_CURRENTS): + limits_agenda[u] = voltage_limit + nominal_agenda[u] = voltage_nominal + limits_agenda[i] = self._limits.get("i", None) or self._limits[u] / self._motor_parameter["r_s"] + nominal_agenda[i] = ( + self._nominal_values.get("i", None) or self._nominal_values[u] / self._motor_parameter["r_s"] + ) + super()._update_limits(limits_agenda, nominal_agenda) \ No newline at end of file diff --git a/src/gym_electric_motor/physical_systems/electric_motors/six_phase_motor.py b/src/gym_electric_motor/physical_systems/electric_motors/six_phase_motor.py new file mode 100644 index 00000000..320a58eb --- /dev/null +++ b/src/gym_electric_motor/physical_systems/electric_motors/six_phase_motor.py @@ -0,0 +1,125 @@ +import math + +import numpy as np + +from .electric_motor import ElectricMotor +from scipy.linalg import block_diag + +class SixPhaseMotor(ElectricMotor): + """ + The SixPhaseMotor and its subclasses implement the technical system of Six Phase Motors. + + """ + + # transformation matrix from abc to alpha-beta representation + # VSD is the modeling approach used for multi-phase machines, which represents a generalized form of the clarke transformation + # for the six phase machine with Winding configuration in the stator- and rotor-fixed coordinate systems with δ =(2/3)*π and γ =π/6 + _t46= 1/ 3 * np.array([ + [1, -0.5, -0.5, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0], + [0, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0.5, 0.5, -1], + [1, -0.5, -0.5, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0], + [0, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0.5, 0.5, -1] + ]) + + # i_s - stator current + @staticmethod + def t_46(quantities): + """ + Transformation from abc representation to alpha-beta representation + + Args: + quantities: The properties in the abc representation like ''[i_sa1, i_sb1, i_sc1, i_sa2, i_sb2, i_sc2]'' + + Returns: + The converted quantities in the alpha-beta representation like ''[i_salpha, i_sbeta, i_sX, i_sY]'' + """ + return np.matmul(SixPhaseMotor._t46, quantities) + + @staticmethod + def q(quantities, epsilon): + """ + Transformation of the abc representation into dq using the electrical angle + + Args: + quantities: the properties in the abc representation like ''[i_sa1, i_sb1, i_sc1, i_sa2, i_sb2, i_sc2]'' + epsilon: electrical rotor position + + Returns: + The converted quantities in the dq representation like ''[i_sd, i_sq, i_sx, i_sy, i_s0+, i_s0-]''. + since 2N topology is considered (case where the neutral points are not connected) i_s0+, i_s0- will not be taken into account + """ + """ + t_vsd = 1/ 3 * np.array([ + [1, -0.5, -0.5, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0], + [0, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0.5, 0.5, -1], + [1, -0.5, -0.5, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0], + [0, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0.5, 0.5, -1], + [1, 1, 1, 0, 0, 0], + [0, 0, 0, 1, 1, 1] + ]) + tp_alphaBetaXY = np.array([ + [cos, sin, 0, 0, 0, 0], + [-sin, cos, 0, 0, 0, 0], + [0, 0, cos, -sin, 0, 0], + [0, 0, sin, cos, 0, 0], + [0, 0, 0, 0, 1, 0] + [0, 0, 0, 0, 0, 1] + ]) + """ + t_vsd = 1/ 3 * np.array([ + [1, -0.5, -0.5, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0], + [0, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0.5, 0.5, -1], + [1, -0.5, -0.5, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0], + [0, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0.5, 0.5, -1], + ]) + + def rotation_matrix(theta): + return np.array([ + [math.cos(theta), math.sin(theta)], + [-math.sin(theta), math.cos(theta)] + ]) + + t1 = rotation_matrix(epsilon) + t2 = rotation_matrix(-epsilon) + tp_alphaBetaXY = block_diag(t1,t2) + tp_vsd = np.matmul(tp_alphaBetaXY, t_vsd) + return np.matmul(tp_vsd, quantities) + + + @staticmethod + def q_inv(quantities, epsilon): + """ + Transformation of the dq representation into abc + + Args: + quantities: the properties in the dq representation like ''[i_sd, i_sq, i_sx, i_sy, i_s0+, i_s0-]''. + epsilon: electrical rotor position + + Returns: + The converted quantities in the abc representation like ''[i_sa1, i_sb1, i_sc1, i_sa2, i_sb2, i_sc2]''. + + """ + t_vsd = 1/ 3 * np.array([ + [1, -0.5, -0.5, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0], + [0, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0.5, 0.5, -1], + [1, -0.5, -0.5, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0], + [0, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0.5, 0.5, -1], + [1, 1, 1, 0, 0, 0], + [0, 0, 0, 1, 1, 1] + ]) + cos = math.cos(epsilon) + sin = math.sin(epsilon) + tp_alphaBetaXY = np.array([ + [cos, sin, 0, 0, 0, 0], + [-sin, cos, 0, 0, 0, 0], + [0, 0, cos, -sin, 0, 0], + [0, 0, sin, cos, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 0, 1], + ]) + tp_vsd = np.matmul(tp_alphaBetaXY, t_vsd) + inv_tpVsd = np.linalg.inv(tp_vsd) + modified_inv_tpVsd = np.delete(inv_tpVsd, [4, 5], axis=1) + return np.matmul(modified_inv_tpVsd, quantities) + + \ No newline at end of file diff --git a/src/gym_electric_motor/physical_systems/physical_systems.py b/src/gym_electric_motor/physical_systems/physical_systems.py index 5e8640e9..3845127e 100644 --- a/src/gym_electric_motor/physical_systems/physical_systems.py +++ b/src/gym_electric_motor/physical_systems/physical_systems.py @@ -1111,3 +1111,201 @@ def reset(self, *_): ] ) return system_state / self._limits + +class SixPhaseMotorSystem(SCMLSystem): + """ + SCML-System that implements the basic transformations needed for six phase drives. + """ + + def abc_to_alphabeta_space(self, abc_quantities): + """ + Transformation from abc to alphabeta space + + Args: + abc_quantities: The properties in the abc representation like ''[i_sa1, i_sb1, i_sc1, i_sa2, i_sb2, i_sc2]'' + + Returns: + The converted quantities in the alpha-beta representation like ''[i_salpha, i_sbeta, i_sX, i_sY]'' + """ + alphabeta_quantity = self._electrical_motor.t_46(abc_quantities) + return alphabeta_quantity + + def abc_to_dq_space(self, abc_quantities,epsilon): + """ + Transformation of the abc representation into dq using the electrical angle + + Args: + abc_quantities: the properties in the abc representation like ''[i_sa1, i_sb1, i_sc1, i_sa2, i_sb2, i_sc2]'' + epsilon: electrical rotor position + + Returns: + The converted quantities in the dq representation like ''[i_sd, i_sq, i_sx, i_sy,]''. + """ + dqxy_quantity = self._electrical_motor.q(abc_quantities,epsilon) + return dqxy_quantity + + def dq_to_abc_space(self, dq_quantities,epsilon): + """ + Transformation of the dq representation into abc using the electrical angle + + Args: + dq_quantities: the properties in the dq representation like ''[i_sd, i_sq, i_sx, i_sy, i_s0+, i_s0-]''. + epsilon: electrical rotor position + + Returns: + The converted quantities in the abc representation like ''[i_sa1, i_sb1, i_sc1, i_sa2, i_sb2, i_sc2]''. + """ + abc_quantity = self._electrical_motor.q_inv(dq_quantities,epsilon) + return abc_quantity + +class SixPhasePMSM(SixPhaseMotorSystem): + def __init__(self, control_space="abc", **kwargs): + """ + Args: + control_space(str):('abc' or 'dq') Choose, if actions the actions space is in dq or abc space + kwargs: Further arguments to pass tp SCMLSystem + """ + super().__init__(**kwargs) + self.control_space = control_space + if control_space == "dq": + assert ( + type(self._converter.action_space) == Box + ), "" + self._action_space = Box(-1, 1, shape=(4,), dtype=np.float64) + + def _build_state_space(self, state_names): + # Docstring of superclass + low = -1 * np.ones_like(state_names, dtype=float) + low[self.U_SUP_IDX] = 0.0 + high = np.ones_like(state_names, dtype=float) + return Box(low, high, dtype=np.float64) + + def _build_state_names(self): + # Docstring of superclass + return self._mechanical_load.state_names + [ + "torque", + "i_a1", + "i_b1", + "i_c1", + "i_a2", + "i_b2", + "i_c2", + "i_sd", + "i_sq", + "i_sx", + "i_sy", + "u_a1", + "u_b1", + "u_c1", + "u_a2", + "u_b2", + "u_c2", + "u_sd", + "u_sq", + "u_sx", + "u_sy", + "epsilon", + "u_sup", + ] + #how to do - logic/know how ? + def _set_indices(self): + # Docstring of superclass + super()._set_indices() + self._omega_ode_idx = self._mechanical_load.OMEGA_IDX + self._load_ode_idx = list(range(len(self._mechanical_load.state_names))) + self._ode_currents_idx = list( + range( + self._load_ode_idx[-1] + 1, + self._load_ode_idx[-1] + 1 + len(self._electrical_motor.CURRENTS), + ) + ) + self._motor_ode_idx = self._ode_currents_idx + self._motor_ode_idx += [self._motor_ode_idx[-1] + 1] + self._ode_currents_idx = self._motor_ode_idx[:-1] + self.OMEGA_IDX = self.mechanical_load.OMEGA_IDX + self.TORQUE_IDX = len(self.mechanical_load.state_names) + currents_lower = self.TORQUE_IDX + 1 + currents_upper = currents_lower + 10 # considering six stator currents in abc and four currents in dqxy + self.CURRENTS_IDX = list(range(currents_lower, currents_upper)) + voltages_lower = currents_upper + voltages_upper = voltages_lower + 10 + self.VOLTAGES_IDX = list(range(voltages_lower, voltages_upper)) + self.EPSILON_IDX = voltages_upper + self.U_SUP_IDX = self.EPSILON_IDX + 1 + self._ode_epsilon_idx = self._motor_ode_idx[-1] + + def simulate(self, action, *_, **__): + # Docstring of superclass + ode_state = self._ode_solver.y + eps = ode_state[self._ode_epsilon_idx] + if self.control_space == "dq": + action = self.dq_to_abc_space(action, eps) + i_in = self.dq_to_abc_space(self._electrical_motor.i_in(ode_state[self._ode_currents_idx]), eps) + switching_times = self._converter.set_action(action, self._t) + + for t in switching_times[:-1]: + i_sup = self._converter.i_sup(i_in) + u_sup = self._supply.get_voltage(self._t, i_sup) + u_in = self._converter.convert(i_in, self._ode_solver.t) + u_in = [u * u_s for u in u_in for u_s in u_sup] + u_dq = self.abc_to_dq_space(u_in, eps) + self._ode_solver.set_f_params(u_dq) + ode_state = self._ode_solver.integrate(t) + eps = ode_state[self._ode_epsilon_idx] + i_in = self.dq_to_abc_space(self._electrical_motor.i_in(ode_state[self._ode_currents_idx]), eps) + + i_sup = self._converter.i_sup(i_in) + u_sup = self._supply.get_voltage(self._t, i_sup) + u_in = self._converter.convert(i_in, self._ode_solver.t) + u_in = [u * u_s for u in u_in for u_s in u_sup] + u_dq = self.abc_to_dq_space(u_in, eps) + self._ode_solver.set_f_params(u_dq) + ode_state = self._ode_solver.integrate(self._t + self._tau) + self._t = self._ode_solver.t + self._k += 1 + torque = self._electrical_motor.torque(ode_state[self._motor_ode_idx]) + mechanical_state = ode_state[self._load_ode_idx] + i_dq = ode_state[self._ode_currents_idx] + i_abc = list(self.dq_to_abc_space(i_dq, eps)) + eps = ode_state[self._ode_epsilon_idx] % (2 * np.pi) + if eps > np.pi: + eps -= 2 * np.pi + + system_state = np.concatenate((mechanical_state, [torque], i_abc, i_dq, u_in, u_dq, [eps], u_sup)) + return system_state / self._limits + + def reset(self, *_): + # Docstring of superclass + motor_state = self._electrical_motor.reset(state_space=self.state_space, state_positions=self.state_positions) + mechanical_state = self._mechanical_load.reset( + state_positions=self.state_positions, + state_space=self.state_space, + nominal_state=self.nominal_state, + ) + ode_state = np.concatenate((mechanical_state, motor_state)) + u_sup = self.supply.reset() + eps = ode_state[self._ode_epsilon_idx] + if eps > np.pi: + eps -= 2 * np.pi + u_abc = self.converter.reset() + u_abc = [u * u_s for u in u_abc for u_s in u_sup] + u_dq = self.abc_to_dq_space(u_abc, eps) + i_dq = ode_state[self._ode_currents_idx] + i_abc = self.dq_to_abc_space(i_dq, eps) + torque = self.electrical_motor.torque(motor_state) + self._t = 0 + self._k = 0 + self._ode_solver.set_initial_value(ode_state, self._t) + system_state = np.concatenate( + ( + mechanical_state, + [torque], + i_abc, + i_dq, + u_abc, + u_dq, + [eps], + u_sup, + ) + ) + return system_state / self._limits \ No newline at end of file diff --git a/tests/test_physical_systems/test_electric_motors.py b/tests/test_physical_systems/test_electric_motors.py index a37ae80f..9a1ce8bf 100644 --- a/tests/test_physical_systems/test_electric_motors.py +++ b/tests/test_physical_systems/test_electric_motors.py @@ -15,9 +15,12 @@ SynchronousMotor, SynchronousReluctanceMotor, ExternallyExcitedSynchronousMotor, - PermanentMagnetSynchronousMotor + PermanentMagnetSynchronousMotor, + SixPhasePMSM ) +from gym_electric_motor.physical_systems.electric_motors.six_phase_motor import SixPhaseMotor from gymnasium.spaces import Box +from scipy.linalg import block_diag import numpy as np #parameters for dc motor @@ -54,6 +57,13 @@ "random_init": None, "random_params": (None, None), } +test_SixPhasePMSM_parameter = {"p": 6, "l_d": 125e-5, "l_q": 126e-5, "l_x": 39e-3, "l_y": 35e-3, "r_s": 64.3e-6, "psi_PM": 4.7e-6, "j_rotor": 0.0110,} +test_SixPhasePMSM_initializer = { + "states": {"i_sd": 10.0, "i_sq": 20.0, "i_sx": 10.0, "i_sy": 20.0, "epsilon": 0.0}, + "interval": None, + "random_init": None, + "random_params": (None, None), + } @pytest.fixture def concreteDCMotor(): """ @@ -1116,3 +1126,192 @@ def test_PermMagSyncMotor_el_Jacobian(): ) assert np.array_equal(expectedJacobian[0],defaultPermMagSyncMotor.electrical_jacobian(state,u_in,omega)[0]) +def test_SixPhaseMotor_t_46(): + defaultSixPhaseMotor = SixPhaseMotor() + t46 = 1/ 3 * np.array([ + [1, -0.5, -0.5, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0], + [0, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0.5, 0.5, -1], + [1, -0.5, -0.5, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0], + [0, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0.5, 0.5, -1] + ]) + i_abc = [1, 2, 3, 1, 2, 3] #[i_sa1, i_sb1, i_sc1, i_sa2, i_sb2, i_sc2] + expectedI_out = np.matmul(t46, i_abc) # [i_salpha, i_sbeta, i_sX, i_sY] + assert np.array_equal(defaultSixPhaseMotor.t_46(i_abc),expectedI_out) + +def test_SixPhaseMotor_q(): + defaultSixPhaseMotor = SixPhaseMotor() + t_vsd = 1/ 3 * np.array([ + [1, -0.5, -0.5, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0], + [0, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0.5, 0.5, -1], + [1, -0.5, -0.5, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0], + [0, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0.5, 0.5, -1], + ]) + def rotation_matrix(theta): + return np.array([ + [math.cos(theta), math.sin(theta)], + [-math.sin(theta), math.cos(theta)] + ]) + epsilon = 10 + t1 = rotation_matrix(epsilon) + t2 = rotation_matrix(-epsilon) + tp_alphaBetaXY = block_diag(t1,t2) + tp_vsd = np.matmul(tp_alphaBetaXY, t_vsd) + i_abc = [1, 2, 3, 1, 2, 3] #[i_sa1, i_sb1, i_sc1, i_sa2, i_sb2, i_sc2] + expectedI_out = np.matmul(tp_vsd, i_abc) # [i_sd, i_sq, i_sx, i_sy] + assert np.array_equal(defaultSixPhaseMotor.q(i_abc,epsilon),expectedI_out) + +def test_SixPhaseMotor_q_inv(): + defaultSixPhaseMotor = SixPhaseMotor() + t_vsd = 1/ 3 * np.array([ + [1, -0.5, -0.5, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0], + [0, 0.5 * np.sqrt(3), -0.5 * np.sqrt(3), 0.5, 0.5, -1], + [1, -0.5, -0.5, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0], + [0, -0.5 * np.sqrt(3), 0.5 * np.sqrt(3), 0.5, 0.5, -1], + [1, 1, 1, 0, 0, 0], + [0, 0, 0, 1, 1, 1] + ]) + epsilon = 0.5 + cos = math.cos(epsilon) + sin = math.sin(epsilon) + tp_alphaBetaXY = np.array([ + [cos, sin, 0, 0, 0, 0], + [-sin, cos, 0, 0, 0, 0], + [0, 0, cos, -sin, 0, 0], + [0, 0, sin, cos, 0, 0], + [0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 0, 1], + ]) + tp_vsd = np.matmul(tp_alphaBetaXY, t_vsd) + inv_tpVsd = np.linalg.inv(tp_vsd) + modified_inv_tpVsd = np.delete(inv_tpVsd, [4, 5], axis=1) + quantities_dqxy = [1,2,2,1] + expected_abc = np.matmul(modified_inv_tpVsd, quantities_dqxy) + assert np.array_equal(defaultSixPhaseMotor.q_inv(quantities_dqxy,epsilon),expected_abc) + +def test_InitSixPhasePMSM(): + defaultSixPhasePMSM = SixPhasePMSM() + assert defaultSixPhasePMSM._default_motor_parameter == {"p": 5, "l_d": 125e-6, "l_q": 126e-6, "l_x": 39e-6, "l_y": 35e-6, "r_s": 64.3e-3, "psi_PM": 4.7e-3, "j_rotor": 0.0110,} + assert defaultSixPhasePMSM.HAS_JACOBIAN + assert defaultSixPhasePMSM.motor_parameter == defaultSixPhasePMSM._default_motor_parameter + assert defaultSixPhasePMSM._initial_states == defaultSixPhasePMSM._default_initializer["states"] + concreteSixPhasePMSM = SixPhasePMSM(test_SixPhasePMSM_parameter,None,None,test_SixPhasePMSM_initializer) + assert concreteSixPhasePMSM.motor_parameter == test_SixPhasePMSM_parameter + assert concreteSixPhasePMSM.initializer == test_SixPhasePMSM_initializer + assert concreteSixPhasePMSM._initial_states == test_SixPhasePMSM_initializer["states"] + assert defaultSixPhasePMSM.CURRENTS_IDX == SixPhasePMSM.CURRENTS_IDX + assert defaultSixPhasePMSM.CURRENTS == SixPhasePMSM.CURRENTS + assert defaultSixPhasePMSM.IO_VOLTAGES == SixPhasePMSM.IO_VOLTAGES + + +def test_SixPhasePMSM_el_ode(): + defaultSixPhasePMSM = SixPhasePMSM() + state = [6, 5, 10, 2, 1]#[i_sd, i_sq, i_sx, i_sy, epsilon] + omega = 60 + u_dqxy = [50, 60, 50, 50]#[u_sd, u_sq, u_sx, u_sy] + expectedOde = np.matmul( + defaultSixPhasePMSM._model_constants, + np.array( + [ + omega, + state[defaultSixPhasePMSM.I_SD_IDX], + state[defaultSixPhasePMSM.I_SQ_IDX], + state[defaultSixPhasePMSM.I_SX_IDX], + state[defaultSixPhasePMSM.I_SY_IDX], + u_dqxy[0], + u_dqxy[1], + u_dqxy[2], + u_dqxy[3], + omega * state[defaultSixPhasePMSM.I_SD_IDX], + omega * state[defaultSixPhasePMSM.I_SQ_IDX], + omega * state[defaultSixPhasePMSM.I_SX_IDX], + omega * state[defaultSixPhasePMSM.I_SY_IDX], + ] + ) + ) + assert np.array_equal(defaultSixPhasePMSM.electrical_ode(state,u_dqxy,omega),expectedOde) + +def test_SixPhasePMSM_el_jacobian(): + defaultSixPhasePMSM = SixPhasePMSM() + mp = defaultSixPhasePMSM._motor_parameter + state = [6, 5, 10, 2, 1]#[i_sd, i_sq, i_sx, i_sy, epsilon] + omega = 60 + u_dqxy = [50, 60, 50, 50]#[u_sd, u_sq, u_sx, u_sy] + expectedJacobian = ( + np.array( + [ # dx'/dx + # i_sd i_sq i_sx i_sy epsilon + [ + -mp["r_s"] / mp["l_d"], + mp["l_q"] / mp["l_d"] * omega, + 0, + 0, + 0 + ], + [ + -mp["l_d"] / mp["l_q"] * omega, + -mp["r_s"] / mp["l_q"], + 0, + 0, + 0 + ], + [ + 0, + 0, + -mp["r_s"] / mp["l_x"], + -mp["l_y"] / mp["l_x"] * omega, + 0 + ], + [ + 0, + 0, + mp["l_x"] / mp["l_y"] * omega, + -mp["r_s"] / mp["l_y"], + 0 + ], + [0, 0, 0, 0, 0], + ] + ), + np.array( + [ # dx'/dw + mp["l_q"] / mp["l_d"], + -mp["l_d"] / mp["l_q"], + -mp["l_y"] / mp["l_x"], + mp["l_x"] / mp["l_y"], + mp["p"], + ] + ), + np.array( + [ # dT/dx + 1.5 * mp["p"] * (mp["l_d"] - mp["l_q"]) * state[defaultSixPhasePMSM.I_SQ_IDX], + 1.5 * mp["p"] * (mp["psi_PM"] + (mp["l_d"] - mp["l_q"]) * state[defaultSixPhasePMSM.I_SD_IDX]), + 0, + 0, + 0 + ] + ), + ) + assert np.array_equal(expectedJacobian[0],defaultSixPhasePMSM.electrical_jacobian(state,u_dqxy,omega)[0]) + assert np.array_equal(expectedJacobian[1],defaultSixPhasePMSM.electrical_jacobian(state,u_dqxy,omega)[1]) + assert np.array_equal(expectedJacobian[2],defaultSixPhasePMSM.electrical_jacobian(state,u_dqxy,omega)[2]) + +def test_SixPhasePMSM_torque(): + defaultSixPhasePMSM = SixPhasePMSM() + mp = defaultSixPhasePMSM._motor_parameter + currents = [3,1,2,3] + expectedTorque = ( + 1.5 * mp["p"] * (mp["psi_PM"] + (mp["l_d"] - mp["l_q"]) * currents[defaultSixPhasePMSM.I_SD_IDX]) * currents[defaultSixPhasePMSM.I_SQ_IDX] + ) + assert defaultSixPhasePMSM.torque(currents) == expectedTorque + +def test_SixPMSM_reset(): + defaultSixPhasePMSM = SixPhasePMSM() + default_initial_state = {"i_sd": 0.0, "i_sq": 0.0, "i_sx": 0.0, "i_sy": 0.0, "epsilon": 0.0} + default_Initial_state_array = [0,0,0,0,0] + new_initial_state = {"i_sd": 10.0, "i_sq": 20.0, "i_sx": 10.0, "i_sy": 10.0, "epsilon": 10.0} + assert defaultSixPhasePMSM._initial_states == default_initial_state + defaultSixPhasePMSM_state_positions = {"i_sd": 0, "i_sq": 1, "i_sx": 2, "i_sy": 3, "epsilon": 4, "torque": 5,"omega": 6,"u": 7} + defaultSixPhasePMSM_state_space = Box(low=-1, high=1, shape=(8,), dtype=np.float64) + assert (defaultSixPhasePMSM.reset(defaultSixPhasePMSM_state_space,defaultSixPhasePMSM_state_positions),default_Initial_state_array) + defaultSixPhasePMSM._initial_states = new_initial_state + defaultSixPhasePMSM.reset(defaultSixPhasePMSM_state_space,defaultSixPhasePMSM_state_positions) + assert defaultSixPhasePMSM._initial_states == new_initial_state