leaderboards v2

.gitignore

@@ -27,6 +27,10 @@ leaderboard/pendubot/simulation_v1/data/*
 leaderboard/pendubot/robustness_v1/data/*
 leaderboard/pendubot/real_hardware_v1/data/*
 leaderboard/pendubot/real_hardware_v1/src_data/*
+leaderboard/acrobot/simulation_v2/data/*
+leaderboard/acrobot/robustness_v2/data/*
+leaderboard/pendubot/simulation_v2/data/*
+leaderboard/pendubot/robustness_v2/data/*
 data/experiment_records/*
 *local*
 

leaderboard/acrobot/robustness_v2/README.rst

@@ -0,0 +1 @@
+../../../docs/text/leaderboard.robustness.rst

leaderboard/acrobot/robustness_v2/benchmark_controller.py

@@ -0,0 +1,181 @@
+import os
+import argparse
+import importlib
+import numpy as np
+import yaml
+
+# import pickle
+# import pprint
+
+from double_pendulum.analysis.benchmark import benchmarker
+from double_pendulum.analysis.utils import get_par_list
+from double_pendulum.analysis.benchmark_scores import get_scores
+from double_pendulum.analysis.benchmark_plot import plot_benchmark_results
+
+from sim_parameters import (
+    mpar,
+    dt,
+    t_final,
+    x0,
+    goal,
+    integrator,
+    # t0,
+    # design,
+    # model,
+    # robot,
+)
+
+
+def benchmark_controller(controller, save_dir, controller_name=""):
+    if not os.path.exists(save_dir):
+        os.makedirs(save_dir)
+
+    # cost par (only for performance cost calculation)
+    sCu = [1.0, 1.0]
+    sCp = [1.0, 1.0]
+    sCv = [0.01, 0.01]
+    fCp = [100.0, 100.0]
+    fCv = [10.0, 10.0]
+
+    Q = np.array(
+        [
+            [sCp[0], 0.0, 0.0, 0.0],
+            [0.0, sCp[1], 0.0, 0.0],
+            [0.0, 0.0, sCv[0], 0.0],
+            [0.0, 0.0, 0.0, sCv[1]],
+        ]
+    )
+    Qf = np.array(
+        [
+            [fCp[0], 0.0, 0.0, 0.0],
+            [0.0, fCp[1], 0.0, 0.0],
+            [0.0, 0.0, fCv[0], 0.0],
+            [0.0, 0.0, 0.0, fCv[1]],
+        ]
+    )
+    R = np.array([[sCu[0], 0.0], [0.0, sCu[1]]])
+
+    # benchmark parameters
+    eps = [0.35, 0.35, 1.0, 1.0]
+    check_only_final_state = True
+    # eps = [0.1, 0.1, 0.5, 0.5]
+    # check_only_final_state = False
+
+    N_var = 21
+
+    mpar_vars = ["Ir", "m1r1", "I1", "b1", "cf1", "m2r2", "m2", "I2", "b2", "cf2"]
+
+    Ir_var_list = np.linspace(0.0, 1e-4, N_var)
+    m1r1_var_list = get_par_list(mpar.m[0] * mpar.r[0], 0.75, 1.25, N_var)
+    I1_var_list = get_par_list(mpar.I[0], 0.75, 1.25, N_var)
+    b1_var_list = np.linspace(-0.1, 0.1, N_var)
+    cf1_var_list = np.linspace(-0.2, 0.2, N_var)
+    m2r2_var_list = get_par_list(mpar.m[1] * mpar.r[1], 0.75, 1.25, N_var)
+    m2_var_list = get_par_list(mpar.m[1], 0.75, 1.25, N_var)
+    I2_var_list = get_par_list(mpar.I[1], 0.75, 1.25, N_var)
+    b2_var_list = np.linspace(-0.1, 0.1, N_var)
+    cf2_var_list = np.linspace(-0.2, 0.2, N_var)
+
+    modelpar_var_lists = {
+        "Ir": Ir_var_list,
+        "m1r1": m1r1_var_list,
+        "I1": I1_var_list,
+        "b1": b1_var_list,
+        "cf1": cf1_var_list,
+        "m2r2": m2r2_var_list,
+        "m2": m2_var_list,
+        "I2": I2_var_list,
+        "b2": b2_var_list,
+        "cf2": cf2_var_list,
+    }
+
+    meas_noise_mode = "vel"
+    meas_noise_sigma_list = np.linspace(0.0, 0.5, N_var)
+
+    u_noise_sigma_list = np.linspace(0.0, 1.1, N_var)
+
+    u_responses = np.linspace(0.1, 2.0, N_var)
+
+    delay_mode = "posvel"
+    delays = np.linspace(0.0, 0.04, N_var)
+
+    perturbation_repetitions = 50
+    perturbations_per_joint = 3
+    perturbation_min_t_dist = 1.0
+    perturbation_sigma_minmax = [0.05, 0.1]
+    perturbation_amp_minmax = [0.5, 5.0]
+
+    ben = benchmarker(
+        controller=controller,
+        x0=x0,
+        dt=dt,
+        t_final=t_final,
+        goal=goal,
+        epsilon=eps,
+        check_only_final_state=check_only_final_state,
+        integrator=integrator,
+    )
+    ben.set_model_parameter(model_pars=mpar)
+    ben.set_cost_par(Q=Q, R=R, Qf=Qf)
+    ben.compute_ref_cost()
+    res = ben.benchmark(
+        compute_model_robustness=True,
+        compute_noise_robustness=True,
+        compute_unoise_robustness=True,
+        compute_uresponsiveness_robustness=True,
+        compute_delay_robustness=True,
+        compute_perturbation_robustness=True,
+        mpar_vars=mpar_vars,
+        modelpar_var_lists=modelpar_var_lists,
+        meas_noise_mode=meas_noise_mode,
+        meas_noise_sigma_list=meas_noise_sigma_list,
+        u_noise_sigma_list=u_noise_sigma_list,
+        u_responses=u_responses,
+        delay_mode=delay_mode,
+        delays=delays,
+        perturbation_repetitions=perturbation_repetitions,
+        perturbations_per_joint=perturbations_per_joint,
+        perturbation_min_t_dist=perturbation_min_t_dist,
+        perturbation_sigma_minmax=perturbation_sigma_minmax,
+        perturbation_amp_minmax=perturbation_amp_minmax,
+    )
+    # pprint.pprint(res)
+
+    mpar.save_dict(os.path.join(save_dir, "model_parameters.yml"))
+    controller.save(save_dir)
+    ben.save(save_dir)
+
+    plot_benchmark_results(
+        save_dir,
+        "benchmark_results.pkl",
+        costlim=[0, 5],
+        show=False,
+        save=True,
+        file_format="png",
+        scale=0.5,
+    )
+    scores = get_scores(save_dir, "benchmark_results.pkl")
+
+    with open(os.path.join(save_dir, "scores.yml"), "w") as f:
+        yaml.dump(scores, f)
+
+    if os.path.exists(f"readmes/{controller_name}.md"):
+        os.system(f"cp readmes/{controller_name}.md {save_dir}/README.md")
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("controller", help="name of the controller to simulate")
+    controller_arg = parser.parse_args().controller
+    if controller_arg[-3:] == ".py":
+        controller_arg = controller_arg[:-3]
+
+    controller_name = controller_arg[4:]
+    print(f"Simulating controller {controller_name}")
+
+    save_dir = f"data/{controller_name}"
+
+    imp = importlib.import_module(controller_arg)
+    controller = imp.controller
+
+    benchmark_controller(controller, save_dir)

leaderboard/acrobot/robustness_v2/con_ilqr_ilqrmpc_lqr.py

@@ -0,0 +1,146 @@
+import os
+from datetime import datetime
+import numpy as np
+import yaml
+
+from double_pendulum.model.model_parameters import model_parameters
+from double_pendulum.controller.ilqr.ilqr_mpc_cpp import ILQRMPCCPPController
+from double_pendulum.controller.lqr.lqr_controller import LQRController
+from double_pendulum.controller.combined_controller import CombinedController
+from double_pendulum.utils.wrap_angles import wrap_angles_top
+
+from sim_parameters import (
+    mpar,
+    dt,
+    t_final,
+    t0,
+    x0,
+    goal,
+    integrator,
+    design,
+    model,
+    robot,
+)
+
+name = "ilqr_ilqrmpc_lqr"
+leaderboard_config = {
+    "csv_path": name + "/sim_swingup.csv",
+    "name": name,
+    "simple_name": "iLQR MPC stabilization",
+    "short_description": "Online optimization with iterative LQR. Stabilization of iLQR trajectory. Top stabilization with LQR.",
+    "readme_path": f"readmes/{name}.md",
+    "username": "fwiebe",
+}
+
+traj_model = "model_1.1"
+
+# controller parameters
+# N = 20
+N = 100
+con_dt = dt
+N_init = 1000
+max_iter = 10
+# max_iter = 100
+max_iter_init = 1000
+regu_init = 1.0
+max_regu = 10000.0
+min_regu = 0.01
+break_cost_redu = 1e-6
+trajectory_stabilization = True
+shifting = 1
+
+init_csv_path = os.path.join(
+    "../../../data/trajectories", design, traj_model, robot, "ilqr_1/trajectory.csv"
+)
+
+sCu = [0.1, 0.1]
+sCp = [0.1, 0.1]
+sCv = [0.01, 0.1]
+sCen = 0.0
+fCp = [100.0, 10.0]
+fCv = [10.0, 1.0]
+fCen = 0.0
+
+f_sCu = [0.1, 0.1]
+f_sCp = [0.1, 0.1]
+f_sCv = [0.01, 0.01]
+f_sCen = 0.0
+f_fCp = [10.0, 10.0]
+f_fCv = [1.0, 1.0]
+f_fCen = 0.0
+
+# sCu = [0.1, 0.1]
+# sCp = [0.1, 0.1]
+# sCv = [0.01, 0.1]
+# sCen = 0.0
+# fCp = [100.0, 10.0]
+# fCv = [10.0, 1.0]
+# fCen = 0.0
+#
+# f_sCu = [0.0001, 0.0001]
+# f_sCp = [1.0, 1.0]
+# f_sCv = [0.1, 0.1]
+# f_sCen = 0.0
+# f_fCp = [10.0, 10.0]
+# f_fCv = [1.0, 1.0]
+# f_fCen = 0.0
+
+# LQR controller
+Q = np.diag((0.97, 0.93, 0.39, 0.26))
+R = np.diag((0.11, 0.11))
+
+
+def condition1(t, x):
+    return False
+
+
+def condition2(t, x):
+    goal = [np.pi, 0.0, 0.0, 0.0]
+    eps = [0.14, 0.14, 0.5, 0.5]
+    # eps = [0.2, 0.2, 1.0, 1.0]
+
+    y = wrap_angles_top(x)
+
+    delta = np.abs(np.subtract(y, goal))
+    max_diff = np.max(np.subtract(delta, eps))
+    if max_diff > 0.0:
+        return False
+    else:
+        return True
+
+
+controller1 = ILQRMPCCPPController(model_pars=mpar)
+controller1.set_start(x0)
+controller1.set_goal(goal)
+controller1.set_parameters(
+    N=N,
+    dt=con_dt,
+    max_iter=max_iter,
+    regu_init=regu_init,
+    max_regu=max_regu,
+    min_regu=min_regu,
+    break_cost_redu=break_cost_redu,
+    integrator=integrator,
+    trajectory_stabilization=trajectory_stabilization,
+    shifting=shifting,
+)
+controller1.set_cost_parameters(
+    sCu=sCu, sCp=sCp, sCv=sCv, sCen=sCen, fCp=fCp, fCv=fCv, fCen=fCen
+)
+controller1.set_final_cost_parameters(
+    sCu=f_sCu, sCp=f_sCp, sCv=f_sCv, sCen=f_sCen, fCp=f_fCp, fCv=f_fCv, fCen=f_fCen
+)
+controller1.load_init_traj(csv_path=init_csv_path, num_break=40, poly_degree=3)
+
+controller2 = LQRController(model_pars=mpar)
+controller2.set_goal(goal)
+controller2.set_cost_matrices(Q=Q, R=R)
+controller2.set_parameters(failure_value=0.0, cost_to_go_cut=1000)
+controller = CombinedController(
+    controller1=controller1,
+    controller2=controller2,
+    condition1=condition1,
+    condition2=condition2,
+)
+
+controller.init()