From 86cb40afc79531b365ffe0ec58b3f56a819b4d3b Mon Sep 17 00:00:00 2001 From: Franek Stark Date: Wed, 5 Mar 2025 12:01:56 +0100 Subject: [PATCH] Update README and Plotting --- README.md | 56 +- doc/controller/class-diagram | 181 +++--- .../config/mit_controller_sim_go2.yaml | 4 +- .../evaluaion_scripts/comparison_paper.ipynb | 551 +++++++++++------- .../process_rosbags_in_directory.py | 4 +- 5 files changed, 467 insertions(+), 329 deletions(-) diff --git a/README.md b/README.md index ca9361f..7f450c1 100644 --- a/README.md +++ b/README.md @@ -126,6 +126,7 @@ Quadrupedal Walking__ follow the following instruction: ``` > **Note**: If you want to run the simulation and the controller on two different machines, like in the paper, the installation has to be done on both. + **2. _Simulation Machine:_ Start the simulation** * On the machine that should run the simulation run the container and start the simulation ```bash @@ -136,36 +137,43 @@ Quadrupedal Walking__ follow the following instruction: * Visualization will be rendered (if configured) and can be seen in the browser (URL will be shown in the terminal, often [localhost:7000](http://localhost:7000/)). -**3. _Target computer:_ Setup experiments** -* The experiments are run from the script [ws/src/solver_experiments/solver_experiments/solver_experiments.py](ws/src/solver_experiments/solver_experiments/solver_experiments.py), -* The experimemts can be configured the following way: - - [Line 609](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L609) sets which MPC solvers are included in the tests - - [Line 619](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L619) sets which condensed levels are tested for MPC solvers with a sparse interface - - [Line 616](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L616) sets which modes are included into the tests for the HPIPM MPC solver - - [Line 617](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L617) sets which WBC solvers are included in the tests - - [Line 618](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L618) sets which WBC scenes/formulations are included in the tests - - [Line 641](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L641) sets how often an experiment is repeated until it is considered as 'failed' - -**4. _Target computer:_ Run the experiments** -* Make sure the [ws/src/solver_experiments/results](ws/src/solver_experiments/results) folder is empty and exists. -* Start the container and run the experiment script. The script will do all experiments automatically +**3. _Target computer:_ Run the experiments** +* Run the automatic script which runs the set of experiments from the simulation automatically for both prediction horizons: ```bash - sr # This sources the ros environment for the simulation - export ROS_DOMAIN_ID=123 # Make sure both machines run on the same ROS 2 Domain ID - ros2 run solver_experiments solver_experiments - ``` + # Outside of docker run: + ./automatic_solver_experiments.sh 123 main 20 N10_prediction_horizon 10 + + # specfies the name of the target computer which is used to label the experiments (in the paper this was out of [arm_orin, desktop_pc, latte_panda]) - > **Note:** The test script will run all experiments (i.e. combinations of MPC solvers, WBC solvers, condensing levels, etc.). For each experiment it first runs the standing and then the trot scenario. + # specifies which test preset should be run for the paper this is both [mpc, wbc] + + # 123 is the ROS_DOMAIN_ID on which the simulation runs + + # main 20 is the branch and the corresponding prediction horizon on that branch + # N10_prediction_horizon 10 is the branch and the corresponding prediction horizon on that branch + ``` + > **Note:** The test script will spin a docker container, build the project and run all experiments (i.e. combinations of MPC solvers, WBC solvers, condensing levels, etc.). For each experiment it first runs the standing and then the trot scenario. If a single experiment fails (*the terminal might show errors etc.*), the script will after a while automatically restart the experiment and mark the old one as failed, it will repeat this as often as specified above. The full script might take depending on your system at least one hour. Please wait until the script tells you that it is finished. - - > **Note:** If the test script is stopped manually or crashes a subprocess (leg driver or controller) might still be running. In that case it is important to manually kill that processes or just **restart the docker container**. -* Move the [ws/src/solver_experiments/results](ws/src/solver_experiments/results) folder to a new location (another empty folder). The folder should be also renamed with the following pattern, such that it can be later imported into the plotting script: `-N-` + +* The automatic script saves the experiment sets to folders under [ws/src/solver_experiments/](ws/src/solver_experiments/)The folders are named automatically in the following way, such that it can be later imported into the plotting script: `-N-` * `` indicates on which target computer the controller was executed, e.g. `arm_orin`. * `` indicates whith which MPC prediction horizon the experiments were executed, e.g. `10`. - * `` is a free field to put some information about the experiments, e.g. if this field contains `wbc` the respective experiments in that folder will later be excluded from any MPC related plot and vice versa. -* **Changing the prediction horizon**: - * The MPC prediction horizon is a compile constant and has to be changed manually in several places. The easiest is to checkout the branch `N10_prediction_horizon` and recompile the software stack. (The docker container doesn't need a rebuild.) + * `` contains the test preset and a timestamp + +* **Optional** If you with to run your own set of experiments you can change the underlying python script which is runned by the [automatic_solver_experiments.sh](automatic_solver_experiments.sh): + * The experiments are run from the script [ws/src/solver_experiments/solver_experiments/solver_experiments.py](ws/src/solver_experiments/solver_experiments/solver_experiments.py) + * The experimemts can be configured the following way: + - [Line 626](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L609) sets which MPC solvers are included in the tests + - [Line 637](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L637) sets which condensed levels are tested for MPC solvers with a sparse interface + - [Line 633](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L633) sets which modes are included into the tests for the HPIPM MPC solver + - [Line 638](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L638) sets which WBC solvers are included in the tests + - [Line 639](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L639) sets which WBC scenes/formulations are included in the tests + - [Line 645](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L645) sets how often an experiment is repeated until it is considered as 'failed' + - [Line 634](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L634) sets which OSQP backends are included in the tests + - [Line 640](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L640) sets which solver tolerances are included in the tests + - [Lines 649ff](ws/src/solver_experiments/solver_experiments/solver_experiments.py#L649) define the test presets as used by the automatic experiment script + **5. Generate the plots** * Assuming all different experiment result folders (named in the scheme defined above) containing the single experiments have been moved to one empty root folder, the plots can be generated. diff --git a/doc/controller/class-diagram b/doc/controller/class-diagram index b773896..0b33278 100644 --- a/doc/controller/class-diagram +++ b/doc/controller/class-diagram @@ -1,4 +1,4 @@ - + @@ -587,134 +587,141 @@ - + - - + + - - + + - - + + - - + + - - + + - - + + - + - - - - + + - - - + + - - + + - - + + - - + + - - + + + + + - - + + + + + + + - - + + - - + + + + + + + - - + + - - + + - - + + - - + + + + - - + + - - + + - - + + - - - - - - - + + - - + + - - + + - + - + + + + - - - - + + + - - - - - - - - - - - - - - + + + + + + - - - - + + + + + - - + + + + + + + diff --git a/ws/src/controllers/config/mit_controller_sim_go2.yaml b/ws/src/controllers/config/mit_controller_sim_go2.yaml index 61037fc..8081fc1 100644 --- a/ws/src/controllers/config/mit_controller_sim_go2.yaml +++ b/ws/src/controllers/config/mit_controller_sim_go2.yaml @@ -22,6 +22,7 @@ mit_controller_node: ##### SIMULATION slc_world_blend: 1.0 mpc_alpha: 0.00001 mpc_solver: "PARTIAL_CONDENSING_HPIPM" + mpc_osqp_linsys_solver: "qdldl" mpc_warm_start: 1 # 0: off, 1: warm start, 2: hot start (some solvers) ########################## roll, pitch, yaw, x , y , z, rolld,pitchd,yawd, xd, yd , zd mpc_state_weights_stand: [ 30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ] @@ -55,7 +56,7 @@ mit_controller_node: ##### SIMULATION foot_position_based_on_target_orientation: false transformation_filter_size: 20 arc_opt: - solver: "EiquadprogSolver" + solver: "HPIPMSolver" model_urdf: "src/common/model/urdf/go2/urdf/go2_description.urdf" feet_names: [ "fl_contact", "fr_contact","bl_contact", "br_contact" ] joint_names: [ "fl_abad", "fl_shoulder", "fl_knee", "fr_abad", "fr_shoulder", "fr_knee", "bl_abad", "bl_shoulder", "bl_knee", "br_abad", "br_shoulder", "br_knee" ] @@ -71,6 +72,7 @@ mit_controller_node: ##### SIMULATION feet_pose_Kp: [ 1200.,1200.,1200. ] feet_pose_Kd: [ 500.,500.,400. ] model_parameter_weights: [ 1.0, 0.5, 0.5, 0.0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 ] + wbc_solver_tolerances: 0.001 joy_to_target: ros__parameters: init_robot_height: 0.30 diff --git a/ws/src/solver_experiments/evaluaion_scripts/comparison_paper.ipynb b/ws/src/solver_experiments/evaluaion_scripts/comparison_paper.ipynb index 38e8992..c990fec 100644 --- a/ws/src/solver_experiments/evaluaion_scripts/comparison_paper.ipynb +++ b/ws/src/solver_experiments/evaluaion_scripts/comparison_paper.ipynb @@ -8,6 +8,8 @@ "source": [ "%load_ext autoreload\n", "%autoreload 2\n", + "import sys\n", + "sys.path.append(\"/root/ros2_ws/install/interfaces/local/lib/python3.10/dist-packages\")\n", "from process_rosbags_in_directory import process_rosbags_in_directory, calculate_sfpw\n", "import numpy as np\n", "\n", @@ -35,6 +37,8 @@ " if len(exp_name.split(\"-\")) > 1: \n", " if exp_name.split(\"-\")[0].__contains__(\"HPIPM\"):\n", " res[\"mpc_solver\"] = exp_name.split(\"-\")[0].split('_')[-1] + \"-\" + exp_name.split(\"-\")[1]\n", + " elif exp_name.split(\"-\")[0].__contains__(\"OSQP\") and len(exp_name.split(\"-\")) > 7:\n", + " res[\"mpc_solver\"] = exp_name.split(\"-\")[0].split('_')[-1] + \"-\" + exp_name.split(\"-\")[7]\n", " else:\n", " res[\"mpc_solver\"] = exp_name.split(\"-\")[0].split('_')[-1] \n", " \n", @@ -48,7 +52,14 @@ " res[\"wbc_scene\"] = exp_name.split(\"-\")[6]\n", " else:\n", " res[\"wbc_scene\"] = \"AccelerationSceneReducedTSID\"\n", - " \n", + " \n", + " if len(exp_name.split(\"-\")) > 9:\n", + " res[\"mpc_tolerance\"] = float(exp_name.split(\"-\")[8]+\"-\"+exp_name.split(\"-\")[9].replace(\" \", \"\"))\n", + " elif len(exp_name.split(\"-\")) > 8:\n", + " res[\"mpc_tolerance\"] = float(exp_name.split(\"-\")[8])\n", + " else:\n", + " res[\"mpc_tolerance\"] = float(0.0)\n", + " \n", "\n", " else: ##Backup case for old jackob data\n", " idxof = 0\n", @@ -122,6 +133,18 @@ " np.vstack((bag_data[\"cpu_power_consumption\"][\"recv_timestamp_ns\"],bag_data[\"cpu_power_consumption\"][\"data\"]))\n", " )[1,:].mean()]\n", " \n", + " mpc_obj_value = np.array(bag_data[\"solve_time\"][\"qp_objective_value\"])\n", + " mpc_res = np.array(bag_data[\"solve_time\"][\"qp_residuals\"])\n", + " \n", + " for arr, name in zip([mpc_obj_value, mpc_res[:,0], mpc_res[:,1], mpc_res[:,2], mpc_res[:,3]], ['mpc_obj_value', 'mpc_res_stat', 'mpc_res_dyn', 'mpc_res_ineq', 'mpc_res_comp']):\n", + " res[f\"avg_{name}\"] = [arr.mean()]\n", + " res[f\"avg_{name}_min\"] = [arr.min()]\n", + " res[f\"avg_{name}_max\"] = [arr.max()]\n", + " res[f\"avg_{name}_median\"] = [np.median(arr)]\n", + " res[f\"avg_{name}_q25\"] = [np.quantile(arr,0.25)]\n", + " res[f\"avg_{name}_q75\"] = [np.quantile(arr,0.75)]\n", + " \n", + " \n", " return res\n", "\n", "def time_wise_data_mpc(exp_folder : str, exp_name : str, bag_data : dict):\n", @@ -145,7 +168,7 @@ " \n", "\n", "\n", - "def time_wise_data_wpc(exp_folder : str, exp_name : str, bag_data : dict):\n", + "def time_wise_data_wbc(exp_folder : str, exp_name : str, bag_data : dict):\n", " res = {}\n", " res[\"wbc_total_time\"] = np.array(bag_data[\"wbc_solve_time\"][\"total_time\"])\n", " res[\"wbc_total_time_ms\"] = res[\"wbc_total_time\"] * 1000.\n", @@ -172,7 +195,7 @@ "mpc_data = {}\n", "wbc_data = {}\n", "\n", - "process_rosbags_in_directory([pre_proccesed_data, mpc_data, wbc_data], result_path, tag_parser_fun, [pre_process_data, time_wise_data_mpc, time_wise_data_wpc])\n" + "process_rosbags_in_directory([pre_proccesed_data, mpc_data, wbc_data], result_path, tag_parser_fun, [pre_process_data, time_wise_data_mpc, time_wise_data_wbc])\n" ] }, { @@ -186,12 +209,26 @@ "import pandas as pd\n", "\n", "data = pd.DataFrame(pre_proccesed_data)\n", + "group_keys = list(pre_proccesed_data.keys())[:11]\n", + "del pre_proccesed_data\n", "data_mpc = pd.DataFrame(mpc_data)\n", - "data = pd.DataFrame(pre_proccesed_data)\n", + "del mpc_data\n", "data_wbc = pd.DataFrame(wbc_data)\n", + "del wbc_data\n", + "\n", + "\n", "\n", "\n", - "group_keys = list(pre_proccesed_data.keys())[:10]\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "\n", "group_keys.remove(\"trial\")\n", "# filter for multiple cond_N in FULL_CONDENSED\n", "unnecc_full_cond_idx = data.loc[((data[\"cond_type\"] == \"FULL_CONDENSING\") & (data[\"cond_N\"] > 1))].index\n", @@ -225,8 +262,8 @@ "leg_below = p9.theme(legend_position='bottom',\n", " #legend_box=\"vertical\",\n", " legend_box_spacing=0.012,\n", - " legend_spacing=25.0,\n", - " legend_text=p9.element_text(size=8, family=\"Times New Roman\", hjust=0),\n", + " legend_spacing=15.0,\n", + " legend_text=p9.element_text(size=8, hjust=0),\n", " legend_title=p9.element_text(face='bold',size=8),\n", " legend_direction='vertical',\n", " legend_text_position='right'\n", @@ -249,7 +286,8 @@ " 'HPIPM-SPEED' : 'HPIPM\\n(speed)',\n", " 'HPIPM-BALANCE' : 'HPIPM\\n(balance)',\n", " 'HPIPM-SPEED_ABS' : 'HPIPM\\n(speed_abs)',\n", - " 'OSQP' : 'OSQP',\n", + " 'OSQP-qdldl' : 'OSQP\\n(qdldl)',\n", + " 'OSQP-mkl pardiso' : 'OSQP\\n(mkl)',\n", " }\n", "\n", "cond_name_beauty_map = {\n", @@ -262,7 +300,8 @@ " 'DAQP_FULL_CONDENSING' : 'DAQP\\n(dense)',\n", " 'QPOASES_FULL_CONDENSING' : 'qpOASES\\n(dense)',\n", " 'HPIPM-SPEED_FULL_CONDENSING' : 'HPIPM (speed)\\n(dense)',\n", - " 'OSQP_PARTIAL_CONDENSING' : 'OSQP\\n(sparse)',\n", + " 'OSQP-qdldl_PARTIAL_CONDENSING' : 'OSQP (qdldl)\\n(sparse)',\n", + " 'OSQP-mkl pardiso_PARTIAL_CONDENSING' : 'OSQP (mkl)\\n(sparse)',\n", " 'HPIPM-SPEED_PARTIAL_CONDENSING' : 'HPIPM (speed)\\n(sparse)',\n", " 'HPIPM-SPEED_PARTIAL_CONDENSING_H' : 'HPIPM (speed)\\n(half dense)',\n", " 'HPIPM-SPEED_ABS_PARTIAL_CONDENSING' : 'HPIPM (speed_abs)\\n(sparse)',\n", @@ -270,6 +309,18 @@ " 'HPIPM-BALANCE_PARTIAL_CONDENSING' : 'HPIPM (balance)\\n(sparse)',\n", " 'HPIPM-BALANCE_PARTIAL_CONDENSING_H' : 'HPIPM (balance)\\n(half dense)',\n", " }\n", + "\n", + "solver_onlyname_color_map = {\n", + " 'DAQP' : 'red',\n", + " 'QPOASES' : 'orange',\n", + " 'HPIPM-BALANCE' : 'blue',\n", + " 'HPIPM-SPEED_ABS' : 'green',\n", + " 'OSQP-qdldl' : 'darkviolet',\n", + " 'OSQP-mkl pardiso' : 'magenta',\n", + "}\n", + "\n", + "solver_onlyname_order = ['DAQP', 'QPOASES', 'HPIPM-BALANCE', 'HPIPM-SPEED_ABS', 'OSQP-qdldl', 'OSQP-mkl pardiso']\n", + "\n", "target_pc_beauty_map = {\n", " 'arm_orin' : 'Jetson Orin',\n", " 'x86_desktop' : 'Desktop',\n", @@ -317,12 +368,11 @@ "outputs": [], "source": [ "\n", - "\n", + "wbc_solver_beauty_map_empty = {'EiquadprogSolver' : '','HPIPMSolver':'', 'ProxQPSolver' : '', 'QPOasesSolver' : ''}\n", "filtered_data = min_trail_data.loc[ \n", " (min_trail_data['experiment'] == \"trot_exp\") &\n", " (min_trail_data['experiment_tag'].str.contains('wbc')) \n", - " & (min_trail_data['N'] == \"N20\")\n", - " &~min_trail_data['failed']\n", + " #& (min_trail_data['N'] == \"N20\")\n", " ].melt(id_vars=group_keys, value_vars=[\"avg_wbc_total_time_ms\"])\n", "\n", "wbc_bars = (\n", @@ -350,28 +400,29 @@ "filtered_data = data_wbc.loc[ \n", " (data_wbc['experiment'] == \"trot_exp\") &\n", " (data_wbc['experiment_tag'].str.contains('wbc')) \n", - " & (data_wbc['N'] == \"N20\")\n", - " &~data_wbc['failed']\n", + " #& (data_wbc['N'] == \"N10\")\n", + " #&~data_wbc['failed']\n", " ]\n", "\n", - "(\n", + "wbc_box = (\n", "ggplot(filtered_data,\n", - " aes(x=\"target_pc\",y=\"wbc_total_time_ms\", fill='wbc_solver')) +\n", - " p9.facet_grid(cols='wbc_scene', labeller=p9.labeller(cols=wbc_scene_beauty_map)) +\n", - " p9.geom_boxplot()\n", + " aes(x='wbc_solver', y=\"wbc_total_time_ms\", fill='wbc_solver')) +\n", + " p9.facet_grid(rows='wbc_scene',cols='target_pc', labeller=p9.labeller(rows=wbc_scene_beauty_map, cols=target_pc_beauty_map), scales='free_x') +\n", + " p9.geom_boxplot(alpha=1.0, outlier_shape='', size=0.15)\n", " + p9.theme(figure_size=(16,4))\n", " + icra_style\n", " + leg_below\n", " + p9.scale_fill_discrete(labels=wbc_solver_beauty_map, name='WBC Solver')\n", - " + p9.scale_x_discrete(labels=target_pc_beauty_map)\n", - " + p9.ylab('Mean WBC total time (ms)')\n", - " + p9.xlab('Target computer')\n", - " + p9.guides(fill=p9.guide_legend(nrow=1))\n", - " + p9.theme(axis_text_y=p9.element_text(rotation=90,vjust=0.5, hjust=1))\n", + " + p9.xlab('')\n", + " + p9.scale_x_discrete(labels=wbc_solver_beauty_map_empty)\n", + " #+ p9.xlab('Target computer')\n", + " + p9.guides(fill=p9.guide_legend(nrow=1), color=False)\n", " + p9.coord_flip()\n", - " \n", - "\n", - ").show()\n", + " + p9.theme(figure_size=(icra_width, 2.5), panel_spacing_x=0.03)\n", + " + p9.ylab('WBC solve time (ms)')\n", + ")\n", + "wbc_box.show()\n", + "wbc_box.save('wbc_box.pdf')\n", "\n", "(\n", "ggplot(filtered_data,\n", @@ -387,7 +438,7 @@ " + p9.xlab('Target computer')\n", " + p9.guides(fill=p9.guide_legend(nrow=1))\n", " + p9.coord_flip()\n", - " + p9.theme(axis_text_y=p9.element_text(rotation=90,vjust=0.5, hjust=1))\n", + " + p9.theme(axis_text_x=p9.element_text(rotation=90,vjust=0.5, hjust=1)) \n", "\n", ").show()\n", "\n", @@ -404,139 +455,6 @@ "metadata": {}, "outputs": [], "source": [ - "filtered_data = data_wbc.loc[ \n", - " (data_wbc['experiment_tag'].str.contains('wbc'))\n", - " \n", - " ]\n", - "\n", - "filtered_data.drop( filtered_data[(filtered_data['wbc_total_time_ms'] > 1.0)].index, inplace=True)\n", - "\n", - "def hist(data):\n", - " return(\n", - " ggplot(data,\n", - " aes(x=\"wbc_total_time_ms\", fill=\"experiment\", y=p9.after_stat('ncount')))\n", - " #+ p9.geom_density(aes(color=\"mpc_solver_cond\", y=p9.after_stat(\"count*binwidth\")),alpha=0.01, size=0.5)\n", - " + p9.facet_grid(cols=\"target_pc\" ,rows=[\"wbc_solver\", \"wbc_scene\"], \n", - " #labeller=p9.labeller(solver_name_beauty_map2)\n", - " )\n", - " + p9.geom_histogram(data.loc[data['experiment'].str.contains('stand')],color=\"None\", alpha=0.5, bins=100)\n", - " + p9.geom_histogram(data.loc[data['experiment'].str.contains('trot')],color=\"None\", alpha=0.5, bins=100)\n", - " #+ icra_style\n", - " + leg_below\n", - " + p9.guides(fill=p9.guide_legend(ncol=4, title='Experiment'))\n", - " + p9.theme(axis_text_x=p9.element_text(rotation=0))\n", - " #+ p9.scale_fill_discrete(\n", - " # labels=solver_name_beauty_map2\n", - " #)\n", - " #+ p9.scale_color_discrete(\n", - " # labels=solver_name_beauty_map2\n", - " #)\n", - " #+p9.geom_vline(xintercept=0.01, color='red')\n", - " + p9.labs(x='WBC total time (ms)',y='Normalized Count')\n", - " #+ p9.geom_text()\n", - " + p9.theme(strip_text_y=p9.element_text(angle=0),axis_text=p9.element_text(size=8) ) \n", - " #+ p9.scale_y_log10()\n", - " #+ p9.scale_fill_discrete(labels={'stand_exp' : 'Standing', 'trot_exp' : 'Trotting'})\n", - " )\n", - "hist(filtered_data.loc[filtered_data['N'] == \"N10\"]).show()\n", - "hist(filtered_data.loc[filtered_data['N'] == \"N20\"]).show()\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "filtered_data = min_trail_data.loc[ \n", - " ~(min_trail_data['failed']) & \n", - " (((min_trail_data['N'] == \"N10\") & (min_trail_data[\"cond_N\"] == 10)) | ((min_trail_data['N'] == \"N20\") & (min_trail_data[\"cond_N\"] == 20)) | (min_trail_data[\"cond_type\"] == \"FULL_CONDENSING\")) &\n", - " (min_trail_data['experiment'].str.contains('trot')) &\n", - " (~min_trail_data['experiment_tag'].str.contains('wbc') \n", - " & ((min_trail_data['mpc_solver'].str.contains(\"SPEED\")\n", - " # &(~min_trail_data['mpc_solver'].str.contains(\"ABS\"))\n", - " )\n", - " | (~min_trail_data['mpc_solver'].str.contains(\"HPIPM\"))))\n", - " & ~((min_trail_data[\"cond_type\"] == \"FULL_CONDENSING\") & min_trail_data['mpc_solver'].str.contains(\"HPIPM\"))\n", - " ]#.melt(id_vars=group_keys, value_vars=[\"avg_mpc_sfpw\", \"avg_wbc_sfpw\", \"avg_watts\"])\n", - "filtered_data[\"mpc_solver_cond\"] = filtered_data[\"mpc_solver\"] + \"_\" + filtered_data[\"cond_type\"]\n", - "\n", - "efficiency_plot = (\n", - " ggplot(filtered_data,\n", - " aes(x=\"mpc_solver_cond\",y=\"avg_mpc_sfpw\", fill=\"target_pc\", label=\"experiment_tag\", shape='experiment_tag')) +\n", - " p9.facet_grid(rows='N', scales='free_y') +\n", - " p9.geom_bar(stat=\"identity\", position=\"dodge2\")\n", - " #+ p9.coord_flip()\n", - " + icra_style\n", - " + leg_below\n", - " + p9.guides(fill=p9.guide_legend(ncol=3))\n", - " + p9.theme(axis_text_x=p9.element_text(rotation=0))\n", - " + p9.scale_x_discrete(\n", - " labels=solver_name_beauty_map\n", - " )\n", - " + p9.labs(x='MPC solver', y='Mean SFPW (Hz/W)')\n", - " #+ p9.geom_text()\n", - ")\n", - "\n", - "efficiency_plot.show()\n", - "\n", - "\n", - "filtered_data = data_mpc.loc[ \n", - " ~(data_mpc['failed']) & \n", - " (\n", - " ((data_mpc['N'] == \"N10\") & (data_mpc[\"cond_N\"] == 10)) | \n", - " ((data_mpc['N'] == \"N20\") & (data_mpc[\"cond_N\"] == 20)) | \n", - " (data_mpc[\"cond_type\"] == \"FULL_CONDENSING\") |\n", - " ((data_mpc[\"mpc_solver\"].str.contains(\"HPIPM\") & (data_mpc[\"N\"] == \"N10\") & (data_mpc[\"cond_N\"] == 5))) |\n", - " ((data_mpc[\"mpc_solver\"].str.contains(\"HPIPM\") & (data_mpc[\"N\"] == \"N20\") & (data_mpc[\"cond_N\"] == 10)))\n", - " \n", - " ) &\n", - " (data_mpc['experiment'].str.contains('trot')) &\n", - " (~data_mpc['experiment_tag'].str.contains('wbc'))\n", - " & ((data_mpc['mpc_solver'].str.contains(\"SPEED\") & (~data_mpc['mpc_solver'].str.contains(\"ABS\")))| (~data_mpc['mpc_solver'].str.contains(\"HPIPM\")))\n", - " & ~((data_mpc[\"cond_type\"] == \"FULL_CONDENSING\") & data_mpc['mpc_solver'].str.contains(\"HPIPM\"))\n", - " ]\n", - "filtered_data[\"mpc_solver_cond\"] = filtered_data[\"mpc_solver\"] + \"_\" + filtered_data[\"cond_type\"]\n", - "\n", - "\n", - "filtered_data['N_numeric'] = filtered_data['N'].str.extract('(\\d+)').astype(float)\n", - "\n", - "\n", - "filtered_data['mpc_solver_cond'] = filtered_data['mpc_solver'] + \"_\" + filtered_data['cond_type']\n", - "\n", - "sel = filtered_data[filtered_data['cond_N'] == filtered_data['N_numeric'] / 2]\n", - "\n", - "filtered_data.loc[sel.index, 'mpc_solver_cond'] = filtered_data.loc[sel.index, 'mpc_solver_cond'] +'_H'\n", - "\n", - "efficiency_plot = (\n", - " ggplot(filtered_data,\n", - " aes(x=\"mpc_solver_cond\",y=\"mpc_sfpw\", color=\"target_pc\", label=\"experiment_tag\")) +\n", - " p9.facet_grid(rows='N', scales='free_y') +\n", - " p9.geom_boxplot(outlier_alpha=0.1) \n", - " #+ p9.coord_flip()\n", - " + icra_style\n", - " + leg_below\n", - " + p9.guides(color=p9.guide_legend(ncol=3))\n", - " + p9.theme(axis_text_x=p9.element_text(rotation=0))\n", - " + p9.scale_x_discrete(\n", - " labels=solver_name_beauty_map2\n", - " )\n", - " + p9.labs(x='MPC solver', y='MPC mean SFPW (Hz/W)')\n", - " #+ p9.geom_text()\n", - ")\n", - "\n", - "efficiency_plot.save(\"efficiency.pdf\")\n", - "efficiency_plot.show()\n", - "\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "\n", "def plot_entity_per_solver(entity: str, data, group_by=\"N\", rows_by='experiment'):\n", " return (\n", " ggplot(data, \n", @@ -558,27 +476,28 @@ " labels={True: \"Exp. failed\", False:\"\"}\n", " )\n", " + p9.scale_linetype_manual({\"PARTIAL_CONDENSING\" : 'dotted', \"FULL_CONDENSING\" : 'solid'}, labels=cond_name_beauty_map)\n", - " + p9.scale_color_discrete(labels=solver_onlyname_beauty_map)\n", - " + p9.scale_color_brewer(type='qualitative', palette=6, labels=solver_onlyname_beauty_map)\n", + " + p9.scale_color_manual(labels=solver_onlyname_beauty_map, values=solver_onlyname_color_map)\n", " + p9.scale_shape_manual({'N10' : 'x', 'N20' : '.'}, labels={'N10' : '$N=10$', 'N20' : '$N=20$'})\n", " + p9.scale_fill_discrete(labels=solver_onlyname_beauty_map)\n", - " + p9.guides(color=p9.guide_legend(order=1, ncol=5), linetype=False, shape=p9.guide_legend(order=3, nrow=1), size=p9.guide_legend(order=4))\n", + " + p9.guides(color=p9.guide_legend(order=1, ncol=6), linetype=False, shape=p9.guide_legend(order=3, nrow=1), size=p9.guide_legend(order=4))\n", " + p9.annotate(\"text\", x=18.5, y=0.0005, label='sparse $\\\\rightarrow$', size=8, color='black')\n", " + p9.annotate(\"text\", x=1.3, y=0.0005, label='$\\leftarrow$ dense', size=8, color='black')\n", " + p9.theme(panel_grid_minor=p9.element_blank())\n", + " + p9.theme(legend_spacing=10)\n", " \n", " \n", " )\n", "\n", "\n", "filtered_data = min_trail_data.loc[\n", + " (min_trail_data['mpc_tolerance'] == 0.001) &\n", " (min_trail_data['experiment'] == \"trot_exp\") &\n", " (~min_trail_data['experiment_tag'].str.contains('wbc')) &\n", " (min_trail_data['wbc_solver'] == \"EiquadprogSolver\") &\n", " ((min_trail_data['mpc_solver'].str.contains('HPIPM')) & (\n", " min_trail_data['mpc_solver'].str.contains(\"BALANCE\") |\n", " min_trail_data['mpc_solver'].str.contains(\"SPEED_ABS\") \n", - " ) | ~(min_trail_data['mpc_solver'].str.contains('HPIPM')))\n", + " ) | ~(min_trail_data['mpc_solver'].str.contains('HPIPM'))) & (~min_trail_data['mpc_solver'].str.contains('OSQP') | min_trail_data['mpc_solver'].str.contains('qdldl'))\n", " #&(((min_trail_data['mpc_solver'].str.contains(\"SPEED\") #& (~min_trail_data['mpc_solver'].str.contains(\"ABS\"))\n", " # )\n", " #| (~min_trail_data['mpc_solver'].str.contains(\"HPIPM\"))\n", @@ -609,6 +528,7 @@ "plot_entity_per_solver(\"mpc_solve_time_over\",filtered_data).show()\n", "plot_entity_per_solver(\"avg_mpc_condensing_time\",filtered_data).show()\n", "plot_entity_per_solver(\"avg_wbc_total_time\",filtered_data).show()\n", + "plot_entity_per_solver(\"avg_mpc_obj_value\",filtered_data).show()\n", "efficiency_big_plots = (plot_entity_per_solver(\"avg_mpc_sfpw_median\",filtered_data)\n", " #+ p9.geom_ribbon(mapping=aes(ymin=\"avg_mpc_sfpw_q25\", ymax=\"avg_mpc_sfpw_q75\", fill='mpc_solver'), alpha=0.2, linetype='None')\n", " #+ p9.geom_rect(data = filtered_data.loc[(filtered_data[\"cond_type\"].str.contains('FULL'))], \n", @@ -620,12 +540,53 @@ "efficiency_big_plots.show()\n", "#plot_entity_per_solver(\"avg_wbc_sfpw\",filtered_data).show()\n", "\n", - "\n", "pd.set_option('display.max_rows', None)\n", "pd.set_option('display.max_columns', None)\n", - "pd.set_option('display.width', None)\n", + "pd.set_option('display.width', None)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "filtered_data = data_wbc.loc[ \n", + " (data_wbc['experiment_tag'].str.contains('wbc'))\n", + " \n", + " ]\n", "\n", - "\n" + "filtered_data.drop( filtered_data[(filtered_data['wbc_total_time_ms'] > 1.0)].index, inplace=True)\n", + "\n", + "def hist(data):\n", + " return(\n", + " ggplot(data,\n", + " aes(x=\"wbc_total_time_ms\", fill=\"experiment\", y=p9.after_stat('ncount')))\n", + " #+ p9.geom_density(aes(color=\"mpc_solver_cond\", y=p9.after_stat(\"count*binwidth\")),alpha=0.01, size=0.5)\n", + " + p9.facet_grid(cols=\"target_pc\" ,rows=[\"wbc_solver\", \"wbc_scene\"], \n", + " #labeller=p9.labeller(solver_name_beauty_map2)\n", + " )\n", + " + p9.geom_histogram(data.loc[data['experiment'].str.contains('stand')],color=\"None\", alpha=0.5, bins=100)\n", + " + p9.geom_histogram(data.loc[data['experiment'].str.contains('trot')],color=\"None\", alpha=0.5, bins=100)\n", + " #+ icra_style\n", + " + leg_below\n", + " + p9.guides(fill=p9.guide_legend(ncol=4, title='Experiment'))\n", + " + p9.theme(axis_text_x=p9.element_text(rotation=0))\n", + " #+ p9.scale_fill_discrete(\n", + " # labels=solver_name_beauty_map2\n", + " #)\n", + " #+ p9.scale_color_discrete(\n", + " # labels=solver_name_beauty_map2\n", + " #)\n", + " #+p9.geom_vline(xintercept=0.01, color='red')\n", + " + p9.labs(x='WBC total time (ms)',y='Normalized Count')\n", + " #+ p9.geom_text()\n", + " + p9.theme(strip_text_y=p9.element_text(angle=0),axis_text=p9.element_text(size=8) ) \n", + " #+ p9.scale_y_log10()\n", + " #+ p9.scale_fill_discrete(labels={'stand_exp' : 'Standing', 'trot_exp' : 'Trotting'})\n", + " )\n", + "hist(filtered_data.loc[filtered_data['N'] == \"N10\"]).show()\n", + "hist(filtered_data.loc[filtered_data['N'] == \"N20\"]).show()\n" ] }, { @@ -634,12 +595,50 @@ "metadata": {}, "outputs": [], "source": [ - "gk = group_keys.copy()\n", - "gk.remove('cond_N')\n", - "minidx = filtered_data.loc[(filtered_data['N'] == 'N10')].groupby(gk)['avg_mpc_solve_time'].idxmin()\n", + "filtered_data = min_trail_data.loc[ #(min_trail_data['mpc_tolerance'] == 0.001) &\n", + " #~(min_trail_data['failed']) &\n", + " (\n", + " ~min_trail_data['mpc_solver'].str.contains('OSQP') | min_trail_data['experiment_tag'].str.contains('osqp')\n", + " ) & \n", + " (((min_trail_data['N'] == \"N10\") & (min_trail_data[\"cond_N\"] == 10)) | ((min_trail_data['N'] == \"N20\") & (min_trail_data[\"cond_N\"] == 20)) | (min_trail_data[\"cond_type\"] == \"FULL_CONDENSING\")) &\n", + " (min_trail_data['experiment'].str.contains('trot')) &\n", + " (~min_trail_data['experiment_tag'].str.contains('wbc') \n", + " & ((min_trail_data['mpc_solver'].str.contains(\"SPEED_ABS\") | (min_trail_data['mpc_solver'].str.contains(\"BALANCE\"))\n", + " # &(~min_trail_data['mpc_solver'].str.contains(\"ABS\"))\n", + " )\n", + " | (~min_trail_data['mpc_solver'].str.contains(\"HPIPM\"))))\n", + " & ~((min_trail_data[\"cond_type\"] == \"FULL_CONDENSING\") & min_trail_data['mpc_solver'].str.contains(\"HPIPM\"))\n", + " ]#.melt(id_vars=group_keys, value_vars=[\"avg_mpc_sfpw\", \"avg_wbc_sfpw\", \"avg_watts\"])\n", + "filtered_data[\"mpc_solver_cond\"] = filtered_data[\"mpc_solver\"] + \"_\" + filtered_data[\"cond_type\"]\n", "\n", + "for name in ['mpc_solve_time', 'mpc_res_stat', 'mpc_res_dyn', 'mpc_res_ineq', 'mpc_res_comp']:\n", + " efficiency_plot = (\n", + " ggplot(filtered_data,\n", + " aes(x=\"mpc_solver_cond\",\n", + " ymin = f\"avg_{name}_min\",\n", + " lower = f\"avg_{name}_q25\",\n", + " middle = f\"avg_{name}_median\",\n", + " upper = f\"avg_{name}_q75\",\n", + " ymax = f\"avg_{name}_max\",\n", + " fill=\"target_pc\")) +\n", + " p9.facet_grid(rows='N', scales='free_y', cols=\"mpc_tolerance\") +\n", + " p9.geom_boxplot(stat=\"identity\")\n", + " #+ p9.coord_flip()\n", + " #+ icra_style\n", + " + leg_below\n", + " + p9.guides(fill=p9.guide_legend(ncol=3))\n", + " + p9.theme(axis_text_x=p9.element_text(rotation=90))\n", + " + p9.scale_x_discrete(\n", + " labels=solver_name_beauty_map2\n", + " )\n", + " + p9.scale_y_log10()\n", + " + p9.labs(x='MPC solver', y=f'{name}')\n", + " #+ p9.geom_text()\n", + " )\n", + " \n", + " efficiency_plot.show()\n", "\n", - "filtered_data.loc[minidx][filtered_data['target_pc'].str.contains('panda')].sort_values('avg_mpc_solve_time')[[\"mpc_solver\",\"cond_type\",\"cond_N\",\"avg_mpc_solve_time\",\"avg_mpc_sfpw\"]]" + "\n" ] }, { @@ -649,6 +648,7 @@ "outputs": [], "source": [ "filtered_data = min_trail_data.loc[ \n", + " (min_trail_data['mpc_tolerance'] == 0.001) &\n", " ~(min_trail_data['experiment_tag'].str.contains('wbc'))\n", " & (((min_trail_data['N'] == \"N10\") & (min_trail_data[\"cond_N\"] == 10)) | ((min_trail_data['N'] == \"N20\") & (min_trail_data[\"cond_N\"] == 20)) | (min_trail_data[\"cond_type\"] == \"FULL_CONDENSING\")) \n", " ]#.melt(id_vars=group_keys, value_vars=[\"avg_mpc_sfpw\", \"avg_wbc_sfpw\"])\n", @@ -656,6 +656,7 @@ "filtered_data[\"mpc_solver_cond\"] = filtered_data[\"mpc_solver\"] + \"_\" + filtered_data[\"cond_type\"]\n", "p9.geoms.geom_boxplot.DEFAULT_AES['size'] = 0.1\n", "\n", + "\n", "variance_plot = (\n", " ggplot(filtered_data,\n", " aes(x=\"mpc_solver_cond\",\n", @@ -673,11 +674,11 @@ " + p9.labs(x='Target computer', y='Solve time (s)', fill='Experiment')\n", " + p9.scale_fill_discrete(labels={'stand_exp':'Standing', 'trot_exp':'Dynamic trot'})\n", " #+ p9.scale_y_log10()\n", - " + icra_style_double_col\n", + " #+ icra_style_double_col\n", " + leg_below\n", " + p9.guides(fill=p9.guide_legend(nrow=1), color=p9.guide_legend(nrow=1))\n", " + p9.scale_x_discrete(\n", - " labels=solver_name_beauty_map\n", + " labels=solver_name_beauty_map2\n", " )\n", " + p9.scale_color_manual({True:'red', False:'black'},labels={True:'Failed', False:'Sucessfull'}, name=\"Experiment status\")\n", ")\n", @@ -687,40 +688,6 @@ "\n" ] }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "filtered_data = data_mpc.loc[ \n", - " ~(data_mpc['failed']) & \n", - " (\n", - " ((data_mpc['N'] == \"N10\") & (data_mpc[\"cond_N\"] == 10)) | \n", - " ((data_mpc['N'] == \"N20\") & (data_mpc[\"cond_N\"] == 20)) | \n", - " (data_mpc[\"cond_type\"] == \"FULL_CONDENSING\") |\n", - " ((data_mpc[\"mpc_solver\"].str.contains(\"HPIPM\") & (data_mpc[\"N\"] == \"N10\") & (data_mpc[\"cond_N\"] == 5))) |\n", - " ((data_mpc[\"mpc_solver\"].str.contains(\"HPIPM\") & (data_mpc[\"N\"] == \"N20\") & (data_mpc[\"cond_N\"] == 10)))\n", - " \n", - " ) &\n", - "\n", - " ((data_mpc['mpc_solver'].str.contains('HPIPM')) & (data_mpc[\"cond_type\"] == \"PARTIAL_CONDENSING\") & (\n", - " data_mpc['mpc_solver'].str.contains(\"BALANCE\") |\n", - " data_mpc['mpc_solver'].str.contains(\"SPEED_ABS\") \n", - " ) | ~(data_mpc['mpc_solver'].str.contains('HPIPM'))) &\n", - " (data_mpc['experiment'].str.contains('trot')) &\n", - " (~data_mpc['experiment_tag'].str.contains('wbc'))\n", - " \n", - " ]\n", - "\n", - "pd.set_option('display.max_rows', None)\n", - "pd.set_option('display.max_columns', None)\n", - "pd.set_option('display.width', None)\n", - "\n", - "filtered_data.groupby(group_keys)[\"mpc_solve_time\"].describe()\n", - "\n" - ] - }, { "cell_type": "code", "execution_count": null, @@ -740,35 +707,128 @@ "\n", "p9.geoms.geom_boxplot.DEFAULT_AES['size'] = 0.1\n", "\n", + "to_plot = \"mpc_sfpw\"\n", + "\n", "variance_plot = (\n", " ggplot(filtered_data,\n", " aes(x=\"mpc_solver_cond\",\n", - " y = \"mpc_solve_time\",\n", + " y = to_plot,\n", " #color=\"failed\",\n", " fill=\"experiment\")) +\n", " p9.facet_wrap(['target_pc', 'N'],scales=\"free_y\", dir='v')\n", - " + p9.geom_boxplot(outlier_alpha=0.1, outlier_size=0.5)\n", + " + p9.geom_boxplot(outlier_alpha=0.2, outlier_size=0.5)\n", " + p9.theme(figure_size=(20,10),\n", " axis_text_x=p9.element_text(rotation=0))\n", - " + p9.labs(x='Solver', y='MPC solve time (s)', fill='Experiment')\n", + " + p9.labs(x='Solver', y=to_plot, fill='Experiment')\n", " + p9.scale_fill_discrete(labels={'stand_exp':'Standing', 'trot_exp':'Trotting'})\n", " #+ p9.scale_y_log10()\n", - " + icra_style_double_col\n", + " #+ icra_style_double_col\n", " + leg_below\n", " + p9.guides(fill=p9.guide_legend(nrow=1), color=p9.guide_legend(nrow=1))\n", " + p9.scale_x_discrete(\n", " labels=solver_name_beauty_map2\n", " )\n", - " + p9.theme(axis_text_x=p9.element_text(size=8.0, angle=90))\n", + " + p9.theme(axis_text_x=p9.element_text(size=14.0, angle=90))\n", " #+ p9.scale_color_manual({True:'red', False:'black'},labels={True:'Failed', False:'Sucessfull'}, name=\"Experiment status\")\n", ")\n", "\n", + "\n", + "\n", "variance_plot.show()\n", "variance_plot.save('mpc_variance_new.pdf')\n", "\n", + "\n", + "(\n", + " ggplot(data_mpc)\n", + " \n", + ")\n", + "\n", "\n" ] }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "filtered_data = data_mpc.loc[ \n", + " \n", + " (data_mpc['mpc_tolerance'] == 0.001) & \n", + " (\n", + " ((data_mpc['N'] == \"N10\") & (data_mpc[\"cond_N\"] == 10)) | \n", + " ((data_mpc['N'] == \"N20\") & (data_mpc[\"cond_N\"] == 20)) | \n", + " (data_mpc[\"cond_type\"] == \"FULL_CONDENSING\") |\n", + " ((data_mpc[\"mpc_solver\"].str.contains(\"HPIPM\") & (data_mpc[\"N\"] == \"N10\") & (data_mpc[\"cond_N\"] == 10))) |\n", + " ((data_mpc[\"mpc_solver\"].str.contains(\"HPIPM\") & (data_mpc[\"N\"] == \"N20\") & (data_mpc[\"cond_N\"] == 20)))\n", + " \n", + " ) &\n", + "\n", + " ((data_mpc['mpc_solver'].str.contains('HPIPM')) & (data_mpc[\"cond_type\"] == \"PARTIAL_CONDENSING\") & (\n", + " data_mpc['mpc_solver'].str.contains(\"BALANCE\") |\n", + " data_mpc['mpc_solver'].str.contains(\"SPEED_ABS\") \n", + " ) | ~(data_mpc['mpc_solver'].str.contains('HPIPM'))) &\n", + " (data_mpc['N'] == \"N10\") &\n", + " (data_mpc['experiment'].str.contains('trot')) &\n", + " (~data_mpc['experiment_tag'].str.contains('wbc'))\n", + " \n", + " ]\n", + "filtered_data['mpc_solver_cond'] = filtered_data['mpc_solver'] + \"_\" + filtered_data['cond_type']\n", + "\n", + "pd.set_option('display.max_rows', None)\n", + "pd.set_option('display.max_columns', None)\n", + "pd.set_option('display.width', None)\n", + "\n", + "\n", + "solver_name_beauty_map2_hack = {\n", + " 'DAQP_FULL_CONDENSING' : 'DAQP\\n(dense)',\n", + " 'QPOASES_FULL_CONDENSING' : 'qpOASES\\n(dense)',\n", + " 'HPIPM-SPEED_FULL_CONDENSING' : 'HPIPM (speed)\\n(dense)',\n", + " 'OSQP-qdldl_PARTIAL_CONDENSING' : 'OSQP\\n (qdldl)\\n(sparse)',\n", + " 'OSQP-mkl pardiso_PARTIAL_CONDENSING' : 'OSQP\\n (mkl)\\n(sparse)',\n", + " 'HPIPM-SPEED_PARTIAL_CONDENSING' : 'HPIPM (speed)\\n(sparse)',\n", + " 'HPIPM-SPEED_PARTIAL_CONDENSING_H' : 'HPIPM (speed)\\n(half dense)',\n", + " 'HPIPM-SPEED_ABS_PARTIAL_CONDENSING' : 'HPIPM\\nspeed_abs\\n(sparse)',\n", + " 'HPIPM-SPEED_ABS_PARTIAL_CONDENSING_H' :'HPIPM (speed_abs)\\n(half dense)',\n", + " 'HPIPM-BALANCE_PARTIAL_CONDENSING' : 'HPIPM\\nbalance\\n(sparse)',\n", + " 'HPIPM-BALANCE_PARTIAL_CONDENSING_H' : 'HPIPM (balance)\\n(half dense)',\n", + " }\n", + "#filtered_data.groupby(group_keys)[\"mpc_solve_time\"].describe()\n", + "\n", + "data_mean = filtered_data.groupby([\"experiment\", \"target_pc\", \"mpc_solver_cond\"])[\"mpc_sfpw\"].describe(percentiles=[0.05,0.95]).reset_index()\n", + "\n", + "bins = 100\n", + "(\n", + " ggplot(filtered_data,\n", + " aes(x=\"mpc_sfpw\", fill=\"experiment\", y=p9.after_stat('ncount')))\n", + " #+ p9.geom_density(aes(color=\"mpc_solver_cond\", y=p9.after_stat(\"count*binwidth\")),alpha=0.01, size=0.5)\n", + " + p9.facet_wrap(['target_pc', 'mpc_solver_cond'],scales=\"free\", dir='v', labeller=p9.labeller(solver_name_beauty_map2_hack,),ncol=7)\n", + " + p9.geom_vline(data=data_mean, mapping=p9.aes(xintercept=\"mean\"))\n", + " + p9.geom_vline(data=data_mean, mapping=p9.aes(xintercept=\"mean + std\"),color='red')\n", + " + p9.geom_vline(data=data_mean, mapping=p9.aes(xintercept=\"mean - std\"),color='red')\n", + " + p9.geom_vline(data=data_mean, mapping=p9.aes(xintercept=\"5%\"), color='green')\n", + " + p9.geom_vline(data=data_mean, mapping=p9.aes(xintercept=\"95%\"), color='green')\n", + " + p9.geom_histogram(color=\"None\", alpha=0.8, bins=bins)\n", + " #+ icra_style\n", + " + leg_below\n", + " + p9.guides(fill=p9.guide_legend(ncol=4, title=''))\n", + " + p9.theme(axis_text_x=p9.element_text(rotation=0))\n", + " + p9.labs(x='MPC solve time (ms)',y='Normalized count')\n", + " #+ p9.xlim(0, max)\n", + " #+ p9.geom_text()\n", + " + p9.theme(strip_text_y=p9.element_text(angle=0),axis_text=p9.element_text(size=8),axis_text_y=p9.element_text(size=8))\n", + " + p9.theme(figure_size=(20,5))\n", + " #+ p9.scale_y_log10()\n", + " + p9.scale_fill_discrete(labels={'stand_exp' : 'Standing', 'trot_exp' : 'Trotting'}, name=\"\")\n", + " + p9.scale_y_continuous(breaks=[0.0,1.0], limits=(0,1.1))\n", + " + p9.scale_color_discrete( \n", + " labels=solver_name_beauty_map2_hack\n", + " )\n", + ").show()\n", + "\n", + "data_mean\n" + ] + }, { "cell_type": "code", "execution_count": null, @@ -791,7 +851,10 @@ " ((data_mpc['mpc_solver'].str.contains('HPIPM')) & (data_mpc[\"cond_type\"] == \"PARTIAL_CONDENSING\") & (\n", " data_mpc['mpc_solver'].str.contains(\"BALANCE\") |\n", " data_mpc['mpc_solver'].str.contains(\"SPEED_ABS\") \n", - " ) | ~(data_mpc['mpc_solver'].str.contains('HPIPM')))\n", + " ) | ~(data_mpc['mpc_solver'].str.contains('HPIPM'))) &\n", + " (\n", + " ~(data_mpc['mpc_solver'].str.contains('OSQP')) | (data_mpc['mpc_solver'].str.contains('qdldl')) \n", + " )\n", " ]\n", "\n", "filtered_data['N_numeric'] = filtered_data['N'].str.extract('(\\d+)').astype(float)\n", @@ -850,7 +913,8 @@ " 'DAQP_FULL_CONDENSING' : 'DAQP\\n(dense)' + add_blank,\n", " 'QPOASES_FULL_CONDENSING' : 'qpOASES\\n(dense)' + add_blank,\n", " 'HPIPM-SPEED_FULL_CONDENSING' : 'HPIPM (speed)\\n(dense)' + add_blank,\n", - " 'OSQP_PARTIAL_CONDENSING' : 'OSQP\\n(sparse)' + add_blank,\n", + " 'OSQP-qdldl_PARTIAL_CONDENSING' : 'OSQP\\n (qdldl)\\n(sparse)' + add_blank,\n", + " 'OSQP-mkl pardiso_PARTIAL_CONDENSING' : 'OSQP\\n (mkl)\\n(sparse)' + add_blank,\n", " 'HPIPM-SPEED_PARTIAL_CONDENSING' : 'HPIPM (speed)\\n(sparse)' + add_blank,\n", " 'HPIPM-SPEED_PARTIAL_CONDENSING_H' : 'HPIPM (speed)\\n(half dense)' + add_blank,\n", " 'HPIPM-SPEED_ABS_PARTIAL_CONDENSING' : 'HPIPM\\nspeed_abs\\n(sparse)' + add_blank,\n", @@ -898,14 +962,69 @@ "mpc_hist_n20.show()\n", "\n", "mpc_hist_n10.save('mpc_hist_n10.pdf')\n", - "mpc_hist_n20.save('mpc_hist_n20.pdf')\n", + "mpc_hist_n20.save('mpc_hist_n20.pdf')\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Create latex table\n", + "filtered_data = data_mpc.loc[\n", + " (\n", + " ((data_mpc['N'] == \"N10\") & (data_mpc[\"cond_N\"] == 10)) |\n", + " ((data_mpc['N'] == \"N20\") & (data_mpc[\"cond_N\"] == 20)) |\n", + " (data_mpc[\"cond_type\"] == \"FULL_CONDENSING\"))\n", + " \n", + " \n", + " &(~(data_mpc['mpc_solver'].str.contains('HPIPM')) | ((data_mpc[\"cond_type\"] == \"PARTIAL_CONDENSING\") & \n", + " ( data_mpc['mpc_solver'].str.contains(\"BALANCE\") | data_mpc['mpc_solver'].str.contains(\"SPEED_ABS\")) )\n", + " )\n", + " \n", + " & (~(data_mpc['mpc_solver'].str.contains('OSQP')) | (data_mpc['mpc_solver'].str.contains('qdldl')))\n", + " \n", + " & (data_mpc['experiment'].str.contains('trot'))\n", + " \n", + " \n", + "]\n", + "\n", + "filtered_data['mpc_solver_cond'] = filtered_data['mpc_solver'] + \"_\" + filtered_data['cond_type']\n", + "\n", + "\n", + "pd.set_option('display.float_format', lambda x: '%.1f' % x)\n", + "\n", + "filtered_data.groupby([\"N\", \"target_pc\", \"mpc_solver_cond\"])[\"mpc_sfpw\"].describe(percentiles=[0.05,0.95]).reset_index()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Create latex table\n", + "filtered_data = data_wbc.loc[\n", + " \n", + " (data_wbc['experiment_tag'].str.contains('wbc')) &\n", + " (data_wbc['experiment'].str.contains('trot'))\n", + " \n", + " \n", + "]\n", + "\n", + "pd.set_option('display.float_format', lambda x: '%.f' % x)\n", + "\n", + "filtered_data.groupby([\"target_pc\", \"wbc_solver\", \"wbc_scene\"])[\"wbc_sfpw\"].describe(percentiles=[0.05,0.95]).reset_index()\n", + "\n", + "\n", "\n" ] } ], "metadata": { "kernelspec": { - "display_name": "Python 3 (ipykernel)", + "display_name": "usr", "language": "python", "name": "python3" }, diff --git a/ws/src/solver_experiments/evaluaion_scripts/process_rosbags_in_directory.py b/ws/src/solver_experiments/evaluaion_scripts/process_rosbags_in_directory.py index 42e7319..70cf049 100644 --- a/ws/src/solver_experiments/evaluaion_scripts/process_rosbags_in_directory.py +++ b/ws/src/solver_experiments/evaluaion_scripts/process_rosbags_in_directory.py @@ -55,6 +55,8 @@ def process_rosbags_in_directory( "acados_num_iter": [], "acados_t_computed": [], "acados_return": [], + "qp_objective_value" :[], + "qp_residuals": [] }, "cpu_power_consumption": {"data": []}, } @@ -102,7 +104,7 @@ def process_rosbags_in_directory( result_data_out[key].extend(data_len * [tag_data[key]]) except Exception as e: - # raise e + #raise e print(f"!!!!!!FAILURE IN Bag: {e}")