Commit 6a3ea813 authored by Guillaume François's avatar Guillaume François
Browse files

First commit

parents
# Compiled source #
###################
*.com
*.class
*.dll
*.exe
*.o
__pycache__
# Packages #
############
# it's better to unpack these files and commit the raw source
# git has its own built in compression methods
*.7z
*.dmg
*.gz
*.iso
*.jar
*.rar
*.tar
*.zip
# Logs and databases #
######################
*.log
*.sql
*.sqlite
*.ini
# OS generated files #
######################
.DS_Store
.DS_Store?
._*
.Spotlight-V100
.Trashes
ehthumbs.db
Thumbs.db
.idea/
**/__pycache__/*
__pycache__/
# Latex files
*.toc
*.snm
*.out
*.nav
*.aux
*.bbl
*.blg
results.txt
# LELEC2313_lab_DC_control
LELEC2313 - Dynamic modelling and control of electromechanical converters
# Homework:
The second part of this homework consists in implementing your controller in one of your favorite language: **python** ! The main difference from a simulink implementation is that you are now working with "variables" instead of "transfer functions". Your controller is called periodically and the system evolves meanwhile.
# Python libraries required:
We suggest that you use python >= 3.8 :)
- numpy for matrices operations: `pip install numpy`
- plotly for the visualization of the graphs: `pip install plotly`
- pandas to read the result file: `pip install pandas`
*Note: a small portion of our code is compiled to quicken the simulation. We already provide the OS-depend builds, but you might have to do it yourself if you do not use a conventional Operating System.*
*In this case: be sure cython is installed: `pip install Cython`, and try again. If it does not work directly, contact us ;)*
# Running the project:
Just launch the `main.py` file using python: `python main.py`
It does 3 things:
1) Runs the simulator using your controller and saves the data of in the file `results.txt`
2) Draws the trajectory taken by your robot and opens it in your favorite browser. *You might have to refresh the page manually (Ctrl+R) for it to work, depending on the OS and browser.*
3) Plots the results of the simulation and opens it in your favorite browser. *You might have to refresh the page manually (Ctrl+R) for it to work, depending on the OS and browser.*
You can set different parameters at lines 23-29:
- noise_value: a relative noise (between 0 and 1) to apply to the speed measurement. Start with 0, and increase it to test the robustness of your controller.
- period_save: decrease the value to save more points (careful: large file/long display time).
- font_size = 15 # Font size of the graphs
- line_thickness = 1 # Line width of the graphs
# What you should see at the beginning:
![](doc/graph_1_exemple.png)
![](doc/graph_2_exemple.png)
# Implement your controller:
You need to complete the file `my_controller.py`. The two methods you need to implement are `initialize_controllers` and `control_motors`. Carefully read what they are supposed to do, and you are good to go!
Here are some common advices:
- Avoid any repetition of code and keep it as clear as possible. Additional methods should be used, for instance to saturate a quantity.
- Do NOT compute Kp and Ki inside your code. The simulink simulations should have helped you pick good values, encode them directly in your code. Of course, you are still allowed to fine-tune them.
- In real life applications, the time between two calls of the controller can vary. Make your code robust to that change.
- An obstacle might block the robot (purple region in the graph). In this case, an anti-windup strategy is mandatory to avoid any weird behavior once the robot gets unblocked.
*The "weird behavior" occurs because the integral part of your controller kept accumulating the error while the robot was blocked. You can act on that ;)*
- The rated current of the motor should not be exceeded for a long time, otherwise you will burn it :( The "safe operating conditions" are delimited by the red lines in the torque graph.
# In case you want to save the graphs:
You can set different parameters at lines 32-35:
- save_image = False # Put it at True
- image_type = 'png' # Choice between jpeg, svg or png. SVG is better ;)
- image_height = 1024 # In px
- image_width = 2048 # In px
\ No newline at end of file
"""
Date: 9/11/2020
Simulator for the class of electomechanical converters LELEC2313
Prof. Bruno Dehez
"""
import time
try:
from provided.simulate_physics import *
except ImportError: # Recompile physics
print("Must recompile sources ..")
import pyximport
pyximport.install()
print("done")
from provided.simulate_physics import *
import pandas as pd
from provided.visualize import visualize_trajectory, visualize_controller
# Noise on speed measurement:
noise_value = 0.0 # In relative to the speed.
# Period between two saved states (s):
period_save = 1e-4 # You can decrease the value until 1 us to have a more detailed evolution on the graphs but those will take more time to be displayed !
# Visualisation:
font_size = 15 # You can choose the size of the font for the display
line_thickness = 1 # You can change line width of the graphs
# If you want to save the graphs:
save_image = False
image_type = 'png' # Choice between jpeg, svg or png
image_height = 1024 # In px
image_width = 2048 # In px
"""
Simulation & display
"""
if __name__ == '__main__':
print("Start simulation ...")
t = time.time()
nbr_steps = simulate(period_save, noise_value)
print("Simulated {} steps ! Elapsed time: {} sec".format(nbr_steps, time.time() - t))
print("Displaying results ...")
data = pd.read_csv('results.txt', engine="python", encoding="ISO-8859-1", sep='\t', skiprows=1, names=['t', 'speed_L', 'speed_R', 'torque_L', 'torque_R', 'voltage_L', 'voltage_R', 'ref_L', 'ref_R', 'pos_x', 'pos_y', 'pos_r', 'pos_x_ref', 'pos_y_ref', 'pos_r_ref'])
visualize_trajectory(data['t'], data['pos_x'], data['pos_y'], data['pos_r'], data['pos_x_ref'], data['pos_y_ref'], data['pos_r_ref'], font_size, animation_time=25)
visualize_controller(data, line_thickness, font_size, image_type, image_height, image_width, save_image, time_ratio)
from provided.sensors import Sensors
class MyController:
def __init__(self, *args, **kwargs):
"""This is your own structure, you can do whatever you want with it. Store values, create new methods, etc."""
self.dummy_variable = args[0]
def initialize_controllers(theSensors: Sensors):
"""Initialization of your controller. This is called automatically upon starting the robot. Only performed once.
The output of this is one of the inputs of the method 'controls_motors'.
Args:
theSensors: check class provided/sensors.py
Returns:
Whatever you want, just know that it will directly be one of the inputs of the method 'controls_motors' ;)
"""
myControllers = dict()
myControllers["left"] = MyController(1.0)
myControllers["right"] = MyController(2.0)
return myControllers
def control_motors(reference_speed_left, reference_speed_right, myControllers, theSensors: Sensors):
"""
This method is automatically called at each timestep increment. The goal of this method is to know what voltage
to apply to the motors to reach the desire speeds.
Args:
reference_speed_left: speed reference in [rad/s] at the motor level (left motor)
reference_speed_right: speed reference in [rad/s] at the motor level (right motor)
myControllers: the object that has been initialized by initialize_controllers()
theSensors: check class provided/sensors.py
Returns:
voltage_left, voltage_right: two floats, indicating a voltage [V] to apply to the motors
"""
print(f'Time: {theSensors.t}\t Speed L: {theSensors.speed_l} R:{theSensors.speed_r} (rad/s)') # Example to retrieve sensors information
print(f'Dummy variables to store things: {myControllers["left"].dummy_variable}, {myControllers["right"].dummy_variable}') # Example to retrieve controllers information
V_l = 0 # Do something here to compute the voltage to apply to the left motor
V_r = 0 # Do something here to compute the voltage to apply to the right motor
return V_l, V_r
class Sensors:
def __init__(self):
self.t = None
self.speed_l = None
self.speed_r = None
def sense(self, t, speed_l, speed_r):
self.t = t
self.speed_l = speed_l
self.speed_r = speed_r
from setuptools import setup
from Cython.Build import cythonize
setup(
ext_modules=cythonize("simulate_physics.pyx"),
package_dir={'provided': ''},
name="Encapsulated equations"
)
# To use this: python setup.py build_ext --inplace
from libc.math cimport abs,cos,sin
from libc.stdio cimport printf
import cython
from my_controller import initialize_controllers, control_motors
from sensors import Sensors
from libc.stdlib cimport rand, RAND_MAX
time_ratio = 5.0
cdef (double,double) _RK4_step(double speed, double torque, double dt, double voltage, double R, double kphi, double L, double J_at_motor):
cdef double k1_speed, k1_torque
cdef double k2_speed, k2_torque
cdef double k3_speed, k3_torque
cdef double k4_speed, k4_torque
cdef double result_speed, result_torque
k1_speed = dt * get_speed(speed, torque, J_at_motor)
k1_torque = dt * get_torque(speed, torque, voltage, R, kphi, L)
k2_speed = dt * get_speed(speed + 0.5*k1_speed, torque + 0.5*k1_torque, J_at_motor)
k2_torque = dt * get_torque(speed + 0.5*k1_speed, torque + 0.5*k1_torque, voltage, R, kphi, L)
k3_speed = dt * get_speed(speed + 0.5*k2_speed, torque + 0.5*k2_torque, J_at_motor)
k3_torque = dt * get_torque(speed + 0.5*k2_speed, torque + 0.5*k2_torque, voltage, R, kphi, L)
k4_speed = dt * get_speed(speed + k3_speed, torque + k3_torque, J_at_motor)
k4_torque = dt * get_torque(speed + k3_speed, torque + k3_torque, voltage, R, kphi, L)
result_speed = speed + (k1_speed + 2.0*k2_speed + 2.0*k3_speed + k4_speed)/6.0
result_torque = torque + (k1_torque + 2.0*k2_torque + 2.0*k3_torque + k4_torque)/6.0
return result_speed, result_torque
cdef (double,double,double) _move_step(double speed_L, double speed_R, double dt, double ratio, double radius, double space, double x0,double y0, double t0):
cdef double linear_move, rotation_move
cdef double move_L, move_R
cdef double dir_x, dir_y
move_L = speed_L * radius/ratio
move_R = speed_R * radius/ratio
linear_move = dt * (move_L + move_R)/2.0
rotation_move = -dt * (move_L - move_R)/space
return x0 + linear_move*cos(t0), y0 + linear_move*sin(t0),t0+rotation_move
cdef double get_speed(double speed, double torque, double J_at_motor):
return (torque - get_resistive_torque(speed)) / J_at_motor
cdef double get_torque(double speed, double torque, double voltage, double R, double kphi, double L):
return (voltage - R * torque / kphi - kphi * speed) / L
cdef double get_resistive_torque(double omega):
cdef double eps, sign
eps = 1e-6
if -eps < omega < eps:
return 0
sign = -1.0 if omega < 0.0 else 1.0
return sign * (0.011 + 4.3e-5 * abs(omega))
def mid_lvl_controller(time):
omega = 1000 * 2 * 3.14159265359 / 60 * 2
if time < 1/time_ratio:
omega_l = omega_r = 0.0
elif time < 3/time_ratio:
omega_l = omega_r = omega
elif time < 5/time_ratio:
omega_l = omega
omega_r = omega*1.5
elif time < 7/time_ratio:
omega_l = -omega*1.5
omega_r = -omega*0.8
elif time < 9/time_ratio:
omega_l = omega*1.5
omega_r = omega*0.3
elif time < 13/time_ratio:
omega_l = -omega*0.5
omega_r = omega
elif time < 15/time_ratio:
omega_l = omega_r = omega
elif time < 18/time_ratio:
omega_r = 1.5*omega
omega_l = omega
else:
omega_l = omega_r = 0.0
return omega_l, omega_r
cpdef simulate(double save_period, double noise):
# Physics data
cdef double R,kphi,L,wheel_radius,robot_mass,rotor_inertia,J_at_wheel,J_at_motor,wheels_space
cdef int reduction_ratio
R = 7.1
kphi = 26.1e-3
L = 265e-6 + 6e-3
reduction_ratio = 14
wheel_radius = 6e-2
robot_mass = 2.45
rotor_inertia = 5.8e-7 # *10^-7 because g . cm² -> kg m²
J_at_wheel = 1 / 2 * robot_mass * wheel_radius ** 2
J_at_motor = J_at_wheel / reduction_ratio ** 2 + rotor_inertia
wheels_space = 0.15
cdef double max_time,controller_period,simulator_period,time_step_control,time_step,time_step_save
cdef double speed_L, speed_R, torque_L, torque_R,voltage_L,voltage_R, ref_L, ref_R
cdef double pos_x, pos_y, pos_r, pos_x_ref, pos_y_ref, pos_r_ref
cdef double rand_noise_L,rand_noise_R
cdef int nbr_steps, i
# Simulation-specific data
max_time = 20.0/time_ratio
controller_period = 4e-3
simulator_period = 1e-6
# Initialize the state
speed_L, torque_L = 0.0,0.0
speed_R, torque_R = 0.0,0.0
voltage_L, voltage_R = 0.0,0.0
pos_x, pos_y, pos_r, pos_x_ref, pos_y_ref, pos_r_ref = 0.0,0.0,0.0,0.0,0.0,0.0
ref_L, ref_R = 0.0,0.0
# Initialize the data structure
theSensors = Sensors()
theSensors.sense(0.0, 0.0, 0.0)
# Initialize the controllers
theControllers = initialize_controllers(theSensors)
# Start the simulation
nbr_steps = int(max_time / simulator_period)
time_step_control = 0.0
time_step = 0.0
time_step_save = 0.0
with open('results.txt', 'w') as fout:
fout.write('t\tspeed_L\tspeed_R\ttorque_L\ttorque_R\tvoltage_L\tvoltage_R\treference_L\treference_R\tpos_x\tpos_y\tpos_r\tpos_x_ref\tpos_y_ref\tpos_r_ref\n')
for i in range(nbr_steps):
if time_step - time_step_control > controller_period:
rand_noise_L = speed_L * (noise * 2*(rand() / (RAND_MAX + 1.0) - 0.5))
rand_noise_R = speed_R * (noise * 2*(rand() / (RAND_MAX + 1.0) - 0.5))
theSensors.sense(time_step, speed_L+rand_noise_L, speed_R+rand_noise_R)
ref_L, ref_R = mid_lvl_controller(time_step)
voltage_L, voltage_R = control_motors(ref_L, ref_R, theControllers, theSensors)
if voltage_L > 24.0:
voltage_L = 24.0
if voltage_R > 24.0:
voltage_R = 24.0
if voltage_L < -24.0:
voltage_L = -24.0
if voltage_R < -24.0:
voltage_R = -24.0
time_step_control = time_step
if time_step - time_step_save > save_period:
fout.write('%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n'%(time_step, speed_L,speed_R,torque_L,torque_R, voltage_L, voltage_R,ref_L, ref_R,pos_x,pos_y,pos_r, pos_x_ref, pos_y_ref,pos_r_ref))
time_step_save = time_step
speed_L, torque_L = _RK4_step(speed_L, torque_L, simulator_period, voltage_L, R, kphi, L, J_at_motor)
speed_R, torque_R = _RK4_step(speed_R, torque_R, simulator_period, voltage_R, R, kphi, L, J_at_motor)
if 14.5/time_ratio < time_step < 17.0/time_ratio:
speed_L = 0.0
speed_R = 0.0
else:
pos_x, pos_y,pos_r = _move_step(speed_L, speed_R, simulator_period, reduction_ratio, wheel_radius, wheels_space, pos_x,pos_y, pos_r)
pos_x_ref, pos_y_ref,pos_r_ref = _move_step(ref_L, ref_R, simulator_period, reduction_ratio, wheel_radius, wheels_space, pos_x_ref, pos_y_ref,pos_r_ref)
time_step += simulator_period
return nbr_steps
import numpy as np
import plotly.graph_objects as go
import plotly.offline as pyoff
from plotly.subplots import make_subplots
def visualize_trajectory(t_init, x, y, r, x_ref, y_ref, r_ref,font_size, animation_time=20):
xrobot = np.array([-2.0, -3.0, -3.0, -3.5, -3.5, -3.0] + list(-3.0 * np.cos(np.linspace(0, np.pi, 10))) + [3.0, 3.5, 3.5, 3.0, 3.0, 2.0, -2.0])
yrobot = np.array([-2.0, 0.0, -1.5, -1.5, 1.5, 1.5] + list(3.0 * np.sin(np.linspace(0, np.pi, 10))) + [1.5, 1.5, -1.5, -1.5, 0.0, -2.0, -2.0])
def __robot(x, y, t, s=0.15 / 6.0):
x = s * xrobot * np.cos(t - np.pi / 2) - s * yrobot * np.sin(t - np.pi / 2) + x
y = s * yrobot * np.cos(t - np.pi / 2) + s * xrobot * np.sin(t - np.pi / 2) + y
return x, y
N = len(t_init)
steps = int(N / 250)
# Generate curve data
t = [0]
xx, yy, tt = [[0.0]], [[0.0]], [[0.0]]
xx_ref, yy_ref, tt_ref = [[0.0]], [[0.0]], [[0.0]]
for i in range(0, N, steps):
xx.append(xx[-1] + [x[i]])
yy.append(yy[-1] + [y[i]])
tt.append(tt[-1] + [r[i]])
xx_ref.append(xx_ref[-1] + [x_ref[i]])
yy_ref.append(yy_ref[-1] + [y_ref[i]])
tt_ref.append(tt_ref[-1] + [r_ref[i]])
t.append(t_init[i])
# make figure
fig_dict = {
"data": [],
"layout": {},
"frames": []
}
# fill in most of layout
# fig_dict["layout"]["xaxis"] = {"range": [min(xm, ym), max(yM, xM)], "title": "X position"}
# fig_dict["layout"]["yaxis"] = {"range": [min(xm, ym), max(yM, xM)], "title": "X position"}
fig_dict["layout"]["hovermode"] = "closest"
fig_dict["layout"]["updatemenus"] = [
{
"buttons": [
{
"args": [None, {"frame": {"duration": animation_time, "redraw": False},
"fromcurrent": True, "transition": {"easing": "quadratic-in-out"}}],
"label": "Play",
"method": "animate"
},
{
"args": [[None], {"frame": {"duration": 0, "redraw": False},
"mode": "immediate",
"transition": {"duration": 0}}],
"label": "Pause",
"method": "animate"
}
],
"direction": "left",
"pad": {"r": 10, "t": 87},
"showactive": False,
"type": "buttons",
"x": 0.1,
"xanchor": "right",
"y": 0,
"yanchor": "top"
}
]
sliders_dict = {
"active": 0,
"yanchor": "top",
"xanchor": "left",
"currentvalue": {
"font": {"size": 20},
"prefix": "Time:",
"visible": True,
"xanchor": "right"
},
"pad": {"b": 10, "t": 50},
"len": 0.9,
"x": 0.1,
"y": 0,
"steps": []
}
# make data
data_dict = {
"x": xx[-1],
"y": yy[-1],
"mode": "lines",
"name": "My controller path",
"line": dict(width=2, color="blue")
}
fig_dict["data"].append(data_dict)
data_dict = {
"x": xx_ref[-1],
"y": yy_ref[-1],
"mode": "lines",
"name": "Reference path",
"line": dict(width=2, color="red", dash='dash')
}
fig_dict["data"].append(data_dict)
xr, yr = __robot(0.0, 0.0, 0.0)
data_dict = {
"x": xr,
"y": yr,
"mode": "lines",
"fill": "toself",
"showlegend": False,
"line": dict(width=2, color="red")
}
fig_dict["data"].append(data_dict)
xr, yr = __robot(0.0, 0.0, 0.0)
data_dict = {
"x": xr,
"y": yr,
"mode": "lines",
"fill": "toself",
"showlegend": False,
"line": dict(width=2, color="blue")
}
fig_dict["data"].append(data_dict)
# make frames
for i in range(len(xx)):
frame = {"data": [], "name": str(round(t[i], 2))}
data_dict = {
"x": xx[i],
"y": yy[i],
"mode": "lines",
"line": dict(width=2, color="blue")
}
frame["data"].append(data_dict)
data_dict = {
"x": xx_ref[i],
"y": yy_ref[i],
"mode": "lines",
"line": dict(width=2, color="red", dash='dash')
}
frame["data"].append(data_dict)
xr, yr = __robot(xx_ref[i][-1], yy_ref[i][-1], tt_ref[i][-1])
data_dict = {
"x": xr,
"y": yr,
"mode": "lines",
"line": dict(width=2, color="red")
}
frame["data"].append(data_dict)
xr, yr = __robot(xx[i][-1], yy[i][-1], tt[i][-1])
data_dict = {
"x": xr,
"y": yr,
"mode": "lines",
"line": dict(width=2, color="blue")
}
frame["data"].append(data_dict)
fig_dict["frames"].append(frame)
slider_step = {"args": [
[round(t[i], 2)],
{"frame": {"redraw": False},
"mode": "immediate"}
],
"label": round(t[i], 2),
"method": "animate"}
sliders_dict["steps"].append(slider_step)
fig_dict["layout"]["sliders"] = [sliders_dict]
fig = go.Figure(fig_dict)
fig.update_yaxes(
scaleanchor="x",
scaleratio=1,
)
fig.update_layout(
title="LELEC2313 Lab Trajectories",
xaxis=dict(title="X position", showgrid=True, zeroline=True, title_font=dict(color="Orange")),