PyBullet CartPole and Quadrotor environments—with CasADi symbolic a priori dynamics—for learning-based control and reinforcement learning

Last update: Dec 28, 2022

Overview

safe-control-gym

Physics-based CartPole and Quadrotor Gym environments (using PyBullet) with symbolic a priori dynamics (using CasADi) for learning-based control, and model-free and model-based reinforcement learning (RL).

These environments include (and evaluate) symbolic safety constraints and implement input, parameter, and dynamics disturbances to test the robustness and generalizability of control approaches.

Branch ar (or release v0.5.0) are the codebase for our review article on safe control and RL:

@article{brunke2021safe,
  title={Safe Learning in Robotics: From Learning-Based Control to Safe Reinforcement Learning}, 
  author={Lukas Brunke and Melissa Greeff and Adam W. Hall and Zhaocong Yuan and Siqi Zhou and Jacopo Panerati and Angela P. Schoellig},
  year={2021},
  journal = {Annual Review of Control, Robotics, and Autonomous Systems},
  eprint = {2108.06266},
  url = {https://arxiv.org/abs/2108.06266}
}

To stay in touch, get involved or ask questions, please contact us via e-mail ({jacopo.panerati, zhaocong.yuan, adam.hall, siqi.zhou, lukas.brunke, melissa.greeff}@robotics.utias.utoronto.ca) or through this form.

Architecture

Overview of safe-control-gym's API:

Install on Ubuntu/macOS

(optional) Create and access a Python 3.7 environment using conda

$ conda create -n safe python=3.7                                  # Create environment (named 'safe' here)
$ conda activate safe                                              # Activate environment 'safe'

Clone and install the safe-control-gym repository

$ git clone -b ar https://github.com/utiasDSL/safe-control-gym.git # Clone repository (the 'ar' branch specifically)
$ cd safe-control-gym                                              # Enter the repository
$ pip install -e .                                                 # Install the repository

2D Quadrotor Lemniscate Trajectory Tracking

Re-create the Results in "Safe Learning in Robotics" [arXiv link]

Figure 6—Robust GP-MPC [1]

$ cd ./experiments/figure6/                                        # Navigate to the experiment folder
$ chmod +x create_fig6.sh                                          # Make the script executable, if needed
$ ./create_fig6.sh                                                 # Run the script (ca. 2')

This will use the models in safe-control-gym/experiments/figure6/trained_gp_model/ to generate

To also re-train the GP models from scratch (ca. 30' on a laptop)

$ chmod +x create_trained_gp_model.sh                              # Make the script executable, if needed
$ ./create_trained_gp_model.sh                                     # Run the script (ca. 30')

Note: this will backup and overwrite safe-control-gym/experiments/figure6/trained_gp_model/

Figure 7—Safe RL Exploration [2]

$ cd ../figure7/                                                   # Navigate to the experiment folder
$ chmod +x create_fig7.sh                                          # Make the script executable, if needed
$ ./create_fig7.sh                                                 # Run the script (ca. 5'')

This will use the data in safe-control-gym/experiments/figure7/safe_exp_results.zip/ to generate

To also re-train all the controllers/agents (warning: >24hrs on a laptop, if necessary, run each one of the loops in the Bash script—PPO, PPO with reward shaping, and the Safe Explorer—separately)

$ chmod +x create_safe_exp_results.sh                              # Make the script executable, if needed
$ ./create_safe_exp_results.sh                                     # Run the script (>24hrs)

Note: this script will (over)write the results in safe-control-gym/experiments/figure7/safe_exp_results/; if you do not run the re-training to completion, delete the partial results rm -r -f ./safe_exp_results/ before running ./create_fig7.sh again.

Figure 8—Model Predictive Safety Certification [3]

(required) Obtain MOSEK's license (free for academia). Once you have received (via e-mail) and downloaded the license to your own ~/Downloads folder, install it by executing

$ mkdir ~/mosek                                                    # Create MOSEK license folder in your home '~'
$ mv ~/Downloads/mosek.lic ~/mosek/                                # Copy the downloaded MOSEK license to '~/mosek/'

Then run

$ cd ../figure8/                                                   # Navigate to the experiment folder
$ chmod +x create_fig8.sh                                          # Make the script executable, if needed
$ ./create_fig8.sh                                                 # Run the script (ca. 1')

This will use the unsafe (pre-trained) PPO controller/agent in folder safe-control-gym/experiments/figure8/unsafe_ppo_model/ to generate

To also re-train the unsafe PPO controller/agent (ca. 2' on a laptop)

$ chmod +x create_unsafe_ppo_model.sh                              # Make the script executable, if needed
$ ./create_unsafe_ppo_model.sh                                     # Run the script (ca. 2')

Note: this script will (over)write the model in safe-control-gym/experiments/figure8/unsafe_ppo_model/

References

[1] Hewing L, Kabzan J, Zeilinger MN. 2020. Cautious model predictive control using Gaussian process regression. IEEE Transactions on Control Systems Technology 28:2736–2743
[2] Dalal G, Dvijotham K, Vecerik M, Hester T, Paduraru C, Tassa Y. 2018. Safe exploration in continuous action spaces. arXiv:1801.08757 [cs.AI]
[3] Wabersich KP, Zeilinger MN. 2018. Linear Model Predictive Safety Certification for Learning-Based Control. In 2018 IEEE Conference on Decision and Control (CDC), pp. 7130–7135

University of Toronto's Dynamic Systems Lab / Vector Institute for Artificial Intelligence

Comments

Re-create Figure 6 in "Safe Learning in Robotics" errors

Hey Jacopo,

Thanks for the great review paper and open sourcing the accompanying codes here.

I would like to reproduce Figure 6 by following commands but got error messages after successfully loading five GP models as

#########################################
#       Loading GP dimension 5         #
#########################################
Path: ./trained_gp_model/best_model_5.pth
Loaded!
Traceback (most recent call last):
  File "./gp_mpc_experiment.py", line 179, in <module>
    ctrl.learn()
  File "/home/jwang/Control_ws/src/safe-control-gym/safe_control_gym/controllers/mpc/gp_mpc.py", line 809, in learn
    self.reset()
  File "/home/jwang/Control_ws/src/safe-control-gym/safe_control_gym/controllers/mpc/gp_mpc.py", line 899, in reset
    self.setup_gp_optimizer(n_ind_points)
  File "/home/jwang/Control_ws/src/safe-control-gym/safe_control_gym/controllers/mpc/gp_mpc.py", line 562, in setup_gp_optimizer
    mean_post_factor_val, Sigma, K_zind_zind_inv, z_ind_val = self.precompute_sparse_gp_values(n_ind_points)
  File "/home/jwang/Control_ws/src/safe-control-gym/safe_control_gym/controllers/mpc/gp_mpc.py", line 394, in precompute_sparse_gp_values
    inds, dist_mat = pairwise_distances_argmin_min(centroids, inputs[:, self.input_mask])
  File "/home/jwang/anaconda3/envs/safe/lib/python3.8/site-packages/sklearn/metrics/pairwise.py", line 680, in pairwise_distances_argmin_min
    values, indices = PairwiseDistancesArgKmin.compute(
  File "sklearn/metrics/_pairwise_distances_reduction.pyx", line 672, in sklearn.metrics._pairwise_distances_reduction.PairwiseDistancesArgKmin.compute
  File "sklearn/metrics/_pairwise_distances_reduction.pyx", line 1055, in sklearn.metrics._pairwise_distances_reduction.FastEuclideanPairwiseDistancesArgKmin.__init__
  File "sklearn/metrics/_dist_metrics.pyx", line 1300, in sklearn.metrics._dist_metrics.DatasetsPair.get_for
  File "sklearn/metrics/_dist_metrics.pyx", line 1349, in sklearn.metrics._dist_metrics.DenseDenseDatasetsPair.__init__
  File "stringsource", line 658, in View.MemoryView.memoryview_cwrapper
  File "stringsource", line 349, in View.MemoryView.memoryview.__cinit__
ValueError: ndarray is not C-contiguous

I wonder if you could help with this? Thank you.

Regards, Jie

bug

opened by jwangjie 15

Fixing LQR stabilization

Fixed small error in LQR code and changed the test to be more interesting. Essentially the problem stems from the fact we changed what X_EQ is in quadrotor, and so I'm unsure if it would cause errors in other places of the code. In quadrotor, it was always stabilizing to (0, 0) instead of the desired goal point since it expected X_EQ to hold the stabilization goal but it now is only an array of 0s.

The code in cartpole does what I believe used to be done, setting X_EQ to be the first X_GOAL: https://github.com/utiasDSL/safe-control-gym/blob/bfe74b3327802d7578217ca1f42ddbdd49d4bf94/safe_control_gym/envs/gym_control/cartpole.py#L423

However, in quadrotor, this is not done (it is set to 0s instead): https://github.com/utiasDSL/safe-control-gym/blob/bfe74b3327802d7578217ca1f42ddbdd49d4bf94/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py#L562

I think either of these approaches is fine, but we should decide since this is inconsistent and is causing problems.
bug

opened by Federico-PizarroBejarano 7
Trying To understand Tracking.py

Thanks a lot for developing this awesome repo. I am new to programming and am trying to learn about how the control algorithm is being executed here. My first question is:- https://github.com/utiasDSL/safe-control-gym/blob/f5a6b16b05cfa7fa5fa21da59c06c4c905b4cb75/safe_control_gym/utils/registration.py#L159 whenever I pass the argument --algo lqr --task traj_tracking in the command line, I get key error for traj_tracking not for lqr. From what I understand the key error should also raise for lqr since the registry.specs is an empty dictionary and key lqr would not be found in it. Please help me understand this.

My second question is:- when I type this in the cmd line $python3 tracking.py --overrides ./tracking.yaml
this line :- https://github.com/utiasDSL/safe-control-gym/blob/main/examples/tracking.py#L44 should give an key error right? As the registry.specs is empty. Also in this line:- https://github.com/utiasDSL/safe-control-gym/blob/main/examples/tracking.py#L48 ctrl does not have any instance variable reference.

Could someone please provide me an explanation.
question

opened by waranyoghes 7

How do I use LinearMPC/MPC with LinearConstraint?

TLDR

TLDR: When I use LinearMPC with LinearConstraint, it seems like self.sym_func = lambda x: self.A @ self.constraint_filter @ x - self.b wouldn't work with A and b being ndarray whereas during initialization, x is still MX symbolic variable.

Context

Hello, I'm trying to test MPC or LinearMPC for my environment (states: [theta, theta_dot])

I am defining my constraints as such in my environment yaml file:

constraints:
  - constraint_form: bounded_constraint
    lower_bounds: [0] # should match state dim
    upper_bounds: [2.6] 
    constrained_variable: state
    active_dims: [0]  # only position

Then I define the control agent using the recommended make procedure as such (for linear_mpc or mpc):

from safe_control_gym.utils.registration import make
env_func = partial(make, config.task, output_dir=config.output_dir, **config.task_config)
control_agent = make(config.algo,
                    env_func,
                    training=True,
                    checkpoint_path=os.path.join(config.output_dir, "model_latest_manippulator.pt"),
                    output_dir=config.output_dir,
                    device=config.device,
                    seed=config.seed,
                    **config.algo_config)

Then when initializing with

control_agent.reset()

Error

I get the following error:

---------------------------------------------------------------------------
TypeError                                 Traceback (most recent call last)
Cell In [9], line 1
----> 1 control_agent.reset()

File ~/code/rm-sc/safe_control_gym/controllers/mpc/mpc.py:128, in MPC.reset(self)
    126 self.set_dynamics_func()
    127 # CasADi optimizer.
--> 128 self.setup_optimizer()
    129 # Previously solved states & inputs, useful for warm start.
    130 self.x_prev = None

File ~/code/rm-sc/safe_control_gym/controllers/mpc/linear_mpc.py:130, in LinearMPC.setup_optimizer(self)
    127     print(state_constraint)
    128     print(x_var[:,i])
--> 130     opti.subject_to(state_constraint(x_var[:,i] + self.X_LIN.T) < 0)
    131 for input_constraint in self.input_constraints_sym:
    132     opti.subject_to(input_constraint(u_var[:,i] + self.U_LIN.T) < 0)

File ~/code/rm-sc/safe_control_gym/envs/constraints.py:260, in LinearConstraint.__init__.<locals>.<lambda>(x)
    258 self.num_constraints = A.shape[0]
    259 print(self.constraint_filter)
--> 260 self.sym_func = lambda x: self.A @ self.constraint_filter @ x - self.b
    261 self.check_tolerance_shape()

TypeError: operand type(s) all returned NotImplemented from __array_ufunc__(<ufunc 'matmul'>, '__call__', array([[-1.],
       [ 1.]]), MX(([2.5, 0]+opti1_x_1[:2]))): 'ndarray', 'MX'

Hypothesis

It seems like this happens because under LinearConstraint, A and b are set to ndarray by default, but under MPC, at state_constraint(x_var[:,i] + self.X_LIN.T), x_var = opti.variable(nx, T + 1) which is a symbolic variable. My current understanding is if this eventually goes to the part where the controller runs and passes in the x array, it would work, but currently stuck during initialization seemingly due to mismatch between array and symbolic var.

I also tested the functionality with a dummy code as such and it would work with x as array:

lower_bounds = np.array( [0.2])
upper_bounds = np.array([5])

dim = lower_bounds.shape[0]

A = np.vstack((-np.eye(dim), np.eye(dim)))
b = np.hstack((-lower_bounds, upper_bounds))

active_dims = [0]
dim_state = 2
constraint_filter = np.eye(dim_state)[active_dims]
x = np.array([3., 5]) 

print([email protected][email protected])

which outputs array([-3., 3.]), whereas if i do

opti = cs.Opti()
nx = 1 
T = 5
x_var = opti.variable(nx, T + 1)

print([email protected][email protected])

A similar error is thrown.

TLDR

Question

May I know how MPC class has been used in the past? Perhaps I'm missing some context here (especially on the casadi side), would love some explanation/clarity on how I can work with the constraints.

Thanks a lot!

Tagging @adamhall @JacopoPan

question

opened by nicholasprayogo 6

Failed to run create_fig6.sh
Hi, I try to run create_fig6.sh, but it failed. Could you help me with this problem? Thanks for your help!

error

pybullet build time: May 20 2022 19:44:17 UserWarning: WARN: Box bound precision lowered by casting to float32 logger.warn( Traceback (most recent call last): File "./gp_mpc_experiment.py", line 197, in plot_xz_comparison_diag_constraint(prior_run, run, 1) File "./gp_mpc_experiment.py", line 27, in plot_xz_comparison_diag_constraint ax = plot_2D_comparison_with_prior(state_inds, prior_run, run, goal, init_ind, dir=dir) File "./gp_mpc_experiment.py", line 53, in plot_2D_comparison_with_prior horizon_cov = run.state_horizon_cov[init_ind] IndexError: list index out of range

info

@1=vertcat(x, x_dot, z, z_dot, theta, theta_dot), @2=vertcat(T1, T2), (mac(mac((0.5*(@1-Xr)'),Q,zeros(1x6)),(@1-Xr),0)+mac(mac((0.5*(@2-Ur)'),R,zeros(1x2)),(@2-Ur),0)) @1=vertcat(x, x_dot, z, z_dot, theta, theta_dot), @2=vertcat(T1, T2), (mac(mac((0.5*(@1-Xr)'),Q,zeros(1x6)),(@1-Xr),0)+mac(mac((0.5*(@2-Ur)'),R,zeros(1x2)),(@2-Ur),0)) @1=vertcat(x, x_dot, z, z_dot, theta, theta_dot), @2=vertcat(T1, T2), (mac(mac((0.5*(@1-Xr)'),Q,zeros(1x6)),(@1-Xr),0)+mac(mac((0.5*(@2-Ur)'),R,zeros(1x2)),(@2-Ur),0)) @1=vertcat(x, x_dot, z, z_dot, theta, theta_dot), @2=vertcat(T1, T2), (mac(mac((0.5*(@1-Xr)'),Q,zeros(1x6)),(@1-Xr),0)+mac(mac((0.5*(@2-Ur)'),R,zeros(1x2)),(@2-Ur),0)) @1=vertcat(x, x_dot, z, z_dot, theta, theta_dot), @2=vertcat(T1, T2), (mac(mac((0.5*(@1-Xr)'),Q,zeros(1x6)),(@1-Xr),0)+mac(mac((0.5*(@2-Ur)'),R,zeros(1x2)),(@2-Ur),0)) Init State: [-1. 0. 0. 0. -0. 0.] iter objective inf_pr inf_du ||d|| lg(rg) ls 0 5.504332e+01 1.00e+00 1.00e+01 0.00e+00 - 0

#################### qpOASES -- QP NO. 1 #####################

Iter | StepLength | Info | nFX | nAC

----------+------------------+------------------+---------+--------- 0 | 6.164476e-08 | REM BND 62 | 85 | 0
1 | 2.208861e-11 | REM BND 60 | 84 | 0
2 | 4.431082e-11 | REM BND 56 | 83 | 0
3 | 2.233967e-11 | REM BND 54 | 82 | 0
4 | 4.481644e-11 | REM BND 50 | 81 | 0
5 | 2.259504e-11 | REM BND 48 | 80 | 0
6 | 4.533077e-11 | REM BND 44 | 79 | 0
7 | 2.285482e-11 | REM BND 42 | 78 | 0
8 | 4.585399e-11 | REM BND 38 | 77 | 0
9 | 2.311911e-11 | REM BND 36 | 76 | 0
10 | 4.638633e-11 | REM BND 32 | 75 | 0
11 | 2.338800e-11 | REM BND 30 | 74 | 0
12 | 4.692799e-11 | REM BND 26 | 73 | 0
13 | 2.366162e-11 | REM BND 24 | 72 | 0
14 | 4.747919e-11 | REM BND 20 | 71 | 0
15 | 2.394007e-11 | REM BND 18 | 70 | 0
16 | 4.804016e-11 | REM BND 14 | 69 | 0
17 | 2.422347e-11 | REM BND 12 | 68 | 0
18 | 4.861113e-11 | REM BND 8 | 67 | 0
19 | 2.451194e-11 | REM BND 6 | 66 | 0
20 | 4.919233e-11 | REM BND 2 | 65 | 0
21 | 2.480558e-11 | REM BND 0 | 64 | 0
22 | 9.365151e-09 | ADD CON 3 | 63 | 1
23 | 1.295850e-09 | ADD CON 5 | 62 | 2
24 | 7.426724e-10 | ADD CON 26 | 61 | 3
25 | 1.332632e-09 | ADD CON 28 | 60 | 4
26 | 6.982862e-10 | ADD CON 49 | 59 | 5
27 | 1.369280e-09 | ADD CON 51 | 58 | 6
28 | 6.540761e-10 | ADD CON 72 | 57 | 7
29 | 1.405795e-09 | ADD CON 74 | 56 | 8
30 | 6.100412e-10 | ADD CON 95 | 55 | 9
31 | 1.442178e-09 | ADD CON 97 | 54 | 10
32 | 5.661805e-10 | ADD CON 118 | 53 | 11
33 | 1.478429e-09 | ADD CON 120 | 52 | 12
34 | 5.224929e-10 | ADD CON 141 | 51 | 13
35 | 1.514549e-09 | ADD CON 143 | 50 | 14
36 | 4.789775e-10 | ADD CON 164 | 49 | 15
37 | 1.550540e-09 | ADD CON 166 | 48 | 16
38 | 4.356331e-10 | ADD CON 187 | 47 | 17
39 | 1.586400e-09 | ADD CON 189 | 46 | 18
40 | 3.924588e-10 | ADD CON 210 | 45 | 19
41 | 1.622132e-09 | ADD CON 212 | 44 | 20
42 | 2.688693e-07 | ADD CON 1 | 44 | 20
43 | 9.136291e-10 | REM BND 3 | 43 | 20
44 | 7.837492e-09 | ADD CON 24 | 43 | 20
45 | 3.278552e-09 | ADD CON 5 | 42 | 21
46 | 5.253162e-09 | REM BND 4 | 41 | 21
47 | 1.157731e-10 | ADD CON 47 | 41 | 21
48 | 3.425317e-09 | ADD CON 10 | 41 | 22
49 | 2.471019e-10 | ADD CON 28 | 40 | 23
50 | 4.873264e-09 | ADD CON 70 | 40 | 23
51 | 4.061019e-09 | ADD CON 51 | 39 | 24
52 | 4.384648e-09 | ADD CON 93 | 39 | 24
53 | 4.444456e-09 | ADD CON 74 | 38 | 25
54 | 3.902940e-09 | ADD CON 116 | 38 | 25
55 | 4.822829e-09 | ADD CON 97 | 37 | 26
56 | 3.428001e-09 | ADD CON 139 | 37 | 26
57 | 5.196239e-09 | ADD CON 120 | 36 | 27
58 | 2.959692e-09 | ADD CON 162 | 36 | 27
59 | 5.564779e-09 | ADD CON 143 | 35 | 28
60 | 2.497879e-09 | ADD CON 185 | 35 | 28
61 | 5.928545e-09 | ADD CON 166 | 34 | 29
62 | 2.042433e-09 | ADD CON 208 | 34 | 29
63 | 6.287626e-09 | ADD CON 189 | 33 | 30
64 | 8.235337e-09 | ADD CON 212 | 32 | 31
65 | 1.275344e-08 | ADD CON 234 | 31 | 32
66 | 3.401954e-08 | ADD CON 248 | 30 | 33
67 | 6.054781e-09 | REM BND 7 | 29 | 33
68 | 1.945595e-08 | ADD CON 33 | 29 | 34
69 | 1.874176e-11 | ADD CON 244 | 28 | 35
70 | 1.418504e-08 | ADD CON 56 | 28 | 35
71 | 1.368721e-08 | ADD CON 5 | 28 | 35
72 | 7.644667e-09 | REM BND 19 | 27 | 35
73 | 2.055939e-08 | ADD CON 79 | 27 | 36
74 | 9.512611e-09 | REM BND 25 | 26 | 36
75 | 2.006088e-08 | REM BND 59 | 25 | 36
76 | 2.039106e-10 | REM BND 53 | 24 | 36
77 | 2.061283e-10 | REM BND 47 | 23 | 36
78 | 2.084984e-10 | REM BND 41 | 22 | 36
79 | 4.756914e-10 | REM BND 35 | 21 | 36
80 | 7.405209e-12 | ADD CON 102 | 21 | 37
81 | 7.049732e-10 | REM BND 31 | 20 | 37
82 | 7.529600e-10 | REM BND 29 | 19 | 37
83 | 2.263203e-09 | REM BND 23 | 18 | 37
84 | 4.633543e-10 | REM BND 65 | 17 | 37
85 | 2.631833e-09 | REM BND 17 | 16 | 37
86 | 8.347436e-10 | REM BND 57 | 15 | 37
87 | 6.632452e-10 | REM BND 51 | 14 | 37
88 | 6.761337e-10 | REM BND 45 | 13 | 37
89 | 6.840066e-10 | REM BND 39 | 12 | 37
90 | 6.919302e-10 | REM BND 33 | 11 | 37
91 | 2.228695e-10 | REM BND 37 | 10 | 37
92 | 1.696806e-10 | REM BND 11 | 9 | 37
93 | 3.074410e-10 | REM BND 27 | 8 | 37
94 | 6.170530e-11 | REM BND 9 | 7 | 37
95 | 6.464885e-10 | REM BND 21 | 6 | 37
96 | 7.056954e-10 | REM BND 15 | 5 | 37
97 | 2.602165e-09 | REM BND 43 | 4 | 37
98 | 1.368755e-09 | REM BND 49 | 3 | 37
99 | 3.208089e-11 | REM BND 55 | 2 | 37
100 | 2.300021e-08 | ADD CON 217 | 2 | 38
101 | 1.430245e-08 | ADD CON 194 | 2 | 39
102 | 1.323568e-08 | ADD CON 171 | 2 | 40
103 | 9.688421e-09 | REM BND 61 | 1 | 40
104 | 1.785168e-07 | REM BND 63 | 0 | 40
105 | 3.645625e-03 | REM CON 102 | 0 | 39
106 | 1.325825e-03 | REM CON 171 | 0 | 38
107 | 3.210844e-03 | ADD CON 1 | 0 | 39
108 | 2.274270e-06 | REM CON 5 | 0 | 38
109 | 4.665971e-03 | ADD CON 5 | 0 | 39
110 | 1.806146e-04 | REM CON 79 | 0 | 38
111 | 5.432293e-03 | REM CON 194 | 0 | 37
112 | 2.756412e-03 | REM CON 56 | 0 | 36
113 | 1.435235e-02 | REM CON 244 | 0 | 35
114 | 3.873458e-05 | REM CON 33 | 0 | 34
115 | 1.585118e-02 | REM CON 217 | 0 | 33
116 | 3.384988e-02 | REM CON 10 | 0 | 32
117 | 6.457739e-02 | REM CON 234 | 0 | 31
118 | 3.665566e-01 | ADD CON 243 | 0 | 32
119 | 8.930384e-04 | ADD CON 245 | 0 | 33
120 | 1.990731e-01 | ADD CON 0 | 0 | 34
121 | 2.796725e-03 | ADD CON 2 | 0 | 35
122 | 1.556592e-01 | REM CON 1 | 0 | 34
123 | 2.386773e-02 | ADD CON 23 | 0 | 35
124 | 1.293852e-03 | ADD CON 25 | 0 | 36
125 | 6.727666e-02 | REM CON 3 | 0 | 35
126 | 2.210024e-02 | ADD CON 3 | 0 | 36
127 | 7.405092e-02 | ADD CON 1 | 0 | 37
128 | 6.743344e-04 | REM CON 24 | 0 | 36
129 | 3.415928e-02 | ADD CON 46 | 0 | 37
130 | 6.557692e-03 | ADD CON 48 | 0 | 38
131 | 4.429775e-02 | ADD CON 244 | 0 | 39
132 | 2.216825e-02 | REM CON 26 | 0 | 38
133 | 2.047346e-02 | ADD CON 26 | 0 | 39
134 | 4.311502e-02 | ADD CON 24 | 0 | 40
135 | 1.421877e-03 | ADD CON 246 | 0 | 41
136 | 4.428786e-03 | ADD CON 4 | 0 | 42
137 | 9.312933e-04 | REM CON 47 | 0 | 41
138 | 4.518884e-02 | ADD CON 69 | 0 | 42
139 | 2.702209e-02 | ADD CON 71 | 0 | 43
140 | 3.123203e-02 | REM CON 5 | 0 | 42
141 | 1.600407e-02 | ADD CON 247 | 0 | 43
142 | 2.728007e-03 | REM CON 1 | 0 | 42
143 | 1.208242e-02 | ADD CON 5 | 0 | 43
144 | 9.654815e-04 | ADD CON 47 | 0 | 44
145 | 2.614917e-03 | REM CON 49 | 0 | 43
146 | 1.949132e-02 | ADD CON 49 | 0 | 44
147 | 2.625171e-04 | REM CON 70 | 0 | 43
148 | 3.235717e-02 | ADD CON 27 | 0 | 44
149 | 2.076213e-02 | REM CON 28 | 0 | 43
150 | 2.160900e-02 | ADD CON 22 | 0 | 44
151 | 2.517275e-03 | ADD CON 92 | 0 | 45
152 | 8.208921e-03 | REM CON 51 | 0 | 44
153 | 4.518666e-03 | ADD CON 51 | 0 | 45
154 | 1.956731e-03 | REM CON 27 | 0 | 44
155 | 1.163759e-02 | ADD CON 28 | 0 | 45
156 | 1.573853e-02 | ADD CON 94 | 0 | 46
157 | 2.863122e-03 | ADD CON 1 | 0 | 47
158 | 1.010808e-03 | REM CON 28 | 0 | 46
159 | 2.057084e-03 | ADD CON 28 | 0 | 47
160 | 3.943013e-03 | REM CON 4 | 0 | 46
161 | 7.831652e-03 | ADD CON 27 | 0 | 47
162 | 2.751723e-03 | ADD CON 70 | 0 | 48
163 | 7.650100e-03 | REM CON 28 | 0 | 47
164 | 1.043970e-02 | ADD CON 28 | 0 | 48
165 | 7.271739e-03 | REM CON 93 | 0 | 47
166 | 8.297252e-03 | REM CON 72 | 0 | 46
167 | 1.656183e-02 | ADD CON 72 | 0 | 47
168 | 1.068573e-02 | REM CON 74 | 0 | 46
169 | 1.245084e-03 | REM CON 248 | 0 | 45
170 | 1.707998e-03 | ADD CON 74 | 0 | 46
171 | 4.896335e-02 | ADD CON 50 | 0 | 47
172 | 2.980662e-04 | ADD CON 115 | 0 | 48
173 | 3.021797e-02 | ADD CON 93 | 0 | 49
174 | 2.118919e-02 | ADD CON 117 | 0 | 50
175 | 5.656994e-03 | REM CON 97 | 0 | 49
176 | 1.682961e-03 | ADD CON 97 | 0 | 50
177 | 1.838983e-03 | REM CON 116 | 0 | 49
178 | 3.675879e-02 | REM CON 3 | 0 | 48
179 | 1.454542e-02 | REM CON 95 | 0 | 47
180 | 1.392441e-02 | ADD CON 73 | 0 | 48
181 | 1.629745e-03 | ADD CON 4 | 0 | 49
182 | 3.231978e-03 | ADD CON 95 | 0 | 50
183 | 1.109460e-03 | REM CON 24 | 0 | 49
184 | 4.744743e-02 | ADD CON 116 | 0 | 50
185 | 1.509485e-03 | ADD CON 21 | 0 | 51
186 | 2.168479e-03 | ADD CON 138 | 0 | 52
187 | 1.726914e-02 | REM CON 120 | 0 | 51
188 | 1.904547e-03 | ADD CON 120 | 0 | 52
189 | 1.394208e-02 | ADD CON 3 | 0 | 53
190 | 4.583378e-03 | ADD CON 44 | 0 | 54
191 | 9.410758e-04 | REM CON 139 | 0 | 53
192 | 1.928623e-02 | ADD CON 24 | 0 | 54
193 | 1.353634e-02 | ADD CON 140 | 0 | 55
194 | 9.828789e-03 | REM CON 28 | 0 | 54
195 | 1.317691e-02 | ADD CON 28 | 0 | 55
196 | 2.719782e-02 | REM CON 118 | 0 | 54
197 | 1.225778e-02 | REM CON 27 | 0 | 53
198 | 2.694436e-03 | ADD CON 118 | 0 | 54
199 | 1.948384e-02 | ADD CON 139 | 0 | 55
200 | 5.339655e-03 | ADD CON 161 | 0 | 56
201 | 1.664699e-02 | REM CON 143 | 0 | 55
202 | 1.740547e-03 | ADD CON 143 | 0 | 56
203 | 3.720334e-03 | ADD CON 96 | 0 | 57
204 | 8.161798e-03 | REM CON 162 | 0 | 56
205 | 2.804793e-05 | ADD CON 248 | 0 | 57
206 | 2.923284e-02 | REM CON 21 | 0 | 56
207 | 2.706891e-02 | ADD CON 163 | 0 | 57
208 | 3.023468e-02 | REM CON 47 | 0 | 56
209 | 2.624891e-04 | ADD CON 142 | 0 | 57
210 | 2.044818e-02 | ADD CON 162 | 0 | 58
211 | 4.625494e-03 | ADD CON 67 | 0 | 59
212 | 1.653917e-03 | REM CON 141 | 0 | 58
213 | 1.275297e-02 | ADD CON 141 | 0 | 59
214 | 2.127710e-03 | REM CON 166 | 0 | 58
215 | 1.410490e-03 | REM CON 26 | 0 | 57
216 | 1.076089e-03 | ADD CON 166 | 0 | 58
217 | 2.277180e-02 | ADD CON 184 | 0 | 59
218 | 1.002708e-02 | ADD CON 119 | 0 | 60
219 | 2.602664e-04 | REM CON 185 | 0 | 59
220 | 4.358732e-03 | ADD CON 47 | 0 | 60
221 | 1.057640e-02 | REM CON 142 | 0 | 59
222 | 8.234371e-03 | ADD CON 26 | 0 | 60
223 | 3.578679e-02 | ADD CON 165 | 0 | 61
224 | 1.482147e-02 | ADD CON 186 | 0 | 62
225 | 3.112816e-02 | ADD CON 185 | 0 | 63
226 | 1.257534e-02 | REM CON 189 | 0 | 62
227 | 2.139787e-03 | ADD CON 189 | 0 | 63
228 | 4.475155e-03 | REM CON 164 | 0 | 62
229 | 1.431492e-02 | ADD CON 164 | 0 | 63
230 | 8.729248e-03 | REM CON 212 | 0 | 62
231 | 4.190827e-03 | ADD CON 212 | 0 | 63
232 | 2.034285e-05 | REM CON 208 | 0 | 62
233 | 2.350469e-03 | ADD CON 142 | 0 | 63
234 | 7.640250e-03 | ADD CON 27 | 0 | 64
235 | 2.139692e-02 | REM CON 165 | 0 | 63
236 | 1.213446e-02 | ADD CON 188 | 0 | 64
237 | 3.237549e-03 | ADD CON 207 | 0 | 65
238 | 7.175201e-05 | REM CON 212 | 0 | 64
239 | 5.228848e-04 | ADD CON 212 | 0 | 65
240 | 2.850372e-02 | ADD CON 90 | 0 | 66
241 | 1.050228e-02 | ADD CON 209 | 0 | 67
242 | 1.963842e-02 | REM CON 49 | 0 | 66
243 | 5.686352e-03 | ADD CON 208 | 0 | 67
244 | 9.536259e-03 | REM CON 212 | 0 | 66
245 | 2.223390e-03 | ADD CON 212 | 0 | 67
246 | 2.255394e-02 | REM CON 187 | 0 | 66
247 | 2.337256e-03 | ADD CON 49 | 0 | 67
248 | 6.116239e-03 | ADD CON 211 | 0 | 68
249 | 4.492136e-03 | ADD CON 187 | 0 | 69
250 | 7.103404e-03 | REM CON 212 | 0 | 68
251 | 8.166099e-04 | REM CON 51 | 0 | 67
252 | 9.079063e-03 | ADD CON 212 | 0 | 68
253 | 4.580307e-03 | ADD CON 51 | 0 | 69
254 | 1.725081e-02 | ADD CON 165 | 0 | 70
255 | 1.899687e-02 | REM CON 212 | 0 | 69
256 | 1.156882e-02 | REM CON 70 | 0 | 68
257 | 5.545275e-03 | ADD CON 212 | 0 | 69
258 | 5.972036e-02 | ADD CON 70 | 0 | 70
259 | 5.822507e-06 | REM CON 188 | 0 | 69
260 | 2.286309e-02 | ADD CON 113 | 0 | 70
261 | 1.124471e-02 | REM CON 210 | 0 | 69
262 | 1.175204e-02 | ADD CON 210 | 0 | 70
263 | 1.112306e-02 | REM CON 212 | 0 | 69
264 | 5.567213e-03 | REM CON 72 | 0 | 68
265 | 1.438150e-02 | ADD CON 212 | 0 | 69
266 | 2.015333e-02 | ADD CON 72 | 0 | 70
267 | 7.296921e-02 | ADD CON 188 | 0 | 71
268 | 2.572062e-02 | REM CON 212 | 0 | 70
269 | 9.703857e-03 | ADD CON 212 | 0 | 71
270 | 3.954899e-02 | ADD CON 137 | 0 | 72
271 | 3.144703e-02 | ADD CON 136 | 0 | 73
272 | 1.407455e-02 | REM CON 95 | 0 | 72
273 | 2.932460e-02 | ADD CON 95 | 0 | 73
274 | 1.392843e-01 | ADD CON 160 | 0 | 74
275 | 5.081134e-02 | REM CON 118 | 0 | 73
276 | 2.616955e-02 | ADD CON 118 | 0 | 74
277 | 1.585479e-01 | ADD CON 183 | 0 | 75
278 | 5.625643e-02 | REM CON 141 | 0 | 74
279 | 1.659738e-02 | REM CON 74 | 0 | 73
280 | 8.647233e-03 | ADD CON 141 | 0 | 74
281 | 1.635669e-03 | ADD CON 74 | 0 | 75
282 | 2.306423e-01 | ADD CON 206 | 0 | 76
283 | 4.255722e-02 | ADD CON 159 | 0 | 77
284 | 1.398526e-02 | REM CON 164 | 0 | 76
285 | 2.496963e-02 | ADD CON 164 | 0 | 77
286 | 6.560276e-02 | REM CON 93 | 0 | 76
287 | 7.091343e-02 | ADD CON 93 | 0 | 77
288 | 2.770174e-01 | ADD CON 45 | 0 | 78
289 | 9.462614e-02 | REM CON 211 | 0 | 77
290 | 1.373792e-02 | REM CON 50 | 0 | 76
291 | 5.253172e-02 | REM CON 187 | 0 | 75
292 | 2.957858e-02 | ADD CON 187 | 0 | 76
293 | 1.129962e-01 | ADD CON 229 | 0 | 77
294 | 1.332607e-01 | ADD CON 50 | 0 | 78
295 | 4.814664e-02 | ADD CON 211 | 0 | 79
296 | 7.323369e-01 | REM CON 97 | 0 | 78
297 | 5.651252e-03 | ADD CON 97 | 0 | 79
298 | 1.000000e+00 | QP SOLVED | 0 | 79
1 9.681463e+01 6.66e-16 2.84e-14 1.25e+00 - 1 MESSAGE(sqpmethod): Convergence achieved after 1 iterations solver : t_proc (avg) t_wall (avg) n_eval QP | 61.97ms ( 61.97ms) 35.20ms ( 35.20ms) 1 linesearch | 15.00us ( 15.00us) 15.11us ( 15.11us) 1 nlp_fg | 9.00us ( 9.00us) 8.95us ( 8.95us) 1 nlp_grad | 15.00us ( 15.00us) 14.67us ( 14.67us) 1 nlp_hess_l | 23.00us ( 23.00us) 2.25us ( 2.25us) 1 nlp_jac_fg | 143.00us ( 71.50us) 22.93us ( 11.47us) 2 total | 62.53ms ( 62.53ms) 35.30ms ( 35.30ms) 1 0 -th step. [0.19143743 0.19999999] [-0.9990555 0.03623872 0.02441309 0.46774115 0.08898196 1.70557299] -11.318308612350087 False {'goal_reached': False, 'mse': 1.9498817124165235, 'constraint_values': array([-1.00094450e+00, -3.40282347e+38, -7.44130887e-02, -3.40282347e+38, -1.57251177e+00, -3.40282347e+38, -2.99905550e+00, -3.40282347e+38, -1.97558691e+00, -3.40282347e+38, -1.39454785e+00, -3.40282347e+38, -1.91437431e-01, -1.99999990e-01, -8.56256923e-03, -9.99999999e-09]), 'constraint_violation': 0}

iter objective inf_pr inf_du ||d|| lg(rg) ls 0 9.682634e+01 1.71e+00 1.01e+01 0.00e+00 - 0

#################### qpOASES -- QP NO. 2 #####################

Iter | StepLength | Info | nFX | nAC

----------+------------------+------------------+---------+--------- 0 | 5.744546e-02 | REM CON 97 | 0 | 78
1 | 9.210616e-17 | ADD CON 97 | 0 | 79
2 | 6.806201e-02 | REM CON 159 | 0 | 78
3 | 5.996598e-04 | REM CON 45 | 0 | 77
4 | 8.439276e-02 | REM CON 50 | 0 | 76
5 | 2.748029e-01 | REM CON 229 | 0 | 75
6 | 5.355555e-01 | ADD CON 50 | 0 | 76
7 | 2.562778e-02 | REM CON 187 | 0 | 75
8 | 3.845621e-02 | REM CON 93 | 0 | 74
9 | 2.182160e-01 | ADD CON 93 | 0 | 75
10 | 2.271468e-01 | ADD CON 187 | 0 | 76
11 | 4.648568e-01 | REM CON 74 | 0 | 75
12 | 1.750064e-02 | ADD CON 74 | 0 | 76
13 | 4.351012e-01 | REM CON 211 | 0 | 75
14 | 5.412789e-02 | REM CON 248 | 0 | 74
15 | 3.168153e-01 | ADD CON 248 | 0 | 75
16 | 8.910957e-02 | ADD CON 211 | 0 | 76
17 | 4.478124e-01 | ADD CON 21 | 0 | 77
18 | 2.163341e-01 | REM CON 27 | 0 | 76
19 | 4.937291e-02 | ADD CON 27 | 0 | 77
20 | 2.086708e-01 | REM CON 22 | 0 | 76
21 | 8.416534e-01 | ADD CON 114 | 0 | 77
22 | 1.000000e+00 | QP SOLVED | 0 | 77
1 6.905100e+01 1.11e-15 2.84e-14 1.71e+00 - 1 MESSAGE(sqpmethod): Convergence achieved after 1 iterations solver : t_proc (avg) t_wall (avg) n_eval QP | 3.93ms ( 3.93ms) 3.94ms ( 3.94ms) 1 linesearch | 14.00us ( 14.00us) 13.88us ( 13.88us) 1 nlp_fg | 8.00us ( 8.00us) 8.09us ( 8.09us) 1 nlp_grad | 13.00us ( 13.00us) 13.07us ( 13.07us) 1 nlp_hess_l | 2.00us ( 2.00us) 1.67us ( 1.67us) 1 nlp_jac_fg | 20.00us ( 10.00us) 18.62us ( 9.31us) 2 total | 4.01ms ( 4.01ms) 4.01ms ( 4.01ms) 1 1 -th step. [0.19999999 0.19420062] [-0.98577947 0.24823515 0.09532049 0.92651825 0.19846765 0.53545231] -9.574529339871601 False {'goal_reached': False, 'mse': 1.7902061840546073, 'constraint_values': array([-1.01422053e+00, -3.40282347e+38, -1.45320487e-01, -3.40282347e+38, -1.68199746e+00, -3.40282347e+38, -2.98577947e+00, -3.40282347e+38, -1.90467951e+00, -3.40282347e+38, -1.28506215e+00, -3.40282347e+38, -1.99999990e-01, -1.94200623e-01, -9.99999999e-09, -5.79937722e-03]), 'constraint_violation': 0}

iter objective inf_pr inf_du ||d|| lg(rg) ls 0 6.906276e+01 1.17e+00 9.99e+00 0.00e+00 - 0

#################### qpOASES -- QP NO. 3 #####################

Iter | StepLength | Info | nFX | nAC

----------+------------------+------------------+---------+--------- 0 | 9.327452e-02 | REM CON 51 | 0 | 76
1 | 2.896507e-17 | ADD CON 51 | 0 | 77
2 | 3.451148e-02 | REM CON 113 | 0 | 76
3 | 2.273130e-03 | REM CON 136 | 0 | 75
4 | 3.532828e-02 | REM CON 160 | 0 | 74
5 | 1.555761e-01 | REM CON 183 | 0 | 73
6 | 3.209650e-01 | REM CON 70 | 0 | 72
7 | 2.698751e-02 | REM CON 206 | 0 | 71
8 | 1.325484e-01 | REM CON 118 | 0 | 70
9 | 1.755746e-01 | REM CON 141 | 0 | 69
10 | 1.382022e-02 | ADD CON 160 | 0 | 70
11 | 2.054029e-01 | ADD CON 70 | 0 | 71
12 | 3.057243e-02 | ADD CON 141 | 0 | 72
13 | 1.234219e-02 | REM CON 164 | 0 | 71
14 | 1.551252e-02 | ADD CON 183 | 0 | 72
15 | 1.691767e-01 | ADD CON 164 | 0 | 73
16 | 3.000990e-02 | ADD CON 118 | 0 | 74
17 | 5.758738e-02 | REM CON 183 | 0 | 73
18 | 6.461810e-02 | REM CON 95 | 0 | 72
19 | 2.248397e-01 | REM CON 160 | 0 | 71
20 | 2.158103e-01 | ADD CON 87 | 0 | 72
21 | 3.755095e-02 | REM CON 248 | 0 | 71
22 | 1.270546e-02 | REM CON 28 | 0 | 70
23 | 7.672153e-03 | ADD CON 28 | 0 | 71
24 | 5.617051e-05 | REM CON 48 | 0 | 70
25 | 6.701472e-02 | ADD CON 64 | 0 | 71
26 | 6.479426e-03 | ADD CON 248 | 0 | 72
27 | 2.541856e-05 | REM CON 25 | 0 | 71
28 | 4.126423e-01 | ADD CON 41 | 0 | 72
29 | 1.057060e-05 | REM CON 246 | 0 | 71
30 | 2.068506e-02 | ADD CON 95 | 0 | 72
31 | 8.307346e-02 | REM CON 2 | 0 | 71
32 | 1.474649e-01 | ADD CON 25 | 0 | 72
33 | 4.303787e-03 | ADD CON 246 | 0 | 73
34 | 2.735864e-03 | REM CON 41 | 0 | 72
35 | 2.206961e-01 | ADD CON 2 | 0 | 73
36 | 3.416706e-05 | REM CON 21 | 0 | 72
37 | 5.667259e-02 | ADD CON 160 | 0 | 73
38 | 4.339598e-02 | ADD CON 48 | 0 | 74
39 | 9.273479e-03 | REM CON 245 | 0 | 73
40 | 2.893517e-01 | ADD CON 245 | 0 | 74
41 | 4.146509e-02 | REM CON 87 | 0 | 73
42 | 4.809294e-02 | ADD CON 113 | 0 | 74
43 | 3.672651e-02 | REM CON 118 | 0 | 73
44 | 2.199743e-02 | ADD CON 118 | 0 | 74
45 | 2.056227e-01 | ADD CON 183 | 0 | 75
46 | 1.585555e-01 | ADD CON 41 | 0 | 76
47 | 1.859697e-02 | REM CON 28 | 0 | 75
48 | 4.142917e-04 | ADD CON 28 | 0 | 76
49 | 5.718643e-03 | REM CON 25 | 0 | 75
50 | 1.387979e-02 | REM CON 141 | 0 | 74
51 | 9.780109e-03 | ADD CON 141 | 0 | 75
52 | 1.294041e-01 | ADD CON 25 | 0 | 76
53 | 2.049099e-02 | REM CON 64 | 0 | 75
54 | 9.626207e-02 | ADD CON 206 | 0 | 76
55 | 1.058965e-01 | REM CON 164 | 0 | 75
56 | 1.008382e-02 | ADD CON 164 | 0 | 76
57 | 4.951753e-01 | ADD CON 136 | 0 | 77
58 | 3.125920e-01 | REM CON 51 | 0 | 76
59 | 1.631619e-03 | ADD CON 51 | 0 | 77
60 | 8.536231e-02 | REM CON 187 | 0 | 76
61 | 1.270538e-02 | ADD CON 187 | 0 | 77
62 | 1.869176e-01 | ADD CON 229 | 0 | 78
63 | 5.755635e-01 | ADD CON 91 | 0 | 79
64 | 1.000000e+00 | QP SOLVED | 0 | 79
1 6.411921e+01 5.55e-15 1.14e-13 1.17e+00 - 1 MESSAGE(sqpmethod): Convergence achieved after 1 iterations solver : t_proc (avg) t_wall (avg) n_eval QP | 10.39ms ( 10.39ms) 10.69ms ( 10.69ms) 1 linesearch | 14.00us ( 14.00us) 14.25us ( 14.25us) 1 nlp_fg | 9.00us ( 9.00us) 8.36us ( 8.36us) 1 nlp_grad | 14.00us ( 14.00us) 14.57us ( 14.57us) 1 nlp_hess_l | 2.00us ( 2.00us) 1.67us ( 1.67us) 1 nlp_jac_fg | 18.00us ( 9.00us) 18.61us ( 9.31us) 2 total | 10.46ms ( 10.46ms) 10.77ms ( 10.77ms) 1 2 -th step. [0.04439637 0.04017326] [-0.95645846 0.33804883 0.15788519 0.34889909 0.25184205 0.53217667] -8.411749986610035 False {'goal_reached': False, 'mse': 1.62397014661692, 'constraint_values': array([-1.04354154e+00, -3.40282347e+38, -2.07885191e-01, -3.40282347e+38, -1.73537186e+00, -3.40282347e+38, -2.95645846e+00, -3.40282347e+38, -1.84211481e+00, -3.40282347e+38, -1.23168776e+00, -3.40282347e+38, -4.43963687e-02, -4.01732595e-02, -1.55603631e-01, -1.59826741e-01]), 'constraint_violation': 0}

iter objective inf_pr inf_du ||d|| lg(rg) ls 0 6.412483e+01 9.80e-01 9.86e+00 0.00e+00 - 0

#################### qpOASES -- QP NO. 4 #####################

Iter | StepLength | Info | nFX | nAC

----------+------------------+------------------+---------+--------- 0 | 1.942278e-01 | REM CON 136 | 0 | 78
1 | 1.068916e-01 | REM CON 4 | 0 | 77
2 | 5.034360e-15 | ADD CON 4 | 0 | 78
3 | 5.022337e-02 | REM CON 51 | 0 | 77
WARNING: Stepsize is 0.000000000000000e+00 4 | 0.000000e+00 | ADD CON 51 | 0 | 78
5 | 6.522466e-01 | REM CON 229 | 0 | 77
6 | 2.243194e-01 | REM CON 47 | 0 | 76
7 | 6.435181e-01 | REM CON 187 | 0 | 75
8 | 1.046850e-01 | ADD CON 47 | 0 | 76
9 | 4.217880e-01 | ADD CON 187 | 0 | 77
WARNING: Stepsize is 0.000000000000000e+00 WARNING: Stepsize is 0.000000000000000e+00 WARNING: Stepsize is 0.000000000000000e+00 WARNING: Stepsize is 0.000000000000000e+00 ERROR: Premature homotopy termination because QP is infeasible Error in Opti::solve [OptiNode] at .../casadi/core/optistack.cpp:159: Error in Function::operator() for 'qpsol' [QpoasesInterface] at .../casadi/core/function.cpp:1368: .../casadi/core/conic.cpp:525: conic process failed. Set 'error_on_fail' option to false to ignore this error. Infeasible MPC Problem #########################################

Loading GP dimension 0

######################################### Path: ./trained_gp_model/best_model_0.pth Loaded! #########################################

Loading GP dimension 1

######################################### Path: ./trained_gp_model/best_model_1.pth Loaded! #########################################

Loading GP dimension 2

######################################### Path: ./trained_gp_model/best_model_2.pth Loaded! #########################################

Loading GP dimension 3

######################################### Path: ./trained_gp_model/best_model_3.pth Loaded! #########################################

Loading GP dimension 4

######################################### Path: ./trained_gp_model/best_model_4.pth Loaded! #########################################

Loading GP dimension 5

######################################### Path: ./trained_gp_model/best_model_5.pth Loaded! Init State: [-1. 0. 0. 0. -0. 0.] CasADi - 2022-09-13 16:26:47 MESSAGE("nlp::init") [.../casadi/core/function_internal.cpp:477] CasADi - 2022-09-13 16:26:47 MESSAGE("nlp::init") [.../casadi/core/x_function.hpp:281] CasADi - 2022-09-13 16:26:47 MESSAGE("nlp::init") [.../casadi/core/mx_function.cpp:102] CasADi - 2022-09-13 16:26:47 MESSAGE("Using live variables: work array is 27 instead of 577") [.../casadi/core/mx_function.cpp:305] CasADi - 2022-09-13 16:26:47 MESSAGE("solver::init") [.../casadi/core/function_internal.cpp:477] CasADi - 2022-09-13 16:26:47 MESSAGE("solver::create_function nlp_grad:[x, p, lam:f, lam:g]->[f, g, grad:gamma:x, grad:gamma:p]") [.../casadi/core/oracle_function.cpp:132] CasADi - 2022-09-13 16:26:48 MESSAGE("solver::create_function nlp_f:[x, p]->[f]") [.../casadi/core/oracle_function.cpp:132] CasADi - 2022-09-13 16:26:48 MESSAGE("solver::create_function nlp_g:[x, p]->[g]") [.../casadi/core/oracle_function.cpp:132] CasADi - 2022-09-13 16:26:48 MESSAGE("solver::create_function nlp_grad_f:[x, p]->[f, grad:f:x]") [.../casadi/core/oracle_function.cpp:132] CasADi - 2022-09-13 16:26:48 MESSAGE("solver::create_function nlp_jac_g:[x, p]->[g, jac:g:x]") [.../casadi/core/oracle_function.cpp:132] CasADi - 2022-09-13 16:26:48 MESSAGE("solver::create_function nlp_hess_l:[x, p, lam:f, lam:g]->[hess:gamma:x:x]") [.../casadi/core/oracle_function.cpp:132] There are 86 variables and 335 constraints. Using exact Hessian Total number of variables............................: 86 variables with only lower bounds: 0 variables with lower and upper bounds: 0 variables with only upper bounds: 0 Total number of equality constraints.................: 66 Total number of inequality constraints...............: 269 inequality constraints with only lower bounds: 0 inequality constraints with lower and upper bounds: 86 inequality constraints with only upper bounds: 183

Number of Iterations....: 100

(scaled) (unscaled)

Objective...............: 1.4793434887362989e+02 1.4793434887362989e+02 Dual infeasibility......: 5.9408874591127736e+06 5.9408874591127736e+06 Constraint violation....: 1.7967820698854311e+00 2.4033257283054743e+00 Complementarity.........: 7.7994891085978524e-01 7.7994891085978524e-01 Overall NLP error.......: 2.9108033978286326e+03 5.9408874591127736e+06

Number of objective function evaluations = 101 Number of objective gradient evaluations = 101 Number of equality constraint evaluations = 101 Number of inequality constraint evaluations = 101 Number of equality constraint Jacobian evaluations = 101 Number of inequality constraint Jacobian evaluations = 101 Number of Lagrangian Hessian evaluations = 100 Total CPU secs in IPOPT (w/o function evaluations) = 0.110 Total CPU secs in NLP function evaluations = 0.988

EXIT: Maximum Number of Iterations Exceeded. solver : t_proc (avg) t_wall (avg) n_eval nlp_f | 362.00us ( 3.58us) 358.95us ( 3.55us) 101 nlp_g | 65.91ms (652.59us) 65.98ms (653.29us) 101 nlp_grad | 2.78ms ( 2.78ms) 2.78ms ( 2.78ms) 1 nlp_grad_f | 372.00us ( 3.65us) 374.08us ( 3.67us) 102 nlp_hess_l | 695.80ms ( 6.96ms) 696.06ms ( 6.96ms) 100 nlp_jac_g | 228.97ms ( 2.24ms) 229.22ms ( 2.25ms) 102 total | 1.11 s ( 1.11 s) 1.11 s ( 1.11 s) 1 GP Mean eq Contribution: [-3.88652854e-04 2.45557454e-02 2.85799483e-02 4.55388601e-01 -1.52168405e-03 5.16830598e-03] True GP value: [-3.93645639e-04 2.33940861e-02 2.85505519e-02 4.66247972e-01 -1.40092526e-03 4.32021969e-03] GP SELECT ACTION TIME: 3.138321131002158 0 -th step. [0.24181908 0.23957112] [-1.00030505 -0.01170715 0.04174303 0.80051446 -0.02337966 -0.44847958] -10.0168287032323 True {'goal_reached': False, 'mse': 1.9188666249301738, 'constraint_values': array([-9.99694948e-01, -3.40282347e+38, -9.17430266e-02, -3.40282347e+38, -1.46015014e+00, -3.40282347e+38, -3.00030505e+00, -3.40282347e+38, -1.95825697e+00, -3.40282347e+38, -1.50690947e+00, -3.40282347e+38, -2.41819078e-01, -2.39571116e-01, 4.18190776e-02, 3.95711161e-02]), 'constraint_violation': 1}
question
opened by merdan-9 6
Allow varying env model priors for PID, LQR, and MPC
This PR is meant for

current PID for quadrotor uses the overridden system params from env instead of the prior params, which should be fixed

Cartpole seems to have an argument prior_prop to set the prior inertial properties (also used in the benchmark experiment for LQR^150%), but Quadrotor does not have such an argument, we should add this missing functionality in Quadrotor.

currently the prior params in env are the same as the default URDF params, but we need to vary the priors for testing the similarity metrics

bug enhancement
opened by Justin-Yuan 6
Understanding BoundedConstraint class
I'm not sure if this is a bug or i'm using the functionality wrong, so please let me know accordingly.

Here is how I'm trying to use BoundedConstraint, specified in my task_config yaml file.

constraints: - constraint_form: bounded_constraint lower_bounds: [0, 0, 0] # should match state dim upper_bounds: [2.6, 0, 0] constrained_variable: state active_dims: 0 # only position

When i use BoundedConstraint for constraining state (I have 3 states but want to constraint only 1), I realize I needed to supply lower_bounds & upper_bounds with shapes that are equal to the number of states (e.g. 3 if have 3 states), due to self.dim being defined as such: self.dim = env.state_dim which is used in self.constraint_filter = np.eye(self.dim)[active_dims] here, where it is supposed to only extract active_dims from these bounds for the target state to be constrained.

But when i do so, where lower_bounds matches shape of env.state_dim, the assertion here assert A.shape[1] == self.dim, '[ERROR] A has the wrong dimension!' fails.

This seem to fail because in Constraint , inside this code chunk, after constraint_filter is defined, self.dim is overwritten by len(active_dims), so it would have shape of active_dims which when I use is 1, while A already has shape (6, 3) due to self.dim being set to env.state_dim which was 3 earlier.

if self.constrained_variable == ConstrainedVariableType.STATE: self.dim = env.state_dim elif self.constrained_variable == ConstrainedVariableType.INPUT: self.dim = env.action_dim elif self.constrained_variable == ConstrainedVariableType.INPUT_AND_STATE: self.dim = env.state_dim + env.action_dim else: raise NotImplementedError('[ERROR] invalid constrained_variable (use STATE, INPUT or INPUT_AND_STATE).') # Save the strictness attribute self.strict = strict # Only want to select specific dimensions, implemented via a filter matrix. if active_dims is not None: if isinstance(active_dims, int): active_dims = [active_dims] assert isinstance(active_dims, (list, np.ndarray)), '[ERROR] active_dims is not a list/array.' assert (len(active_dims) <= self.dim), '[ERROR] more active_dim than constrainable self.dim' assert all(isinstance(n, int) for n in active_dims), '[ERROR] non-integer active_dim.' assert all((n < self.dim) for n in active_dims), '[ERROR] active_dim not stricly smaller than self.dim.' assert (len(active_dims) == len(set(active_dims))), '[ERROR] duplicates in active_dim' self.constraint_filter = np.eye(self.dim)[active_dims] self.dim = len(active_dims)

Could you please check if this was the issue?

Another attempt I took was to set lower_bounds to have shape same as active_dims, e.g. 1 (only want to constraint 1 state). It doesn't work, because matmul would fail here self.sym_func = lambda x: self.A @ self.constraint_filter @ x - self.b for LinearConstraint. Full error: ValueError: matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 1 is different from 3)

Summary:

What is the right shape for lower_bounds and upper_bounds?

When I set it equal to shape of env.state_dim, I get error from assert A.shape[1] == self.dim, '[ERROR] A has the wrong dimension!' because self.dim = len(active_dims).

When I set it equal to len(active_dims), it throws error ValueError: matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 1 is different from 3) because of self.sym_func = lambda x: self.A @ self.constraint_filter @ x - self.b.

Would love to know if this is indeed a bug or if I'm using it wrong. I'll also keep trying again in case maybe I missed anything.

Thank you!

@adamhall @JacopoPan @Justin-Yuan
question
opened by nicholasprayogo 6

bug fix for explicit dynamics of 2d quadrotor

If the dynamics are changed to the explicit dynamics for the tracking example (tracking.yaml, line 7 -> physics: dyn) and tracking.py is run, the following error occurs:

Commands

cd examples
python3 tracking.py --overrides ./tracking.yaml

Error stack

Traceback (most recent call last):
  File "tracking.py", line 77, in <module>
    main()
  File "tracking.py", line 58, in main
    results = ctrl.run(iterations=ITERATIONS)
  File "/home/hanna/Data/Research/SafeRL/safe-control-gym/safe_control_gym/controllers/pid/pid.py", line 97, in run
    obs, reward, done, info = self.env.step(action)
  File "/home/hanna/Data/Research/SafeRL/safe-control-gym/safe_control_gym/envs/gym_pybullet_drones/quadrotor.py", line 367, in step
    super()._advance_simulation(rpm, disturb_force)
  File "/home/hanna/Data/Research/SafeRL/safe-control-gym/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py", line 285, in _advance_simulation
    self._dynamics(clipped_action[i, :], i)
  File "/home/hanna/Data/Research/SafeRL/safe-control-gym/safe_control_gym/envs/gym_pybullet_drones/base_aviary.py", line 552, in _dynamics
    vel = vel + self.TIMESTEP * no_pybullet_dyn_accs
AttributeError: 'Quadrotor' object has no attribute 'TIMESTEP'

This is because the attribute self.TIMESTEP is not defined. Minor changes in the _dynamics function solve this issue. A proposal how to fix this is created as pull request.

bug

opened by hannakrasowski 6

Installation problems

Hi,

When I'm installing the package, I get these errors. When I try to install pycddlib, I also get similar errors. I have already installed gmp, as mentioned in the installation instruction. Do you know what could be the problem, can you please help me?

question

opened by farnammn 6
Add metrics folder and MMD implementation

Other metrics can be implemented as modules under safe_control_gym/math_and_models/common_metrics/ , or if suitable under the existing similarity_metrics.py.

An issue is if there's currently a metrics.py file under math_and_models with the CVaR robustness metric computation, used in experiment.py, it would be better to put it under the common_metrics/ folder as well and rename the folder to metrics/.
enhancement

opened by Justin-Yuan 5
10.13 Pull Version

@JacopoPan Hello Jacopo! Our team made a submission, but this submission is not the final version. The primary purpose of this submission is to verify whether our idea is correct, that is, whether we correctly use variables and methods. I only submitted the demo at level2, where we used num_episodes: 1200 in training. Meanwhile, in order to facilitate the test, I also submitted the trained model (I don't know if this is allowed), and we will iterate several versions later. If the version submitted this time does not violate the rules, I believe we can timely provide the final version. Looking forward to your reply, thank you!
competition

opened by ZQ2413262560 4
MSE counter should be incremented by one?

I noticed that the MSE computation is off by one step. In step, the state gets updated, but self.ctrl_step_counter doesn't get updated until after _get_info and _get_rewardare called. Thus, thestate_error` is subtracting the previous time step's reference state from the current state.
bug

opened by adamhall 4
Adding pytest to the pytoml

I think pytest should be on the pytoml since its useful for locally running the tests. However, using poetry add pytest isn't working for me, fails due to some error in updating casadi. Not sure if someone with more poetry experience could fix this.
bug enhancement

opened by Federico-PizarroBejarano 2
10.16 Pull Version

@JacopoPan Hi Jacopo! Code runs using commands: cd competition/ python3 getting_started.py --overrides level0.yaml It takes about 7-8 hours to train on my machine (1 RTX3090).

Time is limited, and some ideas are too late to put into practice. If there is a chance, I will continue to try this task! I would like to thank the organizers for this opportunity, Thank you!
competition

opened by ZQ2413262560 5
H^2 Team Competition Submission
Num of episodes: 1

Code is in edit_this.py with a helper file custom_utils.py under competition folder. Code runs using default commands:

cd competition/ python3 getting_started.py --overrides level0.yaml

Team members: @aoi3142 @huiyulhy @hashirzahir

CC: @JacopoPan , this is our team's submission. Thank you!
competition
opened by HashirZahir 2
Should controllers be allowed to use `U_GOAL` or only `U_EQ`?

Carrying on from #93.

Many controllers have been using env.U_GOAL either as a linearization point or in the cost which uses the true system mass, not the prior mass, which is what symbolic.U_EQ is for. Should controllers have access to U_GOAL or they should exclusively be using U_EQ. @Justin-Yuan What are your thoughts on this for the RL cost and Normalization?
bug question

opened by adamhall 5

Releases(v0.5.0)

v0.5.0(Aug 21, 2021)

Codebase for "Safe Learning in Robotics: From Learning-Based Control to Safe Reinforcement Learning" https://arxiv.org/abs/2108.06266
Source code(tar.gz)
Source code(zip)

PyBullet CartPole and Quadrotor environments—with CasADi symbolic a priori dynamics—for learning-based control and reinforcement learning

Related tags

Overview

safe-control-gym

Architecture

Install on Ubuntu/macOS

2D Quadrotor Lemniscate Trajectory Tracking

Re-create the Results in "Safe Learning in Robotics" [arXiv link]

Figure 6—Robust GP-MPC [1]

Figure 7—Safe RL Exploration [2]

Figure 8—Model Predictive Safety Certification [3]

References

Comments

TLDR

Context

Error

Hypothesis

TLDR

Question

error

info

Loading GP dimension 0

Loading GP dimension 1

Loading GP dimension 2

Loading GP dimension 3

Loading GP dimension 4

Loading GP dimension 5

Releases(v0.5.0)

v0.5.0(Aug 21, 2021)

Owner

Dynamic Systems Lab

TagLab: an image segmentation tool oriented to marine data analysis

ncnn is a high-performance neural network inference framework optimized for the mobile platform

A simple implementation of Kalman filter in Multi Object Tracking

This repository focus on Image Captioning & Video Captioning & Seq-to-Seq Learning & NLP

Github for the conference paper GLOD-Gaussian Likelihood OOD detector

ML models and internal tensors 3D visualizer

[ICCV'21] NEAT: Neural Attention Fields for End-to-End Autonomous Driving

QAHOI: Query-Based Anchors for Human-Object Interaction Detection (paper)

Contains source code for the winning solution of the xView3 challenge

An Ensemble of CNN (Python 3.5.1 Tensorflow 1.3 numpy 1.13)

Code of Puregaze: Purifying gaze feature for generalizable gaze estimation, AAAI 2022.

This repository implements and evaluates convolutional networks on the Möbius strip as toy model instantiations of Coordinate Independent Convolutional Networks.

Official code of our work, Unified Pre-training for Program Understanding and Generation [NAACL 2021].

Fast Differentiable Matrix Sqrt Root

PRIME: A Few Primitives Can Boost Robustness to Common Corruptions

Unofficial PyTorch implementation of Fastformer based on paper "Fastformer: Additive Attention Can Be All You Need"."

Pytorch implementation of YOLOX、PPYOLO、PPYOLOv2、FCOS an so on.

PyTorch implementation of Deep HDR Imaging via A Non-Local Network (TIP 2020).

maximal update parametrization (µP)

PyTorch Implementation of Google Brain's WaveGrad 2: Iterative Refinement for Text-to-Speech Synthesis