Skip to content

Commit

Permalink
repaired roa example scripts
Browse files Browse the repository at this point in the history
  • Loading branch information
fwiebe committed Jul 2, 2024
1 parent 8a4b7b6 commit 7b256d5
Show file tree
Hide file tree
Showing 6 changed files with 388 additions and 248 deletions.
97 changes: 63 additions & 34 deletions software/python/examples/plot_roa_estimations.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,65 +4,94 @@
from simple_pendulum.model.pendulum_plant import PendulumPlant
from simple_pendulum.controllers.lqr.lqr_controller import LQRController

from simple_pendulum.controllers.lqr.roa.plot import get_ellipse_patch
from simple_pendulum.controllers.lqr.roa.plot import get_ellipse_patch

from simple_pendulum.controllers.lqr.roa.sampling import najafi_based_sampling
from simple_pendulum.controllers.lqr.roa.sos import SOSequalityConstrained, SOSlineSearch
from simple_pendulum.controllers.lqr.roa.sos import (
SOSequalityConstrained,
SOSlineSearch,
)

roa_estimation_methods = [najafi_based_sampling, SOSequalityConstrained, SOSlineSearch]
linestyles = ["-",":","--"]
roa_estimation_methods = [najafi_based_sampling, SOSequalityConstrained, SOSlineSearch]
linestyles = ["-", ":", "--"]

torque_limits = [0.1,0.5,1,2,3]
colors = ["red","darkorange","gold","yellow","yellowgreen"]
torque_limits = [0.1, 0.5, 1, 2, 3]
colors = ["red", "darkorange", "gold", "yellow", "yellowgreen"]

mass = 0.57288
length = 0.5
damping = 0.15
gravity = 9.81
coulomb_fric = 0.0
inertia = mass*length*length
inertia = mass * length * length


fig, ax = plt.subplots(figsize=(20, 8))
x_g = plt.scatter([np.pi],[0],color="black",marker="x")
x_g = plt.scatter([np.pi], [0], color="black", marker="x")

handles = [x_g]
labels = ["Goal state"]
for i,torque_limit in enumerate(torque_limits):
for j,mthd in enumerate(roa_estimation_methods):
for i, torque_limit in enumerate(torque_limits):
for j, mthd in enumerate(roa_estimation_methods):
print(i, j)

pendulum = PendulumPlant(mass=mass,
length=length,
damping=damping,
gravity=gravity,
coulomb_fric=coulomb_fric,
inertia=inertia,
torque_limit=torque_limit)
pendulum = PendulumPlant(
mass=mass,
length=length,
damping=damping,
gravity=gravity,
coulomb_fric=coulomb_fric,
inertia=inertia,
torque_limit=torque_limit,
)

controller = LQRController(mass=mass,
length=length,
damping=damping,
gravity=gravity,
torque_limit=torque_limit)
controller = LQRController(
mass=mass,
length=length,
damping=damping,
gravity=gravity,
torque_limit=torque_limit,
)
controller.set_goal([np.pi, 0])

rho,S = mthd(pendulum,controller)
rho, S = mthd(pendulum, controller)

if (rho != 0):
p = get_ellipse_patch(np.pi,0,rho,S,linec=colors[i],linest=linestyles[j])
if rho != 0:
p = get_ellipse_patch(
np.pi, 0, rho, S, linec=colors[i], linest=linestyles[j]
)
ax.add_patch(p)
handles = np.append(handles,p)
if (j == 0):
labels = np.append(labels, f"torque limit = {torque_limits[i]}, method = najafi-based sampling method")
elif (j == 1):
labels = np.append(labels, f"torque limit = {torque_limits[i]}, method = SOS method wirh equality-constrained formulation")
else:
labels = np.append(labels, f"torque limit = {torque_limits[i]}, method = SOS method with line search")
handles = np.append(handles, p)
if j == 0:
labels = np.append(
labels,
f"torque limit = {torque_limits[i]}, method = najafi-based sampling method",
)
elif j == 1:
labels = np.append(
labels,
f"torque limit = {torque_limits[i]}, method = SOS method wirh equality-constrained formulation",
)
else:
labels = np.append(
labels,
f"torque limit = {torque_limits[i]}, method = SOS method with line search",
)

plt.title("Comparison between different RoA estimation methods with different torque limits")
plt.title(
"Comparison between different RoA estimation methods with different torque limits"
)
ax.set_xlabel("x")
ax.set_ylabel(r"$\dot{x}$")
box = ax.get_position()
ax.set_position([box.x0, box.y0, box.width * 0.7, box.height])
ax.legend(handles = handles.tolist(), labels = labels.tolist(), bbox_to_anchor =(1, 0.5), loc = 'center left', fancybox=True, shadow=True)
ax.legend(
handles=handles.tolist(),
labels=labels.tolist(),
bbox_to_anchor=(1, 0.5),
loc="center left",
fancybox=True,
shadow=True,
)
plt.grid(True)
plt.show()
125 changes: 76 additions & 49 deletions software/python/examples/verify_roa_estimation.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,91 +8,118 @@
from simple_pendulum.controllers.lqr.roa.plot import get_ellipse_patch

from simple_pendulum.controllers.lqr.roa.sampling import najafi_based_sampling
from simple_pendulum.controllers.lqr.roa.sos import SOSequalityConstrained, SOSlineSearch
from simple_pendulum.controllers.lqr.roa.sos import (
SOSequalityConstrained,
SOSlineSearch,
)
from simple_pendulum.controllers.lqr.roa.utils import sample_from_ellipsoid

roa_estimation_methods = [najafi_based_sampling, SOSequalityConstrained, SOSlineSearch]
linestyles = ["-",":","--"]
roa_estimation_methods = [najafi_based_sampling, SOSequalityConstrained, SOSlineSearch]
linestyles = ["-", ":", "--"]

torque_limit = 2 # actuator torque limit
init_num = 500 # number of random initial conditions to check
torque_limit = 2 # actuator torque limit
init_num = 500 # number of random initial conditions to check

mass = 0.57288
length = 0.5
damping = 0.15
gravity = 9.81
coulomb_fric = 0.0
inertia = mass*length*length
inertia = mass * length * length


fig, ax = plt.subplots(figsize=(18, 8))
x_g = plt.scatter([np.pi],[0],color="black",marker="x", linewidths=3)
x_g = plt.scatter([np.pi], [0], color="black", marker="x", linewidths=3)

rho_max = 0
p_buffer = []
# RoA estimation with the three different methods
for j,mthd in enumerate(roa_estimation_methods):

pendulum = PendulumPlant(mass=mass,
length=length,
damping=damping,
gravity=gravity,
coulomb_fric=coulomb_fric,
inertia=inertia,
torque_limit=torque_limit)
for j, mthd in enumerate(roa_estimation_methods):
pendulum = PendulumPlant(
mass=mass,
length=length,
damping=damping,
gravity=gravity,
coulomb_fric=coulomb_fric,
inertia=inertia,
torque_limit=torque_limit,
)

sim = Simulator(plant=pendulum)

controller = LQRController(mass=mass,
length=length,
damping=damping,
gravity=gravity,
torque_limit=torque_limit)
controller = LQRController(
mass=mass,
length=length,
damping=damping,
gravity=gravity,
torque_limit=torque_limit,
)
controller.set_goal([np.pi, 0.0])

rho,S = mthd(pendulum,controller)
rho, S = mthd(pendulum, controller)

if (rho > rho_max):
if rho > rho_max:
rho_max = rho

if (rho != 0):
p = get_ellipse_patch(np.pi,0,rho,S,linec= "black",linest=linestyles[j])
if rho != 0:
p = get_ellipse_patch(np.pi, 0, rho, S, linec="black", linest=linestyles[j])
ax.add_patch(p)
p_buffer = np.append(p_buffer,p)
p_buffer = np.append(p_buffer, p)

# Sampling and check of initial conditions inside the biggest founded ellipse
for i in range(0,init_num):
x_i = sample_from_ellipsoid(S,rho_max)
for i in range(0, init_num):
x_i = sample_from_ellipsoid(S, rho_max)

controller.set_goal([np.pi, 0])
controller.set_clip()
dt = 0.01
t_final = 2.0

T, X, U = sim.simulate(t0=0.0,
x0=[x_i[0]+ np.pi,x_i[1]],
tf=t_final,
dt=dt,
controller=controller,
integrator="runge_kutta"
)

# coloring the checked initial states depending on the result
if (round(np.asarray(X).T[0][-1]) == round(np.pi) and round(np.asarray(X).T[1][-1]) == 0):
greenDot = plt.scatter([x_i[0]+ np.pi],[x_i[1]],color="green",marker="o")
T, X, U = sim.simulate(
t0=0.0,
x0=[x_i[0] + np.pi, x_i[1]],
tf=t_final,
dt=dt,
controller=controller,
integrator="runge_kutta",
)

# coloring the checked initial states depending on the result
if (
round(np.asarray(X).T[0][-1]) == round(np.pi)
and round(np.asarray(X).T[1][-1]) == 0
):
greenDot = plt.scatter([x_i[0] + np.pi], [x_i[1]], color="green", marker="o")
redDot = None
else:
redDot = plt.scatter([x_i[0]+ np.pi],[x_i[1]],color="red",marker="o")
redDot = plt.scatter([x_i[0] + np.pi], [x_i[1]], color="red", marker="o")

ax.set_xlabel("x")
ax.set_ylabel(r"$\dot{x}$")
if (not redDot == None):
ax.legend(handles = [greenDot,redDot,x_g,p_buffer[0],p_buffer[1],p_buffer[2]],
labels = ["successfull initial state","failing initial state", "Goal state",
"najafi-based sampling method", "SOS method wirh equality-constrained formulation", "SOS method with line search"])
else:
ax.legend(handles = [greenDot,x_g,p_buffer[0],p_buffer[1],p_buffer[2]],
labels = ["successfull initial state","Goal state",
"najafi-based sampling method", "SOS method wirh equality-constrained formulation", "SOS method with simple line search"])
if not redDot == None:
ax.legend(
handles=[greenDot, redDot, x_g, p_buffer[0], p_buffer[1], p_buffer[2]],
labels=[
"successfull initial state",
"failing initial state",
"Goal state",
"najafi-based sampling method",
"SOS method wirh equality-constrained formulation",
"SOS method with line search",
],
)
else:
ax.legend(
handles=[greenDot, x_g, p_buffer[0], p_buffer[1], p_buffer[2]],
labels=[
"successfull initial state",
"Goal state",
"najafi-based sampling method",
"SOS method wirh equality-constrained formulation",
"SOS method with simple line search",
],
)
plt.title("Verification of RoA guarantee certificate")
plt.grid(True)
plt.show()
plt.show()

Loading

0 comments on commit 7b256d5

Please sign in to comment.