Skip to content

Commit

Permalink
Synthesize compatible CLF/CBF for 2D quadrotor with input limits. (#74)
Browse files Browse the repository at this point in the history 
Using V-rep is faster than using H-rep.
  • Loading branch information
@hongkai-dai
hongkai-dai authored Sep 21, 2024
1 parent 0a6b4c5 commit d00b8a8
Show file tree
Hide file tree
Showing 3 changed files with 147 additions and 65 deletions.
115 changes: 71 additions & 44 deletions examples/quadrotor2d/demo.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,32 +5,40 @@
cos/sin.
"""

import itertools
import os

import numpy as np
import pydrake.solvers as solvers
import pydrake.symbolic as sym

from compatible_clf_cbf import clf_cbf
import compatible_clf_cbf.clf as clf
from examples.quadrotor2d.plant import Quadrotor2dTrigPlant
import examples.quadrotor2d.demo_clf as demo_clf
import compatible_clf_cbf.utils


def main(use_y_squared: bool, with_u_bound: bool):
def main(use_y_squared: bool, with_u_bound: bool, use_v_rep: bool):
x = sym.MakeVectorContinuousVariable(7, "x")
quadrotor = Quadrotor2dTrigPlant()
f, g = quadrotor.affine_dynamics(x)

if with_u_bound:
Au = np.concatenate((np.eye(2), -np.eye(2)), axis=0)
u_bound = quadrotor.m * quadrotor.g * 1.5
bu = np.concatenate((np.full((2,), u_bound), np.zeros((2,))))
if use_v_rep:
u_vertices = np.array(list(itertools.product([0, u_bound], repeat=2)))
u_extreme_rays = None
Au, bu = None, None
else:
Au = np.concatenate((np.eye(2), -np.eye(2)), axis=0)
bu = np.concatenate((np.full((2,), u_bound), np.zeros((2,))))
u_vertices, u_extreme_rays = None, None
else:
Au, bu = None, None
u_vertices, u_extreme_rays = None, None

# Ground as the unsafe region.
exclude_sets = [clf_cbf.ExcludeSet(np.array([sym.Polynomial(x[1] + 0.5)]))]
exclude_sets = [clf_cbf.ExcludeSet(np.array([sym.Polynomial(x[1] + 2)]))]
state_eq_constraints = quadrotor.equality_constraint(x)
compatible = clf_cbf.CompatibleClfCbf(
f=f,
Expand All @@ -40,33 +48,59 @@ def main(use_y_squared: bool, with_u_bound: bool):
within_set=None,
Au=Au,
bu=bu,
u_vertices=u_vertices,
u_extreme_rays=u_extreme_rays,
num_cbf=1,
with_clf=True,
use_y_squared=use_y_squared,
state_eq_constraints=state_eq_constraints,
)

V_degree = 2
V_init = 10 * demo_clf.find_trig_regional_clf(V_degree, x)
# V_init = 10 * demo_clf.find_trig_regional_clf(V_degree, x)
clf_data = clf.load_clf(
os.path.join(
os.path.dirname(os.path.abspath(__file__)), "../../data/quadrotor2d_clf.pkl"
),
compatible.x_set,
)
V_init = clf_data["V"]
h_degrees = [2]
h_init = np.array([1 - V_init])
kappa_V = 0
kappa_V = 0.1
kappa_h = np.array([kappa_V])
solver_options = solvers.SolverOptions()
solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, True)

compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees(
lambda_y=[clf_cbf.XYDegree(x=1, y=0 if use_y_squared else 1) for _ in range(2)],
xi_y=clf_cbf.XYDegree(x=2, y=0 if use_y_squared else 1),
y=(
None
if use_y_squared
else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)]
),
rho_minus_V=clf_cbf.XYDegree(x=2, y=2),
h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)],
state_eq_constraints=[clf_cbf.XYDegree(x=2, y=2)],
)
if use_v_rep and with_u_bound:
compatible_lagrangian_degrees = clf_cbf.CompatibleWVrepLagrangianDegrees(
u_vertices=[clf_cbf.XYDegree(x=2, y=0) for _ in range(u_vertices.shape[0])],
u_extreme_rays=None,
xi_y=None,
y=(
None
if use_y_squared
else [clf_cbf.XYDegree(x=4, y=2) for _ in range(compatible.y.size)]
),
rho_minus_V=clf_cbf.XYDegree(x=2, y=2),
h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)],
state_eq_constraints=[clf_cbf.XYDegree(x=4, y=2)],
)
else:
compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees(
lambda_y=[
clf_cbf.XYDegree(x=1, y=0 if use_y_squared else 1) for _ in range(2)
],
xi_y=clf_cbf.XYDegree(x=2, y=0 if use_y_squared else 1),
y=(
None
if use_y_squared
else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)]
),
rho_minus_V=clf_cbf.XYDegree(x=2, y=2),
h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)],
state_eq_constraints=[clf_cbf.XYDegree(x=2, y=2)],
)
safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees(
exclude=[
clf_cbf.ExcludeRegionLagrangianDegrees(
Expand All @@ -81,10 +115,11 @@ def main(use_y_squared: bool, with_u_bound: bool):
compatible_states_options = clf_cbf.CompatibleStatesOptions(
candidate_compatible_states=np.array(
[
[0, 0, 0, 0, 0, 0, 0],
[0, -0.3, 0, 0, 0, 0, 0],
[0.5, -0.3, 0, 0, 0, 0, 0],
[-0.5, -0.3, 0, 0, 0, 0, 0],
[-3, 0, 0, 0, 0, 0, 0],
[3, 0, 0, 0, 0, 0, 0],
[-1.5, -0.5, 0, 0, 0, 0, 0],
[1.5, 0.5, 0, 0, 0, 0, 0],
[0, -1.5, 0, 0, 0, 0, 0],
]
),
anchor_states=np.zeros((1, 7)),
Expand All @@ -111,12 +146,19 @@ def main(use_y_squared: bool, with_u_bound: bool):
# solver_id = solvers.ClarabelSolver().id(),
lagrangian_coefficient_tol=None,
compatible_states_options=compatible_states_options,
backoff_scale=compatible_clf_cbf.utils.BackoffScale(rel=0.02, abs=None),
backoff_scale=compatible_clf_cbf.utils.BackoffScale(rel=None, abs=0.01),
)
x_set = sym.Variables(x)

filename = (
"quadrotor2d_clf_cbf"
+ ("_w_y_square" if use_y_squared else "_wo_y_squared")
+ ("_vrep" if use_v_rep else "_hrep")
+ ("_w_u_bound" if with_u_bound else "_wo_u_bound")
+ ".pkl"
)
pickle_path = os.path.join(
os.path.dirname(os.path.abspath(__file__)),
"../../data/quadrotor2d_clf_cbf.pkl",
os.path.dirname(os.path.abspath(__file__)), "../../data/", filename
)
clf_cbf.save_clf_cbf(
V,
Expand All @@ -127,23 +169,8 @@ def main(use_y_squared: bool, with_u_bound: bool):
pickle_path,
)

# Check the CLF/CBF with a positive kappa.
(
compatible_lagrangians,
safety_sets_lagrangians,
) = compatible.search_lagrangians_given_clf_cbf(
V,
h,
kappa_V=1e-4,
kappa_h=np.array([1e-4]),
barrier_eps=barrier_eps,
compatible_lagrangian_degrees=compatible_lagrangian_degrees,
safety_set_lagrangian_degrees=safety_sets_lagrangian_degrees,
solver_options=solver_options,
)
assert compatible_lagrangians is not None
assert safety_sets_lagrangians is not None


if __name__ == "__main__":
main(use_y_squared=False, with_u_bound=False)
# main(use_y_squared=False, with_u_bound=False, use_v_rep=False)
main(use_y_squared=True, with_u_bound=True, use_v_rep=False)
# main(use_y_squared=True, with_u_bound=True, use_v_rep=True)
27 changes: 23 additions & 4 deletions examples/quadrotor2d/demo_clf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
cos/sin.
"""

import os
from typing import Tuple

import numpy as np
Expand Down Expand Up @@ -86,7 +87,7 @@ def main(with_u_bound: bool):

V_degree = 2
V_init = find_trig_regional_clf(V_degree, x)
kappa_V = 1e-4
kappa_V = 0.1
solver_options = solvers.SolverOptions()
solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, True)

Expand All @@ -106,13 +107,31 @@ def main(with_u_bound: bool):
clf_lagrangian_degrees = clf.ClfWoInputLimitLagrangianDegrees(
dVdx_times_f=4, dVdx_times_g=[3, 3], rho_minus_V=4, state_eq_constraints=[4]
)
clf_search.search_lagrangian_given_clf(

candidate_stable_states = np.zeros((2, 7))
candidate_stable_states[0, :4] = np.array([2, 0, 0, 0])
candidate_stable_states[1, :4] = np.array([-2, 0, 0, 0])
stable_states_options = clf.StableStatesOptions(
candidate_stable_states=candidate_stable_states, V_margin=0.01
)
V = clf_search.bilinear_alternation(
V_init,
rho=1,
clf_lagrangian_degrees,
kappa=kappa_V,
lagrangian_degrees=clf_lagrangian_degrees,
clf_degree=2,
x_equilibrium=np.zeros((7,)),
max_iter=10,
stable_states_options=stable_states_options,
solver_options=solver_options,
)
clf.save_clf(
V,
clf_search.x_set,
kappa_V,
pickle_path=os.path.join(
os.path.dirname(os.path.abspath(__file__)), "../../data/quadrotor2d_clf.pkl"
),
)
return


Expand Down
70 changes: 53 additions & 17 deletions examples/quadrotor2d/demo_taylor.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
"""

from enum import Enum
import itertools
from typing import Tuple

import numpy as np
Expand Down Expand Up @@ -35,18 +36,28 @@ def lqr(quadrotor: Quadrotor2dPlant) -> Tuple[np.ndarray, np.ndarray]:


def search_clf_cbf(
use_y_squared: bool, with_u_bound: bool, grow_heuristics: GrowHeuristics
use_y_squared: bool,
with_u_bound: bool,
grow_heuristics: GrowHeuristics,
use_v_rep: bool,
):
x = sym.MakeVectorContinuousVariable(6, "x")
quadrotor = Quadrotor2dPlant()
f, g = quadrotor.taylor_affine_dynamics(x)

if with_u_bound:
Au = np.concatenate((np.eye(2), -np.eye(2)), axis=0)
u_bound = quadrotor.m * quadrotor.g * 1.5
bu = np.concatenate((np.full((2,), u_bound), np.zeros((2,))))
if use_v_rep:
u_vertices = np.array(list(itertools.product([0, u_bound], repeat=2)))
u_extreme_rays = None
Au = bu = None
else:
Au = np.concatenate((np.eye(2), -np.eye(2)), axis=0)
bu = np.concatenate((np.full((2,), u_bound), np.zeros((2,))))
u_vertices = u_extreme_rays = None
else:
Au, bu = None, None
u_vertices = u_extreme_rays = None

K_lqr, S_lqr = lqr(quadrotor)
V_init = sym.Polynomial(x.dot(S_lqr @ x) / 0.01)
Expand All @@ -65,23 +76,40 @@ def search_clf_cbf(
within_set=None,
Au=Au,
bu=bu,
u_vertices=u_vertices,
u_extreme_rays=u_extreme_rays,
num_cbf=1,
with_clf=True,
use_y_squared=use_y_squared,
state_eq_constraints=None,
)
lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees(
lambda_y=[clf_cbf.XYDegree(x=2, y=0) for _ in range(2)],
xi_y=clf_cbf.XYDegree(x=4, y=0),
y=(
None
if use_y_squared
else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)]
),
rho_minus_V=clf_cbf.XYDegree(x=4, y=2),
h_plus_eps=[clf_cbf.XYDegree(x=4, y=2)],
state_eq_constraints=None,
)
if use_v_rep and with_u_bound:
lagrangian_degrees = clf_cbf.CompatibleWVrepLagrangianDegrees(
u_vertices=[clf_cbf.XYDegree(x=2, y=0) for _ in range(u_vertices.shape[0])],
u_extreme_rays=None,
xi_y=None,
y=(
None
if use_y_squared
else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)]
),
rho_minus_V=clf_cbf.XYDegree(x=4, y=2),
h_plus_eps=[clf_cbf.XYDegree(x=4, y=2)],
state_eq_constraints=None,
)
else:
lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees(
lambda_y=[clf_cbf.XYDegree(x=2, y=0) for _ in range(2)],
xi_y=clf_cbf.XYDegree(x=4, y=0),
y=(
None
if use_y_squared
else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)]
),
rho_minus_V=clf_cbf.XYDegree(x=4, y=2),
h_plus_eps=[clf_cbf.XYDegree(x=4, y=2)],
state_eq_constraints=None,
)
barrier_eps = np.array([0.0])

safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees(
Expand Down Expand Up @@ -153,12 +181,20 @@ def main():
use_y_squared=True,
with_u_bound=False,
grow_heuristics=GrowHeuristics.kCompatibleStates,
use_v_rep=False,
)
# SDP with u_bound is a lot slower than without u_bound.
# search_clf_cbf(
# use_y_squared=True,
# with_u_bound=True,
# grow_heuristics=GrowHeuristics.kInnerEllipsoid,
# grow_heuristics=GrowHeuristics.kCompatibleStates,
# use_v_rep=True,
# )
# SDP with u_bound is a lot slower with H-rep than with V-rep.
# search_clf_cbf(
# use_y_squared=True,
# with_u_bound=True,
# grow_heuristics=GrowHeuristics.kCompatibleStates,
# use_v_rep=False,
# )


Expand Down

0 comments on commit d00b8a8

Please sign in to comment.