From f2818662d75d69ed8b52df951925f39752db28bb Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Sun, 25 Aug 2024 22:58:37 -0700 Subject: [PATCH] Simulate quadrotor with CLF/CBF controller. (#64) Also draw the incompatible region for the nonlinear toy example. --- compatible_clf_cbf/cbf.py | 5 +- compatible_clf_cbf/clf.py | 5 +- compatible_clf_cbf/controller.py | 96 ++++++++++ examples/nonlinear_toy/demo_trigpoly.py | 115 ++++++++---- examples/nonlinear_toy/incompatibility.py | 93 ++++++++++ examples/nonlinear_toy/toy_system.py | 7 +- examples/quadrotor/demo.py | 209 ++++++++++++++++++++-- examples/quadrotor2d/demo.py | 5 +- 8 files changed, 472 insertions(+), 63 deletions(-) create mode 100644 compatible_clf_cbf/controller.py create mode 100644 examples/nonlinear_toy/incompatibility.py diff --git a/compatible_clf_cbf/cbf.py b/compatible_clf_cbf/cbf.py index fc6d68d..a18ba01 100644 --- a/compatible_clf_cbf/cbf.py +++ b/compatible_clf_cbf/cbf.py @@ -467,8 +467,9 @@ def __init__( def add_to_prog( self, prog: solvers.MathematicalProgram, x_val: np.ndarray, u: np.ndarray - ): + ) -> solvers.Binding[solvers.LinearConstraint]: env = {self.x[i]: x_val[i] for i in range(x_val.size)} lhs_coeff = np.array([p.Evaluate(env) for p in self.lhs_coeff]) rhs = self.rhs.Evaluate(env) - prog.AddLinearConstraint(lhs_coeff, rhs, np.inf, u) + constraint = prog.AddLinearConstraint(lhs_coeff, rhs, np.inf, u) + return constraint diff --git a/compatible_clf_cbf/clf.py b/compatible_clf_cbf/clf.py index 2aea1e2..56bf913 100644 --- a/compatible_clf_cbf/clf.py +++ b/compatible_clf_cbf/clf.py @@ -634,8 +634,9 @@ def __init__( def add_to_prog( self, prog: solvers.MathematicalProgram, x_val: np.ndarray, u: np.ndarray - ): + ) -> solvers.Binding[solvers.LinearConstraint]: env = {self.x[i]: x_val[i] for i in range(x_val.size)} lhs_coeff = np.array([p.Evaluate(env) for p in self.lhs_coeff]) rhs = self.rhs.Evaluate(env) - prog.AddLinearConstraint(lhs_coeff, -np.inf, rhs, u) + constraint = prog.AddLinearConstraint(lhs_coeff, -np.inf, rhs, u) + return constraint diff --git a/compatible_clf_cbf/controller.py b/compatible_clf_cbf/controller.py new file mode 100644 index 0000000..dad48d7 --- /dev/null +++ b/compatible_clf_cbf/controller.py @@ -0,0 +1,96 @@ +from typing import Optional + +import numpy as np + +import pydrake.solvers as solvers +import pydrake.symbolic as sym +import pydrake.systems.framework + +import compatible_clf_cbf.clf +import compatible_clf_cbf.cbf + +import compatible_clf_cbf.utils + + +class ClfCbfController(pydrake.systems.framework.LeafSystem): + def __init__( + self, + f: np.ndarray, + g: np.ndarray, + V: sym.Polynomial, + b: np.ndarray, + x: np.ndarray, + kappa_V: float, + kappa_b: np.ndarray, + Qu: np.ndarray, + Au: Optional[np.ndarray], + bu: Optional[np.ndarray], + solver_id: Optional[solvers.SolverId], + solver_options: Optional[solvers.SolverOptions], + ): + super().__init__() + self.nu = g.shape[1] + self.DeclareVectorInputPort("state", x.size) + self.action_output_index = self.DeclareVectorOutputPort( + "action", self.nu, self.calc_action + ).get_index() + self.V = V + self.b = b + self.x = x + self.V_output_index = self.DeclareVectorOutputPort( + "V", 1, self.calc_V + ).get_index() + self.b_output_index = self.DeclareVectorOutputPort( + "b", b.size, self.calc_b + ).get_index() + self.clf_constraint = compatible_clf_cbf.clf.ClfConstraint(V, f, g, x, kappa_V) + self.cbf_constraint = [ + compatible_clf_cbf.cbf.CbfConstraint(b[i], f, g, x, kappa_b[i]) + for i in range(b.size) + ] + self.Qu = Qu + self.Au = Au + self.bu = bu + self.solver_id = solver_id + self.solver_options = solver_options + + def action_output_port(self): + return self.get_output_port(self.action_output_index) + + def clf_output_port(self): + return self.get_output_port(self.V_output_index) + + def cbf_output_port(self): + return self.get_output_port(self.b_output_index) + + def calc_action(self, context: pydrake.systems.framework.Context, output): + x_val: np.ndarray = self.get_input_port(0).Eval(context) + prog = solvers.MathematicalProgram() + u = prog.NewContinuousVariables(self.nu, "u") + prog.AddQuadraticCost(self.Qu, np.zeros((self.nu,)), u, is_convex=True) + self.clf_constraint.add_to_prog(prog, x_val, u) + for cbf_cnstr in self.cbf_constraint: + cbf_cnstr.add_to_prog(prog, x_val, u) + if self.Au is not None: + assert self.bu is not None + prog.AddLinearConstraint( + self.Au, np.full_like(self.bu, -np.inf), self.bu, u + ) + result = compatible_clf_cbf.utils.solve_with_id( + prog, self.solver_id, self.solver_options + ) + assert result.is_success() + u_val = result.GetSolution(u) + output.set_value(u_val) + + def calc_V(self, context: pydrake.systems.framework.Context, output): + x_val: np.ndarray = self.get_input_port(0).Eval(context) + env = {self.x[i]: x_val[i] for i in range(self.x.size)} + V_val = self.V.Evaluate(env) + output.set_value(np.array([V_val])) + + def calc_b(self, context: pydrake.systems.framework.Context, output): + x_val: np.ndarray = self.get_input_port(0).Eval(context) + env = {self.x[i]: x_val[i] for i in range(self.x.size)} + b_val = np.array([b_i.Evaluate(env) for b_i in self.b]) + output.set_value(b_val) diff --git a/examples/nonlinear_toy/demo_trigpoly.py b/examples/nonlinear_toy/demo_trigpoly.py index 622368d..fb92f8f 100644 --- a/examples/nonlinear_toy/demo_trigpoly.py +++ b/examples/nonlinear_toy/demo_trigpoly.py @@ -16,8 +16,24 @@ import pydrake.solvers as solvers import compatible_clf_cbf.clf_cbf as clf_cbf -import compatible_clf_cbf.utils as utils +import compatible_clf_cbf.utils as utils # noqa import examples.nonlinear_toy.toy_system as toy_system +import examples.nonlinear_toy.incompatibility + + +def get_grid_pts(): + grid_theta, grid_x2 = np.meshgrid( + np.linspace(-np.pi, np.pi, 100), np.linspace(-3, 4, 100) + ) + grid_x_vals = np.concatenate( + ( + np.sin(grid_theta.reshape((1, -1))), + np.cos(grid_theta.reshape((1, -1))) - 1, + grid_x2.reshape((1, -1)), + ), + axis=0, + ) + return grid_theta, grid_x2, grid_x_vals def plot_clf_cbf( @@ -37,25 +53,11 @@ def plot_clf_cbf( Args: fill_compatible: Fill in the compatible region. """ - grid_theta, grid_theta_dot = np.meshgrid( - np.linspace(-np.pi, np.pi, 100), np.linspace(-3, 3, 100) - ) - grid_x_vals = np.concatenate( - ( - np.sin(grid_theta.reshape((1, -1))), - np.cos(grid_theta.reshape((1, -1))) - 1, - grid_theta_dot.reshape((1, -1)), - ), - axis=0, - ) + grid_theta, grid_x2, grid_x_vals = get_grid_pts() grid_V = V.EvaluateIndeterminates(x, grid_x_vals).reshape(grid_theta.shape) grid_b = b[0].EvaluateIndeterminates(x, grid_x_vals).reshape(grid_theta.shape) - h_V = ax.contour( - grid_theta, grid_theta_dot, grid_V, levels=np.array([1]), colors="red" - ) - h_b = ax.contour( - grid_theta, grid_theta_dot, grid_b, levels=np.array([0]), colors="blue" - ) + h_V = ax.contour(grid_theta, grid_x2, grid_V, levels=np.array([1]), colors="red") + h_b = ax.contour(grid_theta, grid_x2, grid_b, levels=np.array([0]), colors="blue") if fill_compatible: # Fill in the region {x|V(x)<=1, b(x) >= 0}, namely @@ -63,11 +65,11 @@ def plot_clf_cbf( grid_fill_vals = np.maximum(grid_V - 1, -grid_b) h_compatible = ax.contourf( grid_theta, - grid_theta_dot, + grid_x2, grid_fill_vals, levels=[-np.inf, 0], colors="green", - alpha=0.2, + alpha=0.4, ) else: h_compatible = None @@ -82,17 +84,7 @@ def get_unsafe_regions(x: np.ndarray) -> np.ndarray: def plot_unsafe_regions(ax: matplotlib.axes.Axes): x = sym.MakeVectorContinuousVariable(3, "x") unsafe_regions = get_unsafe_regions(x) - grid_theta, grid_theta_dot = np.meshgrid( - np.linspace(-np.pi, np.pi, 100), np.linspace(-3, 3, 100) - ) - grid_x_vals = np.concatenate( - ( - np.sin(grid_theta.reshape((1, -1))), - np.cos(grid_theta.reshape((1, -1))) - 1, - grid_theta_dot.reshape((1, -1)), - ), - axis=0, - ) + grid_theta, grid_x2, grid_x_vals = get_grid_pts() unsafe_values = ( np.concatenate( [ @@ -106,7 +98,7 @@ def plot_unsafe_regions(ax: matplotlib.axes.Axes): ) h_unsafe = ax.contourf( grid_theta, - grid_theta_dot, + grid_x2, unsafe_values, levels=np.array([-np.inf, 0.0]), alpha=0.5, @@ -167,15 +159,17 @@ def search(unit_test_flag: bool = False): within=None, ) ] - kappa_V = 0.01 - kappa_b = np.array([0.01]) + kappa_V = 0.1 + kappa_b = np.array([0.1]) barrier_eps = np.array([0.001]) x_equilibrium = np.array([0, 0.0, 0.0]) clf_degree = 2 cbf_degrees = [2] - max_iter = 20 + # Somehow the code runs into error after 13 iterations on the CI, but is + # fine on my laptop for 20 iterations. + max_iter = 10 if unit_test_flag else 20 binary_search_scale_options = None inner_ellipsoid_options = None @@ -185,6 +179,9 @@ def search(unit_test_flag: bool = False): [np.sin(-np.pi / 3), np.cos(-np.pi / 3) - 1, -0.5], [np.sin(0), np.cos(0) - 1, -1.5], [np.sin(np.pi / 2), np.cos(np.pi / 2) - 1, -1.9], + [np.sin(0), np.cos(0) - 1, 2], + [np.sin(0.75 * np.pi), np.cos(0.75 * np.pi) - 1, 1], + [np.sin(-0.8 * np.pi), np.cos(-0.8 * np.pi) - 1, 1], ] ), anchor_states=np.array([[0.0, 0, 0]]), @@ -212,7 +209,7 @@ def search(unit_test_flag: bool = False): binary_search_scale_options=binary_search_scale_options, compatible_states_options=compatible_states_options, solver_options=solver_options, - backoff_scale=utils.BackoffScale(rel=None, abs=0.01), + # backoff_scale=utils.BackoffScale(rel=None, abs=0.005), ) print(f"V={V}") print(f"b={b}") @@ -233,6 +230,40 @@ def search(unit_test_flag: bool = False): return V, b +def plot_incompatible( + ax: matplotlib.axes.Axes, + V: sym.Polynomial, + b: sym.Polynomial, + x: np.ndarray, + kappa_V: float, + kappa_b: float, +): + grid_theta, grid_x2, grid_x_vals = get_grid_pts() + + compute_incompatible = ( + examples.nonlinear_toy.incompatibility.ComputeIncompatibility( + V, b, x, kappa_V, kappa_b, u_min=-1, u_max=1 + ) + ) + + grid_incompatible_vals = np.array( + [ + compute_incompatible.eval(grid_x_vals[:, i]) + for i in range(grid_x_vals.shape[1]) + ] + ).reshape(grid_theta.shape) + + h_incompatible = ax.contourf( + grid_theta, + grid_x2, + grid_incompatible_vals, + levels=[0, np.inf], + colors="cyan", + alpha=0.4, + ) + return h_incompatible + + def visualize(): x = sym.MakeVectorContinuousVariable(3, "x") f, g = toy_system.affine_trig_poly_dynamics(x) @@ -245,7 +276,7 @@ def visualize(): fig = plt.figure() ax = fig.add_subplot() ax.set_xlabel(r"$\theta$ (rad)", fontsize=16) - ax.set_ylabel(r"$\dot{\theta}$ (rad/s)", fontsize=16) + ax.set_ylabel(r"$\gamma$", fontsize=16) ax.set_xticks([-np.pi, -np.pi / 2, 0, np.pi / 2, np.pi]) ax.set_xticklabels( [r"$-\pi$", r"$-\frac{\pi}{2}$", r"0", r"$\frac{\pi}{2}$", r"$\pi$"] @@ -256,6 +287,14 @@ def visualize(): ) h_V_init, h_b_init = plot_clf_cbf_init(ax) plot_unsafe_regions(ax) + h_incompatible = plot_incompatible( # noqa + ax, + saved_data["V"], + saved_data["b"][0], + x, + saved_data["kappa_V"], + saved_data["kappa_b"][0], + ) ax.legend( [ h_V.legend_elements()[0][0], @@ -267,7 +306,7 @@ def visualize(): prop={"size": 12}, ) fig.show() - for fig_extension in (".png", "pdf"): + for fig_extension in (".png", ".pdf"): fig.savefig( os.path.join( os.path.dirname(os.path.abspath(__file__)), diff --git a/examples/nonlinear_toy/incompatibility.py b/examples/nonlinear_toy/incompatibility.py new file mode 100644 index 0000000..decf8e4 --- /dev/null +++ b/examples/nonlinear_toy/incompatibility.py @@ -0,0 +1,93 @@ +import numpy as np +import pydrake.symbolic as sym +import pydrake.solvers as solvers + +import compatible_clf_cbf.clf as clf +import compatible_clf_cbf.cbf as cbf +import examples.nonlinear_toy.toy_system as toy_system + + +class ComputeIncompatibility: + """ + The CLF constraint is a_V * u <= b_V + The CBF constraint is a_b * u <= b_b + We define the incompatibility as the "signed distance" between the the set + {u | a_V * u <= b_V} and the set {u | a_b * u <= b_b}. + """ + + def __init__( + self, + V: sym.Polynomial, + b: sym.Polynomial, + x: np.ndarray, + kappa_V: float, + kappa_b: float, + u_min: float, + u_max: float, + ): + f, g = toy_system.affine_trig_poly_dynamics(x) + self.clf_constraint = clf.ClfConstraint(V, f, g, x, kappa_V) + self.cbf_constraint = cbf.CbfConstraint(b, f, g, x, kappa_b) + self.u_min = u_min + self.u_max = u_max + + def eval(self, x_val: np.ndarray) -> float: + prog = solvers.MathematicalProgram() + u = prog.NewContinuousVariables(1, "u") + clf_cnstr = self.clf_constraint.add_to_prog(prog, x_val, u) + cbf_cnstr = self.cbf_constraint.add_to_prog(prog, x_val, u) + # Write clf_cnstr in the a_V * u <= b_V form + if np.isinf(clf_cnstr.evaluator().upper_bound()): + a_V = -clf_cnstr.evaluator().GetDenseA()[0, 0] + b_V = -clf_cnstr.evaluator().lower_bound()[0] + else: + a_V = clf_cnstr.evaluator().GetDenseA()[0, 0] + b_V = clf_cnstr.evaluator().upper_bound()[0] + # Write cbf_cnstr in the a_b * u <= b_b form + if np.isinf(cbf_cnstr.evaluator().upper_bound()): + a_b = -cbf_cnstr.evaluator().GetDenseA()[0, 0] + b_b = -cbf_cnstr.evaluator().lower_bound()[0] + else: + a_b = cbf_cnstr.evaluator().GetDenseA()[0, 0] + b_b = cbf_cnstr.evaluator().upper_bound()[0] + if a_V > 0: + # By CLF constraint, we have u <= b_V / a_V + u_clf_upper = b_V / a_V + u_clf_lower = -np.inf + elif a_V == 0: + if b_V >= 0: + u_clf_upper = np.inf + u_clf_lower = -np.inf + else: + u_clf_upper = -np.inf + u_clf_lower = -np.inf + else: + u_clf_upper = np.inf + u_clf_lower = b_V / a_V + + if a_b > 0: + u_cbf_upper = b_b / a_b + u_cbf_lower = -np.inf + elif a_b == 0: + if b_b >= 0: + u_cbf_upper = np.inf + u_cbf_lower = -np.inf + else: + u_cbf_upper = -np.inf + u_cbf_lower = -np.inf + else: + u_cbf_upper = np.inf + u_cbf_lower = b_b / a_b + + u_upper = np.min([u_cbf_upper, u_clf_upper, self.u_max]) + u_lower = np.max([u_clf_lower, u_cbf_lower, self.u_min]) + + if not np.isinf(u_upper) and not np.isinf(u_lower): + incompatible = u_lower - u_upper + elif np.isinf(u_upper) and not np.isinf(u_lower): + incompatible = -np.abs(u_clf_lower - u_cbf_lower) + elif np.isinf(u_lower) and not np.isinf(u_upper): + incompatible = -np.abs(u_clf_upper - u_clf_upper) + else: + incompatible = -np.inf + return incompatible diff --git a/examples/nonlinear_toy/toy_system.py b/examples/nonlinear_toy/toy_system.py index e766ab4..6a4f59a 100644 --- a/examples/nonlinear_toy/toy_system.py +++ b/examples/nonlinear_toy/toy_system.py @@ -43,8 +43,11 @@ def affine_trig_poly_state_constraints(x: np.ndarray) -> sym.Polynomial: def affine_trig_poly_dynamics(x: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: """ - With the trigonometric state x̅ = [sinx₀, cosx₀−1, x₁], we can write the - dynamics as affine form f(x̅) + g(x̅)u + With the trigonometric state x̅ = [sinθ, cosθ−1, x₁], we can write the + dynamics + θdot = u + x₁_dot = -sinθ - u + as affine form f(x̅) + g(x̅)u """ assert x.shape == (3,) if x.dtype == object: diff --git a/examples/quadrotor/demo.py b/examples/quadrotor/demo.py index f713d28..b260122 100644 --- a/examples/quadrotor/demo.py +++ b/examples/quadrotor/demo.py @@ -6,19 +6,36 @@ """ import os +from typing import Tuple import numpy as np + +from pydrake.geometry import ( + Box, + MeshcatVisualizer, + MeshcatVisualizerParams, + Role, + Rgba, + StartMeshcat, + SceneGraph, +) +import pydrake.math import pydrake.solvers as solvers import pydrake.symbolic as sym +import pydrake.systems.analysis +from pydrake.systems.framework import Diagram, DiagramBuilder +from pydrake.systems.primitives import LogVectorOutput, VectorLogSink + import compatible_clf_cbf.clf as clf from compatible_clf_cbf import clf_cbf -from examples.quadrotor.plant import QuadrotorPolyPlant +from examples.quadrotor.plant import QuadrotorPolyPlant, QuadrotorPolyGeometry import examples.quadrotor.demo_clf from compatible_clf_cbf.utils import BackoffScale +import compatible_clf_cbf.controller -def main(use_y_squared: bool, with_u_bound: bool): +def search(use_y_squared: bool, with_u_bound: bool): x = sym.MakeVectorContinuousVariable(13, "x") quadrotor = QuadrotorPolyPlant() f, g = quadrotor.affine_dynamics(x) @@ -67,19 +84,19 @@ def main(use_y_squared: bool, with_u_bound: bool): b_init = np.array([1 - V_init]) - load_clf_cbf = True + load_clf_cbf = False if load_clf_cbf: data = clf_cbf.load_clf_cbf( os.path.join( os.path.dirname(os.path.abspath(__file__)), - "../../data/quadrotor_clf_cbf3.pkl", + "../../data/quadrotor_clf_cbf7.pkl", ), x_set, ) V_init = data["V"] b_init = data["b"] - kappa_V_sequences = [0, 1e-3, 1e-3, 1e-3, 1e-3] - kappa_b_sequences = [np.array([kappa_V]) for kappa_V in kappa_V_sequences] + kappa_V_sequences = [0.05, 0.075, 0.1, 0.125, 0.15, 0.175, 0.2, 0.2] + kappa_b_sequences = [np.array([0.2]) for _ in range(len(kappa_V_sequences))] compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( lambda_y=[ @@ -145,16 +162,15 @@ def main(use_y_squared: bool, with_u_bound: bool): candidate_compatible_states_sequences[-1][5, 4] = -0.5 candidate_compatible_states_sequences[-1][5, 6] = -0.35 - candidate_compatible_states_sequences.append(np.zeros((6, 13))) + candidate_compatible_states_sequences.append(np.zeros((5, 13))) + candidate_compatible_states_sequences[-1][1, 5] = -0.6 candidate_compatible_states_sequences[-1][1, 6] = -0.35 - candidate_compatible_states_sequences[-1][2, 5] = -0.6 + candidate_compatible_states_sequences[-1][2, 5] = 0.6 candidate_compatible_states_sequences[-1][2, 6] = -0.35 - candidate_compatible_states_sequences[-1][3, 5] = 0.6 + candidate_compatible_states_sequences[-1][3, 4] = 0.6 candidate_compatible_states_sequences[-1][3, 6] = -0.35 - candidate_compatible_states_sequences[-1][4, 4] = 0.6 + candidate_compatible_states_sequences[-1][4, 4] = -0.6 candidate_compatible_states_sequences[-1][4, 6] = -0.35 - candidate_compatible_states_sequences[-1][5, 4] = -0.6 - candidate_compatible_states_sequences[-1][5, 6] = -0.35 candidate_compatible_states_sequences.append(np.zeros((5, 13))) candidate_compatible_states_sequences[-1][1, 5] = -0.7 @@ -166,31 +182,79 @@ def main(use_y_squared: bool, with_u_bound: bool): candidate_compatible_states_sequences[-1][4, 4] = -0.7 candidate_compatible_states_sequences[-1][4, 6] = -0.35 + candidate_compatible_states_sequences.append(np.zeros((5, 13))) + candidate_compatible_states_sequences[-1][1, 5] = -0.8 + candidate_compatible_states_sequences[-1][1, 6] = -0.35 + candidate_compatible_states_sequences[-1][2, 5] = 0.8 + candidate_compatible_states_sequences[-1][2, 6] = -0.35 + candidate_compatible_states_sequences[-1][3, 4] = 0.8 + candidate_compatible_states_sequences[-1][3, 6] = -0.35 + candidate_compatible_states_sequences[-1][4, 4] = -0.8 + candidate_compatible_states_sequences[-1][4, 6] = -0.35 + + candidate_compatible_states_sequences.append(np.zeros((5, 13))) + candidate_compatible_states_sequences[-1][1, 5] = -0.9 + candidate_compatible_states_sequences[-1][1, 6] = -0.35 + candidate_compatible_states_sequences[-1][2, 5] = 0.9 + candidate_compatible_states_sequences[-1][2, 6] = -0.35 + candidate_compatible_states_sequences[-1][3, 4] = 0.9 + candidate_compatible_states_sequences[-1][3, 6] = -0.35 + candidate_compatible_states_sequences[-1][4, 4] = -0.9 + candidate_compatible_states_sequences[-1][4, 6] = -0.35 + + candidate_compatible_states_sequences.append(np.zeros((5, 13))) + candidate_compatible_states_sequences[-1][1, 5] = -1 + candidate_compatible_states_sequences[-1][1, 6] = -0.35 + candidate_compatible_states_sequences[-1][2, 5] = 1 + candidate_compatible_states_sequences[-1][2, 6] = -0.35 + candidate_compatible_states_sequences[-1][3, 4] = 1 + candidate_compatible_states_sequences[-1][3, 6] = -0.35 + candidate_compatible_states_sequences[-1][4, 4] = -1 + candidate_compatible_states_sequences[-1][4, 6] = -0.35 lagrangian_sos_types = [ solvers.MathematicalProgram.NonnegativePolynomial.kSdsos, - solvers.MathematicalProgram.NonnegativePolynomial.kSos, solvers.MathematicalProgram.NonnegativePolynomial.kSdsos, solvers.MathematicalProgram.NonnegativePolynomial.kSdsos, - solvers.MathematicalProgram.NonnegativePolynomial.kSos, + solvers.MathematicalProgram.NonnegativePolynomial.kSdsos, + solvers.MathematicalProgram.NonnegativePolynomial.kSdsos, + solvers.MathematicalProgram.NonnegativePolynomial.kSdsos, + solvers.MathematicalProgram.NonnegativePolynomial.kSdsos, + solvers.MathematicalProgram.NonnegativePolynomial.kSdsos, + ] + V_margin_sequence = [ + None, + None, + None, + None, + None, + 0.01, + 0.01, + 0.01, ] b_margins_sequence = [ None, None, np.array([0.05]), - np.array([0.05]), + np.array([0.04]), np.array([0.02]), + np.array([0.02]), + np.array([0.03]), + np.array([0.03]), ] solver_options = solvers.SolverOptions() solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, True) V = V_init b = b_init - max_iter_sequence = [1, 2, 1, 2, 1] + max_iter_sequence = [1, 1, 1, 2, 1, 1, 2, 2] backoff_scale_sequence = [ BackoffScale(rel=None, abs=0.2), BackoffScale(rel=None, abs=0.2), BackoffScale(rel=None, abs=0.2), BackoffScale(rel=None, abs=0.1), BackoffScale(rel=None, abs=0.2), + BackoffScale(rel=None, abs=0.1), + BackoffScale(rel=None, abs=0.1), + BackoffScale(rel=None, abs=0.15), ] for i in range(len(candidate_compatible_states_sequences)): compatible_states_options = clf_cbf.CompatibleStatesOptions( @@ -199,6 +263,7 @@ def main(use_y_squared: bool, with_u_bound: bool): b_anchor_bounds=[(np.array([0.6]), np.array([1.0]))], weight_V=1, weight_b=np.array([1]), + V_margin=V_margin_sequence[i], b_margins=b_margins_sequence[i], ) @@ -229,5 +294,115 @@ def main(use_y_squared: bool, with_u_bound: bool): clf_cbf.save_clf_cbf(V, b, x_set, kappa_V, kappa_b, pickle_path) +def build_diagram() -> Tuple[ + Diagram, + QuadrotorPolyPlant, + compatible_clf_cbf.controller.ClfCbfController, + VectorLogSink, + VectorLogSink, + VectorLogSink, + VectorLogSink, +]: + builder = DiagramBuilder() + quadrotor = builder.AddSystem(QuadrotorPolyPlant()) + scene_graph = builder.AddSystem(SceneGraph()) + QuadrotorPolyGeometry.AddToBuilder( + builder, quadrotor.get_output_port(0), scene_graph + ) + + meshcat = StartMeshcat() + + MeshcatVisualizer.AddToBuilder( + builder, scene_graph, meshcat, MeshcatVisualizerParams(role=Role.kPerception) + ) + meshcat.SetObject("ground", Box(10, 10, 0.1), rgba=Rgba(0.5, 0.5, 0.5)) + meshcat.SetTransform("ground", pydrake.math.RigidTransform(np.array([0, 0, -0.55]))) + + state_logger = LogVectorOutput(quadrotor.get_output_port(), builder) + + poly_plant = QuadrotorPolyPlant() + x = sym.MakeVectorContinuousVariable(13, "x") + x_set = sym.Variables(x) + + f, g = poly_plant.affine_dynamics(x) + + pickle_path = os.path.join( + os.path.dirname(os.path.abspath(__file__)), + "../../data/quadrotor_clf_cbf7.pkl", + ) + clf_cbf_data = clf_cbf.load_clf_cbf(pickle_path, x_set) + V = clf_cbf_data["V"] + b = clf_cbf_data["b"] + kappa_V = clf_cbf_data["kappa_V"] + kappa_b = clf_cbf_data["kappa_b"] + + Qu = np.eye(4) + clf_cbf_controller = builder.AddSystem( + compatible_clf_cbf.controller.ClfCbfController( + f, + g, + V, + b, + x, + kappa_V, + kappa_b, + Qu, + Au=None, + bu=None, + solver_id=None, + solver_options=None, + ) + ) + builder.Connect( + clf_cbf_controller.action_output_port(), quadrotor.get_input_port(0) + ) + builder.Connect(quadrotor.get_output_port(0), clf_cbf_controller.get_input_port(0)) + + action_logger = LogVectorOutput(clf_cbf_controller.action_output_port(), builder) + + clf_logger = LogVectorOutput(clf_cbf_controller.clf_output_port(), builder) + + cbf_logger = LogVectorOutput(clf_cbf_controller.cbf_output_port(), builder) + diagram = builder.Build() + return ( + diagram, + quadrotor, + clf_cbf_controller, + state_logger, + action_logger, + clf_logger, + cbf_logger, + ) + + +def simulate(x0: np.ndarray, duration: float): + ( + diagram, + quadrotor, + clf_cbf_controller, + state_logger, + action_logger, + clf_logger, + cbf_logger, + ) = build_diagram() + simulator = pydrake.systems.analysis.Simulator(diagram) + simulator.get_mutable_context().SetContinuousState(x0) + simulator.AdvanceTo(duration) + + state_data = state_logger.FindLog(simulator.get_context()).data() + action_data = action_logger.FindLog(simulator.get_context()).data() + clf_data = clf_logger.FindLog(simulator.get_context()).data() + cbf_data = cbf_logger.FindLog(simulator.get_context()).data() + return state_data, action_data, clf_data, cbf_data + + +def main(): + search(use_y_squared=True, with_u_bound=False) + x0 = np.zeros((13,)) + x0[4:7] = np.array([0.7, 0, -0.3]) + state_data, action_data, clf_data, cbf_data = simulate(x0, duration=20) + + if __name__ == "__main__": - main(use_y_squared=True, with_u_bound=False) + with solvers.MosekSolver.AcquireLicense(): + main() diff --git a/examples/quadrotor2d/demo.py b/examples/quadrotor2d/demo.py index 201af61..ce47588 100644 --- a/examples/quadrotor2d/demo.py +++ b/examples/quadrotor2d/demo.py @@ -90,14 +90,15 @@ def main(use_y_squared: bool, with_u_bound: bool): [ [0, 0, 0, 0, 0, 0, 0], [0, -0.3, 0, 0, 0, 0, 0], - [0.4, -0.3, 0, 0, 0, 0, 0], - [-0.4, -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], ] ), anchor_states=np.zeros((1, 7)), b_anchor_bounds=[(np.array([0.0]), np.array([1]))], weight_V=1, weight_b=np.array([1]), + b_margins=np.array([0.02]), ) max_iter = 5