diff --git a/README.md b/README.md index 169fd56..8df8e30 100644 --- a/README.md +++ b/README.md @@ -42,6 +42,17 @@ pip install --no-deps -e . ## Usage Take a look at the [examples](./examples) folder! +To run the examples you will need to install notebook +``` +conda install notebook +``` + +> [!NOTE] +> Running the example using the [drake simulator](https://drake.mit.edu/) needs installing the following additional dependencies, +``` +conda install meshio tqdm +pip install drake git+https://github.com/akhilsathuluri/odio_urdf.git@comodo +``` ### Maintainer diff --git a/examples/drake_gain_tuning.ipynb b/examples/drake_gain_tuning.ipynb new file mode 100644 index 0000000..0a0d5f3 --- /dev/null +++ b/examples/drake_gain_tuning.ipynb @@ -0,0 +1,9634 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "# Comodo import\n", + "from comodo.drakeSimulator.drakeSimulator import DrakeSimulator\n", + "from comodo.mujocoSimulator.mujocoSimulator import MujocoSimulator\n", + "from comodo.robotModel.robotModel import RobotModel\n", + "from comodo.robotModel.createUrdf import createUrdf\n", + "from comodo.centroidalMPC.centroidalMPC import CentroidalMPC\n", + "from comodo.centroidalMPC.mpcParameterTuning import MPCParameterTuning\n", + "from comodo.TSIDController.TSIDParameterTuning import TSIDParameterTuning\n", + "from comodo.TSIDController.TSIDController import TSIDController\n", + "\n", + "# optimisation import\n", + "import pygmo as pg\n", + "\n", + "# General import \n", + "import xml.etree.ElementTree as ET\n", + "import numpy as np\n", + "import tempfile\n", + "import urllib.request\n", + "import logging\n", + "import time\n", + "import os\n", + "from contextlib import redirect_stderr, redirect_stdout" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "# set the logging level\n", + "logging.getLogger().setLevel(logging.INFO)" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "# Getting stickbot urdf file and convert it to string \n", + "urdf_robot_file = tempfile.NamedTemporaryFile(mode=\"w+\")\n", + "url = 'https://raw.githubusercontent.com/icub-tech-iit/ergocub-gazebo-simulations/master/models/stickBot/model.urdf'\n", + "urllib.request.urlretrieve(url, urdf_robot_file.name)\n", + "# Load the URDF file\n", + "tree = ET.parse(urdf_robot_file.name)\n", + "root = tree.getroot()\n", + "\n", + "# Convert the XML tree to a string\n", + "robot_urdf_string_original = ET.tostring(root)\n", + "\n", + "create_urdf_instance = createUrdf(\n", + " original_urdf_path=urdf_robot_file.name, save_gazebo_plugin=False\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "# Define parametric links and controlled joints \n", + "legs_link_names = [\"hip_3\", \"lower_leg\"]\n", + "joint_name_list = [\n", + " \"r_shoulder_pitch\",\n", + " \"r_shoulder_roll\",\n", + " \"r_shoulder_yaw\",\n", + " \"r_elbow\",\n", + " \"l_shoulder_pitch\",\n", + " \"l_shoulder_roll\",\n", + " \"l_shoulder_yaw\",\n", + " \"l_elbow\",\n", + " \"r_hip_pitch\",\n", + " \"r_hip_roll\",\n", + " \"r_hip_yaw\",\n", + " \"r_knee\",\n", + " \"r_ankle_pitch\",\n", + " \"r_ankle_roll\",\n", + " \"l_hip_pitch\",\n", + " \"l_hip_roll\",\n", + " \"l_hip_yaw\",\n", + " \"l_knee\",\n", + " \"l_ankle_pitch\",\n", + " \"l_ankle_roll\",\n", + "]" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [], + "source": [ + "# Define the robot modifications\n", + "modifications = {}\n", + "for item in legs_link_names:\n", + " left_leg_item = \"l_\" + item\n", + " right_leg_item = \"r_\" + item\n", + " modifications.update({left_leg_item: 1.2})\n", + " modifications.update({right_leg_item: 1.2})\n", + "# Motors Parameters \n", + "Im_arms = 1e-3*np.ones(4) # from 0-4\n", + "Im_legs = 1e-3*np.ones(6) # from 5-10\n", + "kv_arms = 0.001*np.ones(4) # from 11-14\n", + "kv_legs = 0.001*np.ones(6) #from 20\n", + "\n", + "Im = np.concatenate((Im_arms, Im_arms, Im_legs, Im_legs))\n", + "kv = np.concatenate((kv_arms, kv_arms, kv_legs, kv_legs))" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [], + "source": [ + "# Modify the robot model and initialize\n", + "create_urdf_instance.modify_lengths(modifications)\n", + "urdf_robot_string = create_urdf_instance.write_urdf_to_file()\n", + "create_urdf_instance.reset_modifications()" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [], + "source": [ + "def cost_function(x_k, simulator=\"drake\", urdf_robot_string=urdf_robot_string, visualise_flag = False):\n", + " with redirect_stderr(open(os.devnull, \"w\")):\n", + " robot_model_init = RobotModel(urdf_robot_string, \"stickBot\", joint_name_list)\n", + " s_des, xyz_rpy, H_b = robot_model_init.compute_desired_position_walking()\n", + " # Define simulator and initial position\n", + " if simulator == \"drake\":\n", + " sim_instance = DrakeSimulator()\n", + " sim_instance.set_visualize_robot_flag(visualise_flag)\n", + " sim_instance.load_model(robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im)\n", + "\n", + " elif simulator == \"mujoco\":\n", + " sim_instance = MujocoSimulator()\n", + " sim_instance.load_model(robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im)\n", + " sim_instance.set_visualize_robot_flag(visualise_flag)\n", + " \n", + " # sim_instance.load_model(robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im)\n", + " s, ds, tau = sim_instance.get_state()\n", + " t = sim_instance.get_simulation_time()\n", + " H_b = sim_instance.get_base()\n", + " w_b = sim_instance.get_base_velocity()\n", + " # Controller Parameters\n", + " tsid_parameter = TSIDParameterTuning()\n", + " mpc_parameters = MPCParameterTuning()\n", + " if x_k is not None:\n", + " tsid_parameter.set_from_x_k(x_k)\n", + "\n", + " # TSID Instance\n", + " TSID_controller_instance = TSIDController(frequency=0.01, robot_model=robot_model_init)\n", + " TSID_controller_instance.define_tasks(tsid_parameter)\n", + " TSID_controller_instance.set_state_with_base(s, ds, H_b, w_b, t)\n", + "\n", + " # MPC Instance\n", + " step_lenght = 0.1\n", + " mpc = CentroidalMPC(robot_model=robot_model_init, step_length=step_lenght)\n", + " mpc.intialize_mpc(mpc_parameters=mpc_parameters)\n", + "\n", + " # Set desired quantities\n", + " mpc.configure(s_init=s_des, H_b_init=H_b)\n", + " TSID_controller_instance.compute_com_position()\n", + " mpc.define_test_com_traj(TSID_controller_instance.COM.toNumPy())\n", + "\n", + " # TODO: Set initial robot state and plan trajectories \n", + " sim_instance.step(1)\n", + "\n", + " # Reading the state \n", + " s, ds, tau = sim_instance.get_state()\n", + " try:\n", + " H_b = sim_instance.get_base()\n", + " except RuntimeError as e:\n", + " return sim_instance.get_simulation_time()\n", + " w_b = sim_instance.get_base_velocity()\n", + " t = sim_instance.get_simulation_time()\n", + "\n", + " # MPC\n", + " mpc.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + " mpc.initialize_centroidal_integrator(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + " mpc_output = mpc.plan_trajectory()\n", + "\n", + " # Set loop variables \n", + " TIME_TH = 20.0\n", + "\n", + " # Define number of steps\n", + " n_step = int(\n", + " TSID_controller_instance.frequency / sim_instance.get_simulation_frequency()\n", + " )\n", + " n_step_mpc_tsid = int(mpc.get_frequency_seconds() / TSID_controller_instance.frequency)\n", + "\n", + " counter = 0\n", + " mpc_success = True\n", + " energy_tot = 0.0\n", + " succeded_controller = True\n", + "\n", + " # Simulation-control loop\n", + " if simulator == \"drake\" and visualise_flag:\n", + " sim_instance.meshcat.StartRecording()\n", + "\n", + " while t < TIME_TH:\n", + " \n", + " # Reading robot state from simulator\n", + " s, ds, tau = sim_instance.get_state()\n", + " energy_i = np.linalg.norm(tau)\n", + " try:\n", + " H_b = sim_instance.get_base()\n", + " except RuntimeError as e:\n", + " return sim_instance.get_simulation_time()\n", + "\n", + " w_b = sim_instance.get_base_velocity()\n", + " t = sim_instance.get_simulation_time()\n", + "\n", + " # Update TSID\n", + " TSID_controller_instance.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + "\n", + " # MPC plan \n", + " if counter == 0:\n", + " mpc.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + " mpc.update_references()\n", + " mpc_success = mpc.plan_trajectory()\n", + " mpc.contact_planner.advance_swing_foot_planner()\n", + " if not (mpc_success):\n", + " logging.error(\"MPC failed\")\n", + " break\n", + "\n", + " # Reading new references\n", + " com, dcom, forces_left, forces_right = mpc.get_references()\n", + " left_foot, right_foot = mpc.contact_planner.get_references_swing_foot_planner()\n", + "\n", + " # Update references TSID \n", + " TSID_controller_instance.update_task_references_mpc(\n", + " com=com,\n", + " dcom=dcom,\n", + " ddcom=np.zeros(3),\n", + " left_foot_desired=left_foot,\n", + " right_foot_desired=right_foot,\n", + " s_desired=np.array(s_des),\n", + " wrenches_left=forces_left,\n", + " wrenches_right=forces_right,\n", + " )\n", + "\n", + " # Run control \n", + " succeded_controller = TSID_controller_instance.run()\n", + "\n", + " if not (succeded_controller):\n", + " logging.error(\"Controller failed\")\n", + " return sim_instance.get_simulation_time()\n", + "\n", + " tau = TSID_controller_instance.get_torque()\n", + "\n", + " # Step the simulator\n", + " sim_instance.set_input(tau)\n", + " # sim_instance.step_with_motors(n_step=n_step, torque=tau)\n", + " try:\n", + " sim_instance.step(n_step=n_step)\n", + " except RuntimeError as e:\n", + " logging.error(e)\n", + " return sim_instance.get_simulation_time()\n", + "\n", + " counter = counter + 1\n", + "\n", + " if counter == n_step_mpc_tsid:\n", + " counter = 0\n", + " \n", + " if simulator == \"drake\" and visualise_flag:\n", + " sim_instance.meshcat.StopRecording()\n", + " sim_instance.meshcat.PublishRecording()\n", + " # sim_instance.close_visualization()\n", + "\n", + "\n", + " return sim_instance.get_simulation_time()" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [], + "source": [ + "class GainTuningProblem:\n", + " # @staticmethod\n", + " def fitness(self, x):\n", + " with redirect_stdout(open(os.devnull, \"w\")):\n", + " result = -cost_function(x, simulator=\"mujoco\") - cost_function(x, simulator=\"drake\")\n", + " return [result]\n", + "\n", + " def get_bounds(self):\n", + " return ([0] * 28, [1000] * 28)" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:40:50.423] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:50.423] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:50.423] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:50.423] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:50.423] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:50.423] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:50.423] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:50.423] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:50.430] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:40:50.430] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:40:50.430] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:40:50.430] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:40:50.430] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:50.430] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:40:50.430] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:50.430] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:50.430] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:50.430] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:50.505] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:50.505] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:40:50.505] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:50.505] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:40:50.811] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:50.820] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:50.972] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:50.983] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:50.995] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:51.140] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:51.166] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:51.312] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:40:51.150] [thread: 43757] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:40:51.433] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:51.442] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:51.452] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:51.691] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:51.701] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:51.710] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:51.719] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:40:51.728] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING:drake:Rigid (non-deformable) half spaces are not currently supported for deformable contact; registration is allowed, but no contact data will be reported.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:40:51.851] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:51.851] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:51.851] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:51.851] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:51.851] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:51.851] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:51.851] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:51.851] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:51.857] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:40:51.857] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:40:51.857] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:40:51.857] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:40:51.857] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:51.857] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:40:51.857] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:51.857] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:51.857] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:51.857] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:51.931] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:51.931] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:40:51.931] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:51.931] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:40:52.224] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.241] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.254] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.266] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.284] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.297] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.310] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.442] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.463] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.478] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.491] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.503] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:52.516] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:40:52.646] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:52.646] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:52.646] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:52.646] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:52.646] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:52.646] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:52.646] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:52.646] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:52.653] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:40:52.653] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:40:52.653] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:40:52.653] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:40:52.653] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:52.653] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:40:52.653] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:52.653] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:52.653] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:52.653] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:52.727] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:52.727] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:40:52.727] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:52.727] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:40:53.012] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.021] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.030] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.042] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.055] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.195] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.204] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.215] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.229] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.373] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.384] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.527] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.537] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.546] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:53.554] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:40:53.563] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:40:53.686] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:53.686] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:53.686] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:53.686] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:53.687] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:53.687] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:53.687] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:53.687] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:53.693] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:40:53.693] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:40:53.693] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:40:53.693] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:40:53.693] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:53.693] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:40:53.693] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:53.693] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:53.693] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:53.693] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:53.768] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:53.768] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:40:53.768] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:53.768] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:40:54.037] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:54.056] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:54.070] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:54.084] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:54.096] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:40:54.109] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:40:54.239] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:54.239] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:54.239] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:54.239] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:54.239] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:54.239] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:54.239] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:54.239] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:54.246] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:40:54.246] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:40:54.246] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:40:54.246] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:40:54.246] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:54.246] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:40:54.246] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:54.246] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:54.246] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:54.246] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:54.321] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:54.321] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:40:54.321] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:54.321] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:40:56.714] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:56.723] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:56.732] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:56.742] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:40:56.751] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:40:56.872] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:56.872] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:56.872] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:56.872] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:56.872] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:56.872] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:56.872] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:56.872] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:56.879] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:40:56.879] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:40:56.879] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:40:56.879] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:40:56.879] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:56.879] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:40:56.879] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:56.879] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:56.879] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:56.879] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:56.953] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:56.953] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:40:56.953] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:56.953] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:40:58.855] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:58.868] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:58.881] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.072] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:40:59.078] [thread: 43757] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:40:59.078] [thread: 43757] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:40:59.083] [thread: 43757] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:40:59.083] [thread: 43757] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:40:59.087] [thread: 43757] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:40:59.087] [thread: 43757] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:40:59.091] [thread: 43757] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:40:59.091] [thread: 43757] [blf] [QPTSID::advance] Unable to update the QP solver.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:40:58.898] [thread: 43757] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:40:59.224] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:59.224] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:59.224] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:59.224] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:59.224] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:59.224] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:59.224] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:40:59.224] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:40:59.231] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:40:59.231] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:40:59.231] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:40:59.231] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:40:59.231] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:40:59.231] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:40:59.231] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:59.231] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:59.231] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:40:59.231] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:40:59.305] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:59.305] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:40:59.305] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:40:59.305] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:40:59.604] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.613] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.624] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.638] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.784] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.793] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.803] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.939] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.949] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.962] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.971] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:40:59.981] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:00.112] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:00.121] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:00.130] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:00.139] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:00.148] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:00.270] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:00.270] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:00.270] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:00.270] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:00.270] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:00.270] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:00.270] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:00.270] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:00.276] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:00.276] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:00.276] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:00.276] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:00.276] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:00.276] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:00.276] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:00.276] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:00.276] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:00.276] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:00.351] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:00.351] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:00.351] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:00.351] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:00.624] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:00.642] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:00.657] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:00.671] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:00.683] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:00.696] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:00.875] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:00.875] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:00.875] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:00.875] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:00.875] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:00.875] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:00.875] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:00.875] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:00.882] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:00.882] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:00.882] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:00.882] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:00.882] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:00.882] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:00.882] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:00.882] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:00.882] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:00.882] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:00.956] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:00.956] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:00.956] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:00.956] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:01.244] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:01.253] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:03.336] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:03.345] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:03.355] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:03.364] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:03.373] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:03.497] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:03.497] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:03.497] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:03.497] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:03.497] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:03.497] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:03.497] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:03.497] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:03.503] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:03.503] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:03.503] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:03.503] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:03.503] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:03.503] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:03.503] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:03.503] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:03.503] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:03.503] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:03.579] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:03.579] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:03.579] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:03.579] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:05.442] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:MultibodyPlant's discrete update solver failed to converge at simulation time = 1.034 with discrete update period = 0.001. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:\n", + " 1. reduce the discrete update period set at construction,\n", + " 2. decrease the high gains in your controller whenever possible,\n", + " 3. switch to a continuous model (discrete update period is zero), though this might affect the simulation run time.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:05.661] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:05.661] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:05.661] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:05.661] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:05.661] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:05.661] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:05.661] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:05.661] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:05.668] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:05.668] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:05.668] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:05.668] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:05.668] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:05.668] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:05.668] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:05.668] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:05.668] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:05.668] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:05.743] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:05.743] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:05.743] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:05.743] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:06.055] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.065] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.074] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.084] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.094] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.241] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.251] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.259] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.269] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:06.278] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:06.402] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:06.402] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:06.402] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:06.402] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:06.402] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:06.402] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:06.402] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:06.402] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:06.408] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:06.408] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:06.408] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:06.408] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:06.408] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:06.408] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:06.408] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:06.408] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:06.408] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:06.408] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:06.483] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:06.484] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:06.484] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:06.484] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:06.747] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.765] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.780] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.794] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.809] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:06.823] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:06.953] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:06.953] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:06.953] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:06.953] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:06.953] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:06.953] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:06.953] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:06.953] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:06.959] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:06.959] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:06.959] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:06.959] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:06.959] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:06.959] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:06.959] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:06.959] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:06.959] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:06.959] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:07.034] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:07.034] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:07.034] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:07.034] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:07.322] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:07.332] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:07.349] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:07.367] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:07.520] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:07.528] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:07.538] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:07.548] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:07.556] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:07.358] [thread: 43757] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:41:07.679] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:07.679] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:07.679] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:07.679] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:07.679] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:07.679] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:07.680] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:07.680] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:07.686] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:07.686] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:07.686] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:07.686] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:07.686] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:07.686] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:07.686] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:07.686] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:07.686] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:07.686] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:07.761] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:07.761] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:07.761] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:07.761] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:08.032] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.049] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.062] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.076] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.088] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:08.102] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:08.232] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:08.232] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:08.232] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:08.232] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:08.232] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:08.232] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:08.232] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:08.232] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:08.239] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:08.239] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:08.239] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:08.239] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:08.239] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:08.239] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:08.239] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:08.239] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:08.239] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:08.239] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:08.314] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:08.314] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:08.314] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:08.314] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:08.608] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.617] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.628] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.638] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.783] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.792] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.813] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.823] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.978] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:08.987] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:08.996] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:09.121] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:09.121] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:09.121] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:09.121] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:09.121] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:09.121] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:09.121] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:09.121] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:09.127] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:09.127] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:09.127] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:09.127] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:09.127] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:09.127] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:09.127] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:09.127] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:09.127] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:09.127] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:09.202] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:09.202] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:09.202] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:09.202] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:09.477] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:09.495] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:09.508] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:09.521] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:09.534] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:09.549] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:09.681] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:09.681] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:09.681] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:09.681] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:09.682] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:09.682] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:09.682] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:09.682] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:09.688] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:09.688] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:09.688] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:09.688] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:09.688] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:09.688] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:09.688] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:09.688] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:09.688] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:09.688] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:09.762] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:09.762] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:09.762] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:09.762] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:10.048] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.057] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.067] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.086] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.252] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:10.261] [thread: 43757] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:10.271] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.284] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.293] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.438] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.447] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.465] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.474] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.607] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:10.616] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:10.625] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:10.748] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:10.748] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:10.748] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:10.748] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:10.748] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:10.748] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:10.748] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:10.748] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:10.754] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:10.754] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:10.754] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:10.754] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:10.754] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:10.754] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:10.754] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:10.754] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:10.754] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:10.754] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:10.829] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:10.829] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:10.829] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:10.829] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:11.098] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:11.115] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:11.129] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:11.143] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:11.156] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:11.168] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:11.341] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:11.341] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:11.341] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:11.341] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:11.341] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:11.341] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:11.341] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:11.341] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:11.347] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:11.347] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:11.347] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:11.347] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:11.347] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:11.347] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:11.347] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:11.347] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:11.347] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:11.347] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:11.422] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:11.422] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:11.422] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:11.422] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:11.712] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:14.123] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:14.277] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:14.286] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:14.295] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:14.305] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:14.428] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:14.428] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:14.428] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:14.428] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:14.428] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:14.428] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:14.428] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:14.428] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:14.434] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:14.434] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:14.435] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:14.435] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:14.435] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:14.435] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:14.435] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:14.435] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:14.435] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:14.435] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:14.509] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:14.509] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:14.509] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:14.509] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:16.414] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:16.425] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:16.435] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:16.445] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:16.454] [thread: 43757] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Number of islands: 2\n", + "Topology: Unconnected\n", + "Migration type: point-to-point\n", + "Migrant handling policy: preserve\n", + "Status: busy\n", + "\n", + "Islands summaries:\n", + "\n", + "\t# Type Algo Prob Size Status \n", + "\t-------------------------------------------------------------------------------------------------------------------------------------------\n", + "\t0 Multiprocessing island CMA-ES: Covariance Matrix Adaptation Evolutionary Strategy 5 busy \n", + "\t1 Multiprocessing island CMA-ES: Covariance Matrix Adaptation Evolutionary Strategy 5 busy \n", + "\n", + "CMAES 4 PaGMO: \n", + "mu: 2 - lambda: 5 - mueff: 1.45979 - N: 28\n", + "cc: 0.126218 - cs: 0.100401 - c1: 0.00232572 - cmu: 0.000321301 - sigma: 0.5 - damps: 1.1004 - chiN: 5.24458\n", + "\n", + " Gen: Fevals: Best: dx: df: sigma:\n", + " 1 0 -2.913 2100.44 2.69 0.5\n", + "CMAES 4 PaGMO: \n", + "mu: 2 - lambda: 5 - mueff: 1.45979 - N: 28\n", + "cc: 0.126218 - cs: 0.100401 - c1: 0.00232572 - cmu: 0.000321301 - sigma: 0.5 - damps: 1.1004 - chiN: 5.24458\n", + "\n", + " Gen: Fevals: Best: dx: df: sigma:\n", + " 1 0 -2.793 2100.44 2.39 0.5\n", + "[DEBUG] [2023-12-18 10:41:17.364] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:17.364] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:17.364] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:17.364] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:17.364] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:17.364] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:17.364] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:17.364] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:17.365] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:17.365] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:17.365] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[[INFODEBUG] ] [2023-12-18 10:41:17.365] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.[2023-12-18 10:41:17.365] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "\n", + "[INFO] [2023-12-18 10:41:17.365] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:17.365] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:17.365] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:17.371] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:17.371] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:17.371] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:17.371] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:17.371] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:17.371] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:17.371] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:17.371] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:17.371] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:17.371] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:17.371] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:17.371] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:17.371] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:17.371] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:17.371] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:17.371] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:17.371] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:17.371] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:17.371] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:17.371] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name 'WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.' encountered in URDF, creating an unnamed geom.\n", + "\n", + "WARNING: Geom with duplicate name 'WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.WARNING: Geom with duplicate name '\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.WARNING: Geom with duplicate name '\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.WARNING: Geom with duplicate name '\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.WARNING: Geom with duplicate name '\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.WARNING: Geom with duplicate name '\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.WARNING: Geom with duplicate name '\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.WARNING: Geom with duplicate name '\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name 'WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.' encountered in URDF, creating an unnamed geom.\n", + "\n", + "WARNING: Geom with duplicate name 'WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.' encountered in URDF, creating an unnamed geom.\n", + "\n", + "WARNING: Geom with duplicate name 'WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.' encountered in URDF, creating an unnamed geom.\n", + "\n", + "WARNING: Geom with duplicate name 'WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.' encountered in URDF, creating an unnamed geom.\n", + "\n", + "WARNING: Geom with duplicate name 'WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.' encountered in URDF, creating an unnamed geom.\n", + "\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.WARNING: Geom with duplicate name '\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name 'WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[[DEBUGDEBUG] [2023-12-18 10:41:17.445] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.] [2023-12-18 10:41:17.445] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "\n", + "[[INFOINFO] ] [2023-12-18 10:41:17.445] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.[2023-12-18 10:41:17.445] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "\n", + "[DEBUG] [[2023-12-18 10:41:17.445] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.DEBUG\n", + "] [2023-12-18 10:41:17.445] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [[2023-12-18 10:41:17.445] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.INFO\n", + "] [2023-12-18 10:41:17.445] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:17.881] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:17.890] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:17.900] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:17.903] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:17.909] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:17.914] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:17.918] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:17.924] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:17.930] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.126] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.139] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.158] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.251] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.409] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.427] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.439] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.592] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.602] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.614] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.622] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.628] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.631] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.642] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.815] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:18.829] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.058] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.151] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.164] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.176] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.185] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.194] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:19.243] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.252] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING:drake:Rigid (non-deformable) half spaces are not currently supported for deformable contact; registration is allowed, but no contact data will be reported.\n", + "[ERROR] [2023-12-18 10:41:19.397] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:19.340] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:19.340] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:19.340] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:19.340] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:19.340] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:19.340] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:19.340] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:19.340] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:19.350] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:19.350] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:19.350] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:19.350] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:19.350] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:19.350] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:19.350] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:19.350] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:19.350] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:19.350] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:19.431] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:19.431] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:19.431] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:19.432] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:19.559] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.573] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.779] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.796] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.809] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.810] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.818] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.825] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.827] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.841] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.866] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:19.888] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:19.797] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:19.999] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.012] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:20.079] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.092] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.105] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING:drake:Rigid (non-deformable) half spaces are not currently supported for deformable contact; registration is allowed, but no contact data will be reported.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:20.152] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:20.152] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:20.153] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:20.153] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:20.153] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:20.153] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:20.153] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:20.153] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:20.159] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:20.159] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:20.159] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:20.159] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:20.159] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:20.159] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:20.159] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:20.159] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:20.159] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:20.159] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:20.232] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:20.232] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:20.232] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:20.232] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:20.233] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:20.233] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:20.233] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:20.233] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:20.233] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:20.233] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:20.233] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:20.233] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:20.239] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:20.239] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:20.239] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:20.239] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:20.239] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:20.239] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:20.239] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:20.239] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:20.239] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:20.239] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:20.339] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:20.340] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:20.340] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:20.340] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[ERROR] [2023-12-18 10:41:20.618] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.669] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.690] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.706] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.709] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.716] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.725] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.727] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.737] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.752] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.767] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.881] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.900] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.918] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.949] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.967] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.979] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:20.996] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.005] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.016] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.027] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.218] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.229] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.231] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.244] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.270] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.293] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.312] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.330] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.348] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:21.401] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[ERROR] [2023-12-18 10:41:21.531] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.540] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.549] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.557] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.568] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.577] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.586] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.594] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.604] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:21.510] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:21.510] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:21.510] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:21.510] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:21.510] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:21.510] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:21.510] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:21.510] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:21.521] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:21.521] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:21.521] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:21.521] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:21.521] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:21.521] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:21.521] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:21.521] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:21.521] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:21.521] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:21.594] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:21.594] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:21.594] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:21.594] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:21.766] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:21.767] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:21.767] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:21.767] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:21.767] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:21.767] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:21.767] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:21.767] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:21.777] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:21.777] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:21.777] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:21.777] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:21.777] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:21.777] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:21.777] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:21.777] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:21.777] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:21.777] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:21.878] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:21.878] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:21.878] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:21.878] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:21.896] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.910] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:21.924] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.230] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.256] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.277] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.296] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.336] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.351] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.363] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:22.316] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:22.547] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.571] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.574] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.580] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.586] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.591] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.600] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.617] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:22.635] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:22.722] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:22.561] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:22.816] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:22.821] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:22.821] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:22.821] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:22.821] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:22.821] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:22.821] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:22.821] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:22.837] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:22.837] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:22.837] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:22.837] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:22.837] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:22.837] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:22.837] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:22.837] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:22.837] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:22.837] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:22.928] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:22.928] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:22.929] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:22.929] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:23.015] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:23.029] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:23.043] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:23.240] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:23.255] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:23.499] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:23.508] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:23.518] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:23.718] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:23.732] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:23.901] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:23.901] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:23.901] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:23.901] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:23.901] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:23.902] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:23.902] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:23.902] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:23.912] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:23.912] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:23.912] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:23.912] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:23.912] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:23.913] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:23.913] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:23.913] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:23.913] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:23.913] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:24.009] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:24.009] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:24.009] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:24.009] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:25.390] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:25.804] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:26.228] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:26.250] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:26.424] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:26.441] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:26.452] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:26.462] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:26.475] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:MultibodyPlant's discrete update solver failed to converge at simulation time = 1.068 with discrete update period = 0.001. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:\n", + " 1. reduce the discrete update period set at construction,\n", + " 2. decrease the high gains in your controller whenever possible,\n", + " 3. switch to a continuous model (discrete update period is zero), though this might affect the simulation run time.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:26.653] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:26.653] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:26.653] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:26.653] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:26.654] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:26.654] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:26.654] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:26.654] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:26.660] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:26.660] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:26.660] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:26.660] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:26.660] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:26.660] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:26.660] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:26.660] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:26.660] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:26.660] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:26.744] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:26.744] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:26.744] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:26.744] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:27.264] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:27.287] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:27.309] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:27.322] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:27.331] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:27.344] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:27.588] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:27.588] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:27.588] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:27.588] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:27.588] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:27.588] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:27.588] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:27.588] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:27.594] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:27.594] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:27.594] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:27.594] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:27.594] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:27.594] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:27.594] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:27.594] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:27.594] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:27.594] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:27.687] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:27.687] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:27.687] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:27.687] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:30.241] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "Function solver (0x55df8da98fc0)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, nan, -nan, -nan, 0.0408154, -0.130473, 0.0164051, 0.0673407, 0.0149911, -0.000329873, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [nan, -nan, -nan, 0.0408154, -0.130473, 0.0164051, 0.0673407, 0.0149911, -0.000329873, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [nan, -nan, -nan, 0.0408154, -0.130473, 0.0164051, 0.0673407, 0.0149911, -0.000329873, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:41:30.253] [thread: 43815] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = nan and UBG[0] = nan..\n", + "ERROR:root:MPC failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:30.424] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:30.424] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:30.424] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:30.424] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:30.424] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:30.424] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:30.424] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:30.424] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:30.434] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:30.434] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:30.434] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:30.434] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:30.434] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:30.434] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:30.434] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:30.435] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:30.435] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:30.435] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:30.544] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:30.544] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:30.544] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:30.544] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:30.866] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:30.876] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:30.887] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.049] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.069] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.086] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.231] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.240] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.251] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.264] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.424] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.583] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.595] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.613] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.753] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.918] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.932] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.952] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:31.970] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.117] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.132] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.146] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.160] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.174] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:32.314] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:32.315] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:32.315] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:32.315] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:32.315] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:32.315] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:32.315] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:32.315] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:32.326] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:32.326] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:32.326] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:32.326] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:32.326] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:32.326] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:32.326] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:32.326] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:32.326] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:32.326] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:32.414] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:32.414] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:32.414] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:32.414] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:32.720] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.747] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.761] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.774] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.791] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.805] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.818] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:32.997] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:33.015] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:33.028] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:33.030] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:33.042] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:33.063] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:33.084] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:33.234] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[ERROR] [2023-12-18 10:41:33.248] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:33.262] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:33.278] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:33.293] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:33.265] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:33.265] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:33.265] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:33.273] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:33.274] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:33.274] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:33.274] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:33.274] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:33.285] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:33.285] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:33.285] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:33.285] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:33.285] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:33.285] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:33.285] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:33.285] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:33.285] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:33.285] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:33.377] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:33.377] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:33.377] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:33.377] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:33.439] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:33.439] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:33.439] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:33.439] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:33.439] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:33.439] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:33.439] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:33.439] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:33.450] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:33.450] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:33.450] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:33.450] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:33.450] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:33.450] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:33.450] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:33.451] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:33.451] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:33.451] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:33.550] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:33.550] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:33.551] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:33.551] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:35.520] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:35.532] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:35.546] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:35.561] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:35.575] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:35.758] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:35.758] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:35.758] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:35.759] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:35.759] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:35.759] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:35.759] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:35.759] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:35.770] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:35.770] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:35.770] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:35.770] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:35.770] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:35.771] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:35.771] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:35.771] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:35.771] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:35.771] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:35.847] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:35.847] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:35.847] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:35.847] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "ERROR:root:MultibodyPlant's discrete update solver failed to converge at simulation time = 1.061 with discrete update period = 0.001. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:\n", + " 1. reduce the discrete update period set at construction,\n", + " 2. decrease the high gains in your controller whenever possible,\n", + " 3. switch to a continuous model (discrete update period is zero), though this might affect the simulation run time.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:36.505] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:36.506] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:36.507] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:36.508] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:36.509] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:36.509] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:36.509] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:36.509] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:36.522] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:36.522] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:36.522] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:36.522] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:36.522] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:36.522] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:36.522] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:36.522] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:36.522] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:36.522] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:36.608] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:36.608] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:36.608] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:36.608] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:37.102] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:37.113] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:37.282] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + " 2 5 -3.073 2341.38 2.42 0.471766\n", + "[DEBUG] [2023-12-18 10:41:38.638] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:38.638] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:38.638] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:38.638] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:38.638] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:38.638] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:38.638] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:38.638] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:41:38.441] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:41:38.441] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:41:38.447] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:41:38.447] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:41:38.453] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:41:38.453] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:41:38.459] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:41:38.459] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55df8c075580)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, -nan, -nan, -nan, 0.0139716, -0.00481405, 0.00350012, -0.00385782, -0.00198442, 4.35728e-05, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [-nan, -nan, -nan, 0.0139716, -0.00481405, 0.00350012, -0.00385782, -0.00198442, 4.35728e-05, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [-nan, -nan, -nan, 0.0139716, -0.00481405, 0.00350012, -0.00385782, -0.00198442, 4.35728e-05, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:41:38.481] [thread: 43815] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = -nan and UBG[0] = -nan..\n", + "ERROR:root:MPC failed\n", + "[ERROR] [2023-12-18 10:41:38.522] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:38.645] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:38.645] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:38.645] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:38.645] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:38.645] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:38.645] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:38.645] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:38.645] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:38.645] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:38.645] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:38.738] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:38.738] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:38.738] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:38.738] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:39.055] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.143] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.173] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.205] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.225] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.449] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.458] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.467] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.477] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.491] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:39.665] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:39.665] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:39.665] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:39.665] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:39.665] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:39.665] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:39.666] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:39.666] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:39.677] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:39.677] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:39.677] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:39.677] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:39.677] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:39.677] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:39.677] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:39.677] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:39.677] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:39.677] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:39.787] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:39.787] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:39.787] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:39.787] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:39.845] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.860] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.874] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.889] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:39.903] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:40.093] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:40.093] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:40.093] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:40.093] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:40.093] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:40.093] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:40.093] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:40.093] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:40.100] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:40.100] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:40.100] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:40.100] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:40.100] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:40.100] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:40.100] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:40.100] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:40.100] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:40.100] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:40.187] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:40.187] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:40.187] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:40.187] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:40.930] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:40.940] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:40.951] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:40.961] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:41.285] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:41.295] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:41.304] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:41.530] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:41.541] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:41.551] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:41.560] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:41.571] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:41.732] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:41.732] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:41.732] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:41.732] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:41.732] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:41.732] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:41.732] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:41.732] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:41.739] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:41.739] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:41.739] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:41.739] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:41.739] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:41.739] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:41.739] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:41.739] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:41.739] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:41.739] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:41.841] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:41.841] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:41.841] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:41.841] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:42.276] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.286] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.298] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.309] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.746] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.761] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.837] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.857] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.877] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.903] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.918] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.936] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:42.958] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:43.131] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[ERROR] [2023-12-18 10:41:43.339] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:43.352] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:43.361] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:43.371] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:43.306] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:43.306] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:43.306] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:43.306] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:43.306] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:43.306] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:43.306] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:43.306] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:43.312] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:43.312] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:43.312] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:43.312] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:43.312] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:43.312] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:43.312] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:43.312] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:43.312] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:43.312] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:43.388] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:43.388] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:43.388] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:43.388] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:43.710] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:43.719] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:43.730] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:43.745] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:43.880] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:44.023] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:44.038] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:44.053] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:44.068] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:44.082] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:44.141] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:44.150] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:44.159] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:44.268] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:44.268] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:44.268] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:44.268] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:44.268] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:44.268] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:44.268] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:44.268] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:44.275] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:44.275] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:44.275] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:44.275] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:44.275] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:44.275] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:44.275] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:44.275] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:44.275] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:44.275] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:44.348] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:44.348] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:44.348] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:44.348] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:44.457] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:44.457] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:44.457] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:44.457] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:44.458] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:44.458] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:44.458] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:44.458] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:44.469] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:44.469] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:44.469] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:44.469] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:44.469] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:44.469] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:44.469] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:44.469] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:44.469] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:44.469] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:44.294] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:44.303] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:44.576] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:44.576] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:44.576] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:44.576] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:45.041] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.071] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.092] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.261] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.286] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.319] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.351] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.369] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.391] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:45.332] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:45.586] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.602] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.613] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.624] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.635] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.673] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.696] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.728] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.742] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.752] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:45.754] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + " 2 5 -4.103 2335.07 3.42 0.470494\n", + "[DEBUG] [2023-12-18 10:41:45.934] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:45.934] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:45.934] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:45.934] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:45.934] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:45.934] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:45.934] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:45.934] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:45.940] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:45.940] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:45.940] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:45.940] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:45.940] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:45.940] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:45.940] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:45.940] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:45.940] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:45.940] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:45.904] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.917] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[ERROR] [2023-12-18 10:41:45.934] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.947] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.962] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.973] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:45.993] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:46.020] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:46.020] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:46.020] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:46.020] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:46.192] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.212] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.239] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.250] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.261] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.275] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.290] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.308] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.449] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.452] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.460] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.461] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.469] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.471] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.479] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.481] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.504] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.522] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.540] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.678] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.687] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.696] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.705] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.714] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:46.728] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.738] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.749] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.760] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.773] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.784] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.795] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.812] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:46.846] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:46.846] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:46.847] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:46.847] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:46.847] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:46.847] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:46.847] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:46.847] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:46.858] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:46.859] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:46.859] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:46.859] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:46.859] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:46.859] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:46.859] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:46.859] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:46.859] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:46.859] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:46.953] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:46.953] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:46.953] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:46.953] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:46.949] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.962] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.978] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:46.995] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:47.013] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:47.034] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:47.052] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:47.069] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:47.243] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:47.260] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:47.444] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:47.451] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:47.451] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:47.451] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:47.451] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:47.451] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:47.451] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:47.451] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:47.473] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:47.473] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:47.473] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:47.473] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:47.473] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:47.473] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:47.473] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:47.473] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:47.473] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:47.473] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:47.574] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:47.574] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:47.575] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:47.575] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:48.088] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:48.097] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:48.110] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:48.140] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:48.170] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:48.206] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:48.229] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:48.405] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[ERROR] [2023-12-18 10:41:48.559] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:48.572] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:48.582] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:48.473] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:48.479] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:48.479] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:48.479] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:48.479] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:48.479] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:48.479] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:48.479] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:48.501] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:48.501] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:48.501] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:48.501] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:48.501] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:48.501] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:48.501] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:48.501] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:48.501] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:48.501] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:48.601] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:48.601] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:48.601] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:48.601] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:48.731] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:48.888] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:48.906] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.044] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.053] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.069] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.070] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.083] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.104] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.242] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.257] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.304] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.320] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.335] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.414] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.439] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.458] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.563] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.580] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.601] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.619] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.628] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.638] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.652] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.667] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.681] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.696] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:49.762] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.771] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:49.780] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:49.828] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:49.828] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:49.828] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:49.828] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:49.828] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:49.828] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:49.828] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:49.828] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:49.834] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:49.834] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:49.834] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:49.834] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:49.834] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:49.834] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:49.834] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:49.834] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:49.834] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:49.834] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:49.900] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:49.900] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:49.900] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:49.900] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:49.900] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:49.900] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:49.900] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:49.900] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:49.906] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:49.906] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:49.906] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:49.906] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:49.906] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:49.906] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:49.906] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:49.906] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:49.906] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:49.906] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:49.908] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:49.908] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:49.908] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:49.908] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:50.006] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:50.007] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:50.007] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:50.007] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:50.342] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.371] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.394] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.424] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.441] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.445] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.454] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.462] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.494] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.544] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.595] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.603] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.609] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.622] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.639] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.659] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:50.681] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:50.798] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:MultibodyPlant's discrete update solver failed to converge at simulation time = 0.10200000000000001 with discrete update period = 0.001. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:\n", + " 1. reduce the discrete update period set at construction,\n", + " 2. decrease the high gains in your controller whenever possible,\n", + " 3. switch to a continuous model (discrete update period is zero), though this might affect the simulation run time.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:50.838] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:50.838] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:50.838] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:50.838] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:50.838] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:50.838] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:50.838] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:50.838] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:50.845] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:50.845] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:50.845] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:50.845] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:50.845] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:50.845] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:50.845] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:50.845] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:50.845] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:50.845] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:50.920] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:50.920] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:50.920] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:50.920] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:50.946] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:50.946] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:50.947] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:50.947] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:50.947] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:50.947] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:50.947] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:50.947] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:50.957] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:50.957] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:50.957] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:50.957] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:50.957] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:50.957] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:50.957] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:50.957] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:50.957] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:50.957] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:51.062] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:51.062] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:51.062] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:51.062] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:51.235] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:51.244] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:51.258] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:51.275] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:51.483] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:51.555] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:51.577] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:51.854] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:51.863] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:51.877] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:51.892] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.019] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.035] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.179] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.194] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.213] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.219] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.386] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.403] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.571] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.578] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.580] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:52.567] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:52.587] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.596] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.766] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.918] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.925] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.933] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.950] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.964] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.980] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:52.996] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.181] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.203] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.219] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.374] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.388] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.402] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.412] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.421] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:53.450] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.461] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.470] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:53.603] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:53.603] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:53.603] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:53.603] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:53.603] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:53.603] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:53.603] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:53.603] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:53.609] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:53.609] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:53.609] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:53.609] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:53.609] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:53.609] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:53.609] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:53.609] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:53.609] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:53.609] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:53.705] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:53.705] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:53.706] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:53.706] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:53.623] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:53.820] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:54.055] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:54.077] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:54.327] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:54.806] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:54.824] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:54.839] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:55.113] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:55.128] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:55.143] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:55.298] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:55.298] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:55.298] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:55.298] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:55.299] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:55.299] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:55.299] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:55.299] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:55.310] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:55.310] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:55.310] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:55.310] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:55.310] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:55.310] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:55.310] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:55.310] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:55.310] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:55.310] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:55.385] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:55.385] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:55.385] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:55.385] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:55.893] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:55.921] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:55.944] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:55.966] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:55.989] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.003] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.016] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.186] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.204] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.218] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.231] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.246] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.259] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:56.425] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[ERROR] [2023-12-18 10:41:56.443] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.454] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.473] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:56.476] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:41:56.482] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:41:56.482] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55df8c880bc0)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, -nan, -nan, -nan, 0.00823112, -0.00275592, 0.00548778, -0.00150407, -0.00312834, 4.51725e-05, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [-nan, -nan, -nan, 0.00823112, -0.00275592, 0.00548778, -0.00150407, -0.00312834, 4.51725e-05, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [-nan, -nan, -nan, 0.00823112, -0.00275592, 0.00548778, -0.00150407, -0.00312834, 4.51725e-05, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:41:56.492] [thread: 43815] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = -nan and UBG[0] = -nan..\n", + "ERROR:root:MPC failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:56.438] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:56.438] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:56.438] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:56.438] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:56.439] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:56.439] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:56.439] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:56.439] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:56.445] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:56.445] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:56.445] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:56.445] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:56.445] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:56.445] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:56.445] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:56.445] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:56.445] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:56.445] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "[DEBUG] [2023-12-18 10:41:56.519] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:56.519] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:56.519] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:56.519] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[ERROR] [2023-12-18 10:41:56.832] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:56.660] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:56.660] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:56.661] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:56.661] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:56.661] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:56.661] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:56.661] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:56.661] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:56.667] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:56.667] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:56.667] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:56.667] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:56.667] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:56.667] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:56.667] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:56.667] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:56.667] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:56.667] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:56.772] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:56.772] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:56.772] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:56.773] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:56.847] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.082] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.247] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.263] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.283] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.296] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.308] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.494] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.496] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.514] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.716] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.725] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.727] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.733] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.737] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.742] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:57.745] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.902] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.911] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.920] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.929] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:57.938] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:57.931] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:57.931] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:57.931] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:57.931] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:57.931] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:57.931] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:57.931] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:57.931] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:57.938] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:57.938] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:57.938] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:57.938] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:57.938] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:57.938] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:57.938] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:57.938] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:57.938] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:57.938] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:58.011] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:58.011] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:58.011] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:58.011] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:58.081] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:58.081] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:58.081] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:58.081] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:58.081] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:58.081] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:58.081] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:58.081] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:58.092] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:58.092] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:58.093] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:58.093] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:58.093] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:58.093] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:58.093] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:58.093] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:58.093] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:58.093] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:58.199] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:58.199] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:58.199] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:58.199] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:58.355] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.384] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.407] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.426] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.447] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.628] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.646] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.658] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.670] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.673] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.687] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.690] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.703] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.707] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.715] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.720] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.745] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.768] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:58.789] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:41:58.968] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:58.904] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.926] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.946] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.985] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.993] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:58.997] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.007] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.009] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.020] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.022] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.033] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:41:59.053] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.072] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.086] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.099] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + " 3 10 -2.163 2285.2 1.63 0.447016\n", + "[DEBUG] [2023-12-18 10:41:59.177] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:59.177] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:59.177] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:59.177] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:59.178] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:59.178] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:59.178] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:59.178] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:59.189] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:59.189] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:59.189] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:59.189] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:59.189] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:59.189] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:59.189] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:59.189] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:59.189] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:59.189] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:59.117] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[ERROR] [2023-12-18 10:41:59.259] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.272] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.285] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.312] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:59.284] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:59.284] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:41:59.284] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:41:59.284] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:41:59.336] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.358] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.379] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.409] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.569] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.592] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.614] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.652] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:41:59.692] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:MultibodyPlant's discrete update solver failed to converge at simulation time = 0.454 with discrete update period = 0.001. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:\n", + " 1. reduce the discrete update period set at construction,\n", + " 2. decrease the high gains in your controller whenever possible,\n", + " 3. switch to a continuous model (discrete update period is zero), though this might affect the simulation run time.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:41:59.917] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:59.919] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:59.920] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:59.921] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:59.922] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:59.923] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:59.924] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:41:59.925] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:41:59.937] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:41:59.938] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:41:59.939] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:41:59.940] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:41:59.941] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:41:59.942] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:41:59.942] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:59.943] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:41:59.944] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:41:59.945] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:00.054] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:00.054] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:00.054] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:00.054] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:00.543] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:00.559] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:00.572] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:00.586] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:00.595] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:00.819] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:00.834] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:00.854] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:01.121] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:01.139] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:01.154] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:01.168] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:01.179] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:01.366] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:01.375] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:01.384] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:01.589] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:01.598] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:01.751] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:01.751] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:01.751] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:01.751] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:01.751] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:01.751] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:01.751] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:01.751] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:01.758] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:01.758] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:01.758] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:01.758] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:01.758] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:01.758] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:01.758] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:01.758] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:01.758] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:01.758] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:01.864] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:01.864] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:01.864] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:01.864] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:02.343] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:02.375] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:02.398] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:02.412] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:02.425] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:02.443] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:02.464] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:02.727] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:02.750] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:02.773] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + " 3 10 -1.883 2274.23 1.38 0.444913\n", + "[DEBUG] [2023-12-18 10:42:02.965] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:02.969] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:02.969] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:02.969] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:02.969] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:02.970] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:02.970] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:02.970] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:02.993] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:02.993] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:02.993] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:02.993] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:02.993] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:02.993] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:02.993] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:02.993] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:02.993] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:02.993] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:03.079] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:03.079] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:03.080] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:03.080] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:03.682] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:03.701] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:03.750] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:03.765] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:03.692] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:03.911] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:03.921] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:03.930] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:03.939] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:04.078] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:04.242] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:04.242] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:04.242] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:04.242] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:04.242] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:04.242] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:04.242] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:04.242] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:04.253] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:04.253] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:04.253] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:04.254] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:04.254] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:04.254] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:04.254] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:04.254] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:04.254] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:04.254] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:04.354] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:04.354] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:04.354] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:04.354] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:04.384] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:04.399] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:04.414] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:04.428] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:04.444] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:04.459] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:04.640] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:04.640] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:04.640] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:04.640] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:04.641] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:04.641] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:04.641] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:04.641] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:04.647] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:04.647] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:04.647] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:04.647] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:04.647] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:04.647] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:04.647] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:04.647] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:04.647] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:04.647] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:04.716] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:42:04.720] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:04.720] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:04.720] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:04.720] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:04.642] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:04.660] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:04.673] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:04.691] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:04.703] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:04.740] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:05.037] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:05.079] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:05.141] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:05.170] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:05.183] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:05.197] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:05.211] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:05.383] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:05.383] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:05.383] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:05.383] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:05.384] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:05.384] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:05.384] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:05.384] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:05.393] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:05.393] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:05.393] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:05.393] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:05.393] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:05.393] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:05.393] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:05.393] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:05.393] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:05.393] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:05.507] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:05.508] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:05.508] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:05.508] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:05.959] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:05.971] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:05.982] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:05.991] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:06.218] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:06.227] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:06.236] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:06.244] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:06.253] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:06.406] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:06.406] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:06.406] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:06.406] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:06.406] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:06.406] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:06.406] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:06.406] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:06.412] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:06.412] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:06.412] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:06.412] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:06.412] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:06.412] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:06.412] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:06.412] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:06.412] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:06.412] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:06.509] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:06.509] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:06.509] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:06.509] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:06.890] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:06.907] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:06.946] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:07.001] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:07.017] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:07.032] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:42:07.237] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:07.237] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:07.241] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:07.241] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:07.245] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:07.245] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:07.249] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:07.249] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55df8dfb9ee0)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, -nan, -nan, -nan, 0.0452941, -0.12967, 0.0176223, 0.0666103, 0.0173785, -0.000368914, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [-nan, -nan, -nan, 0.0452941, -0.12967, 0.0176223, 0.0666103, 0.0173785, -0.000368914, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [-nan, -nan, -nan, 0.0452941, -0.12967, 0.0176223, 0.0666103, 0.0173785, -0.000368914, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:42:07.259] [thread: 43815] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = -nan and UBG[0] = -nan..\n", + "ERROR:root:MPC failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:07.194] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:07.194] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:07.194] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:07.194] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:07.195] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:07.195] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:07.195] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:07.195] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:07.201] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:07.201] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:07.201] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:07.201] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:07.201] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:07.201] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:07.201] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:07.201] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:07.201] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:07.201] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "[DEBUG] [2023-12-18 10:42:07.274] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:07.274] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:07.274] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:07.274] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:07.431] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:07.432] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:07.432] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:07.432] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:07.432] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:07.432] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:07.432] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:07.432] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:07.439] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:07.439] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:07.439] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:07.439] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:07.439] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:07.439] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:07.439] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:07.439] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:07.439] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:07.439] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:07.552] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:07.552] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:07.552] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:07.552] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:08.147] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:08.626] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:09.017] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:09.036] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:09.370] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:09.784] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:09.802] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:09.964] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:10.152] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:10.529] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:11.789] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:11.798] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:11.808] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:11.810] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:11.816] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:11.819] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:11.828] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:11.843] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:11.856] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:11.801] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:42:11.831] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:12.081] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:12.090] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:42:12.111] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:12.120] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:12.128] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:12.242] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:12.242] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:12.242] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:12.242] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:12.242] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:12.242] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:12.242] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:12.242] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:12.248] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:12.248] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:12.248] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:12.248] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:12.248] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:12.248] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:12.248] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:12.248] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:12.248] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:12.248] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:12.249] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:12.249] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:12.249] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:12.249] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:12.249] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:12.249] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:12.249] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:12.249] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:12.256] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:12.256] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:12.256] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:12.256] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:12.256] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:12.256] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:12.256] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:12.256] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:12.256] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:12.256] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:12.322] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:12.322] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:12.322] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:12.322] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:12.329] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:12.329] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:12.329] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:12.329] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:12.812] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:12.843] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:12.875] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.073] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.097] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.126] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.177] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.200] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.213] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:13.149] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:13.415] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.435] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.448] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.462] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.497] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.529] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.563] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:13.719] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:MultibodyPlant's discrete update solver failed to converge at simulation time = 0.316 with discrete update period = 0.001. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:\n", + " 1. reduce the discrete update period set at construction,\n", + " 2. decrease the high gains in your controller whenever possible,\n", + " 3. switch to a continuous model (discrete update period is zero), though this might affect the simulation run time.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:13.914] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:13.914] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:13.914] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:13.914] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:13.914] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:13.914] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:13.914] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:13.914] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:13.922] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:13.922] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:13.922] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:13.922] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:13.922] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:13.922] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:13.922] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:13.922] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:13.922] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:13.922] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:14.008] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:14.008] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:14.008] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:14.008] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:14.433] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:14.449] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:14.475] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:14.492] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:14.685] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:14.700] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:14.716] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:14.731] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:14.747] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:42:14.887] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:42:14.894] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:14.894] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:14.898] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:14.898] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55fea6ee4580)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, nan, -nan, -nan, 0.0406285, -0.130514, 0.0167595, 0.0673749, 0.0148727, -0.000326763, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [nan, -nan, -nan, 0.0406285, -0.130514, 0.0167595, 0.0673749, 0.0148727, -0.000326763, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [nan, -nan, -nan, 0.0406285, -0.130514, 0.0167595, 0.0673749, 0.0148727, -0.000326763, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:42:14.908] [thread: 43818] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = nan and UBG[0] = nan..\n", + "ERROR:root:MPC failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "[DEBUG] [2023-12-18 10:42:14.931] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:14.931] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:14.931] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:14.931] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:14.931] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:14.931] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:14.931] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:14.931] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:14.937] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:14.937] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:14.937] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:14.937] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:14.937] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:14.937] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:14.937] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:14.937] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:14.937] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:14.937] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:15.011] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:15.011] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:15.011] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:15.012] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:15.044] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:15.044] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:15.044] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:15.044] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:15.044] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:15.044] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:15.044] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:15.044] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:15.056] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:15.056] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:15.056] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:15.056] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:15.056] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:15.056] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:15.056] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:15.056] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:15.056] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:15.056] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:15.164] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:15.164] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:15.164] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:15.164] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:15.368] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:15.384] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:15.396] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:15.413] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:15.433] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:15.452] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:15.643] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:15.643] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:15.643] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:15.643] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:15.644] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:15.644] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:15.644] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:15.644] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:15.655] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:15.656] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:15.656] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:15.656] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:15.656] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:15.656] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:15.656] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:15.656] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:15.656] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:15.656] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:15.754] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:15.754] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:15.754] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:15.754] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:16.219] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.228] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.239] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.251] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.263] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.426] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.440] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.457] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.472] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.493] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.633] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.848] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.857] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.866] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.875] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:16.883] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:17.039] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:17.039] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:17.039] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:17.039] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:17.039] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:17.039] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:17.039] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:17.039] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:17.047] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:17.047] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:17.047] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:17.047] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:17.047] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:17.047] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:17.047] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:17.047] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:17.047] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:17.048] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:17.161] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:17.163] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:17.163] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:17.163] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:17.591] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:17.619] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:17.646] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:17.672] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:17.699] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:17.722] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:17.887] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:17.887] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:17.887] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:17.887] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:17.887] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:17.887] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:17.887] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:17.887] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:17.893] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:17.893] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:17.893] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:17.893] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:17.893] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:17.893] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:17.893] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:17.893] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:17.893] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:17.893] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:17.995] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:17.995] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:17.995] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:17.995] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:19.779] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:42:19.794] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:19.809] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:19.824] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:19.839] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:19.849] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:19.858] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:20.016] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:20.016] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:20.016] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:20.016] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:20.016] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:20.016] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:20.016] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:20.016] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:20.023] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:20.023] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:20.023] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:20.023] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:20.023] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:20.023] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:20.023] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:20.023] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:20.023] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:20.023] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:20.130] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:20.130] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:20.130] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:20.130] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:22.631] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:22.640] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:22.880] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:22.889] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:22.903] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:23.062] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:23.062] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:23.062] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:23.062] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:23.062] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:23.062] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:23.062] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:23.062] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:23.074] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:23.075] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:23.075] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:23.075] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:23.075] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:23.075] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:23.075] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:23.075] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:23.075] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:23.075] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:23.190] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:23.190] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:23.190] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:23.190] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "[DEBUG] [2023-12-18 10:42:23.556] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:23.556] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:23.556] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:23.556] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:23.556] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:23.556] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:23.556] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:23.556] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:42:23.358] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:23.358] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:23.364] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:23.365] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:23.370] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:23.370] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:23.375] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:23.375] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:23.379] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:23.379] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:23.567] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:23.567] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:23.567] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:23.567] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:23.567] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:23.567] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:23.567] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:23.567] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:23.567] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:23.567] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:23.681] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:23.681] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:23.681] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:23.681] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + " 4 15 -3.473 2348.06 3.25 0.424943\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:42:25.794] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:25.794] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:25.800] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:25.800] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55df8c6043e0)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, -nan, -nan, -nan, 0.0406996, -0.131289, 0.0162444, 0.0678452, 0.0149105, -0.000327601, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [-nan, -nan, -nan, 0.0406996, -0.131289, 0.0162444, 0.0678452, 0.0149105, -0.000327601, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [-nan, -nan, -nan, 0.0406996, -0.131289, 0.0162444, 0.0678452, 0.0149105, -0.000327601, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:42:25.850] [thread: 43815] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = -nan and UBG[0] = -nan..\n", + "ERROR:root:MPC failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:26.031] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:26.031] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:26.031] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:26.031] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:26.031] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:26.031] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:26.031] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:26.031] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:26.038] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:26.038] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:26.038] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:26.038] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:26.038] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:26.038] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:26.038] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:26.038] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:26.038] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:26.038] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:26.134] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:26.134] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:26.134] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:26.134] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:27.113] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:27.191] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:27.264] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:27.274] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:27.284] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:27.254] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:42:27.295] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:27.633] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:27.642] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:27.796] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:27.811] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:27.826] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:27.841] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.461] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.476] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.628] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.637] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.645] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.654] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.663] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:42:28.663] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.672] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.681] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.690] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:28.699] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:28.785] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:28.785] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:28.785] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:28.785] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:28.785] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:28.785] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:28.785] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:28.785] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:28.791] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:28.791] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:28.791] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:28.791] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:28.791] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:28.791] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:28.791] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:28.791] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:28.791] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:28.791] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:28.820] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:28.820] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:28.820] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:28.820] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:28.821] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:28.821] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:28.821] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:28.821] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:28.827] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:28.827] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:28.827] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:28.827] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:28.827] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:28.827] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:28.827] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:28.827] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:28.827] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:28.827] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:28.866] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:28.866] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:28.866] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:28.866] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:28.912] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:28.912] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:28.913] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:28.913] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "[DEBUG] [2023-12-18 10:42:31.588] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:31.588] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:31.588] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:31.588] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:31.588] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:31.588] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:31.588] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:31.588] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:31.595] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:31.595] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:31.595] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:31.595] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:31.595] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:31.595] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:31.595] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:31.595] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:31.595] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:31.595] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:42:31.412] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:31.412] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:31.417] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:31.417] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:31.423] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:31.423] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:31.429] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:31.429] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:31.435] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:31.435] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:31.709] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:31.709] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:31.709] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:31.709] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + " 4 15 -3.343 2335.27 3.12 0.422639\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:31.787] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:31.814] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:42:31.828] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:31.828] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:31.834] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:31.834] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:31.840] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:31.840] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55fea3cd6cb0)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, -nan, -nan, -nan, 0.0126363, -0.00438329, 0.0110752, -0.00137498, -0.000973271, 1.47881e-05, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [-nan, -nan, -nan, 0.0126363, -0.00438329, 0.0110752, -0.00137498, -0.000973271, 1.47881e-05, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [-nan, -nan, -nan, 0.0126363, -0.00438329, 0.0110752, -0.00137498, -0.000973271, 1.47881e-05, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:42:31.856] [thread: 43818] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = -nan and UBG[0] = -nan..\n", + "ERROR:root:MPC failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:32.035] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:32.035] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:32.035] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:32.035] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:32.035] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:32.035] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:32.035] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:32.035] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:32.047] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:32.047] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:32.047] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:32.047] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:32.047] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:32.047] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:32.047] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:32.047] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:32.047] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:32.047] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:32.156] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:32.156] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:32.156] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:32.156] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:36.157] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:36.378] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:36.396] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:36.496] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:36.506] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:36.516] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:36.525] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:36.534] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:42:36.566] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:36.575] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:36.387] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:42:36.455] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:42:36.481] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:42:36.548] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:42:36.557] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:36.584] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:36.593] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:36.601] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:36.657] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:36.657] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:36.657] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:36.657] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:36.657] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:36.657] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:36.657] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:36.657] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:36.664] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:36.664] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:36.664] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:36.664] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:36.664] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:36.664] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:36.664] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:36.664] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:36.664] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:36.664] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:36.724] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:36.724] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:36.724] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:36.724] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:36.724] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:36.724] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:36.724] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:36.724] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:36.730] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:36.730] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:36.730] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:36.730] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:36.730] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:36.730] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:36.730] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:36.730] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:36.730] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:36.730] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:36.737] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:36.737] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:36.737] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:36.737] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:36.832] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:36.832] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:36.832] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:36.832] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:39.355] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:42:39.378] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:39.378] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55fea6ad9f90)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, nan, -nan, -nan, 0.0126716, -0.00560675, 0.00673091, -0.00209862, -0.000877991, 1.18529e-05, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [nan, -nan, -nan, 0.0126716, -0.00560675, 0.00673091, -0.00209862, -0.000877991, 1.18529e-05, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [nan, -nan, -nan, 0.0126716, -0.00560675, 0.00673091, -0.00209862, -0.000877991, 1.18529e-05, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:42:39.388] [thread: 43818] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = nan and UBG[0] = nan..\n", + "ERROR:root:MPC failed\n", + "[ERROR] [2023-12-18 10:42:39.403] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:42:39.409] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:39.409] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:39.413] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:39.413] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:39.417] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:39.417] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:39.420] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:39.420] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:39.367] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "[DEBUG] [2023-12-18 10:42:39.511] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:39.511] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:39.511] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:39.511] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:39.511] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:39.511] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:39.511] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:39.511] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:39.518] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:39.518] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:39.518] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:39.518] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:39.518] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:39.518] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:39.518] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:39.518] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:39.518] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:39.518] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:39.572] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:39.572] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:39.572] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:39.572] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:39.572] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:39.572] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:39.572] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:39.572] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:39.579] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:39.579] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:39.579] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:39.579] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:39.579] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:39.579] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:39.579] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:39.579] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:39.579] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:39.579] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:39.593] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:39.593] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:39.593] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:39.593] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:39.676] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:39.677] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:39.677] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:39.677] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:40.093] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:40.105] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:40.122] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:40.151] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:40.403] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:40.418] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:40.636] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:40.651] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:40.665] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:40.674] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:40.683] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:40.840] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:40.840] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:40.840] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:40.840] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:40.840] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:40.840] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:40.840] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:40.841] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:40.853] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:40.853] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:40.853] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:40.853] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:40.853] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:40.853] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:40.853] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:40.853] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:40.853] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:40.853] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:40.955] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:40.955] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:40.955] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:40.955] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:41.420] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:41.450] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:41.475] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:41.489] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:41.503] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:41.516] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:41.683] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:41.683] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:41.683] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:41.683] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:41.683] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:41.683] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:41.683] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:41.683] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:41.695] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:41.695] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:41.695] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:41.695] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:41.695] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:41.695] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:41.695] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:41.695] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:41.695] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:41.695] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:41.808] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:41.808] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:41.808] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:41.808] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:42.555] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:43.520] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:43.529] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:43.539] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:43.552] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:43.570] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:43.825] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:43.835] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:43.843] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:43.853] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:43.862] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:44.033] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:44.033] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:44.033] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:44.033] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:44.033] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:44.033] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:44.033] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:44.033] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:44.039] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:44.039] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:44.039] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:44.039] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:44.039] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:44.039] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:44.039] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:44.039] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:44.039] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:44.039] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:44.144] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:44.144] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:44.144] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:44.144] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:46.404] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:46.413] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:46.422] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:46.431] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:46.444] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:46.596] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:46.596] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:46.596] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:46.596] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:46.597] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:46.597] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:46.597] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:46.597] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:46.603] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:46.603] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:46.603] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:46.603] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:46.603] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:46.603] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:46.603] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:46.603] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:46.603] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:46.603] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:46.707] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:46.707] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:46.707] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:46.707] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:47.067] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:47.089] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:47.101] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:47.111] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:47.121] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:47.130] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:47.332] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:47.332] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:47.332] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:47.332] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:47.332] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:47.332] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:47.332] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:47.332] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:47.342] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:47.342] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:47.342] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:47.342] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:47.342] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:47.342] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:47.342] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:47.342] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:47.342] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:47.342] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:47.448] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:47.448] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:47.448] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:47.448] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:49.442] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:49.442] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:49.443] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:49.443] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:49.443] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:49.443] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:49.443] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:49.443] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:49.454] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:49.454] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:49.454] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:49.454] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:49.454] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:49.454] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:49.454] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:49.455] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:49.455] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:49.455] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:49.568] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:49.568] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:49.568] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:49.568] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:51.493] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:51.502] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:51.712] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:51.730] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:51.739] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:51.748] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:51.760] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:51.776] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:51.721] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:42:51.960] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:51.960] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:51.960] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:51.960] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:51.960] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:51.960] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:51.960] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:51.960] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:51.972] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:51.972] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:51.972] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:51.972] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:51.973] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:51.973] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:51.973] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:51.973] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:51.973] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:51.973] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:52.066] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:52.066] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:52.066] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:52.066] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:54.577] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:54.595] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:54.612] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:54.630] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:54.647] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[ERROR] [2023-12-18 10:42:54.870] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:54.879] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:54.888] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:54.809] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:54.809] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:54.809] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:54.809] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:54.809] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:54.809] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:54.809] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:54.809] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:54.816] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:54.816] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:54.816] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:54.816] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:54.816] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:54.816] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:54.816] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:54.816] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:54.816] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:54.816] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:54.891] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:54.891] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:54.891] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:54.891] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:54.897] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:42:55.100] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:55.327] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:55.338] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:55.360] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:55.376] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:55.639] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:55.649] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:55.658] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:55.668] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:55.677] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:42:55.753] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:55.817] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:55.817] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:55.818] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:55.818] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:55.818] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:55.818] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:55.818] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:55.818] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:55.829] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:55.830] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:55.830] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:55.830] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:55.830] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:55.830] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:55.830] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:55.830] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:55.830] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:55.830] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:55.918] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:55.918] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:55.918] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:55.918] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:56.225] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:56.244] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:56.251] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:56.257] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:56.261] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:56.270] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:56.272] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:56.279] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:56.285] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:56.288] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:42:56.298] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:56.409] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:56.409] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:56.409] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:56.409] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:56.410] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:56.410] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:56.410] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:56.410] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:56.416] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:56.416] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:56.416] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:56.416] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:56.416] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:56.416] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:56.416] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:56.416] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:56.416] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:56.416] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:56.425] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:56.425] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:56.426] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:56.426] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:56.426] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:56.426] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:56.426] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:56.426] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:56.432] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:56.432] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:56.432] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:56.432] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:56.432] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:56.432] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:56.432] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:56.432] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:56.432] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:56.432] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:56.490] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:56.490] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:56.490] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:56.490] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:56.509] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:56.509] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:56.509] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:56.509] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:42:58.108] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:58.303] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:58.319] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:58.332] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:42:58.341] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:42:58.488] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:58.488] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:58.488] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:58.488] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:58.489] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:58.489] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:58.489] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:58.489] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:58.501] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:58.501] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:58.501] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:58.501] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:58.501] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:58.501] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:58.501] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:58.501] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:58.501] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:58.501] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:58.583] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:58.583] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:58.583] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:58.583] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:42:59.362] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:59.362] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:42:59.366] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:42:59.366] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55df8ddfd090)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, nan, -nan, -nan, 0.0356175, -0.0574283, 0.00677392, 0.0170424, 0.00966186, -0.000151797, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [nan, -nan, -nan, 0.0356175, -0.0574283, 0.00677392, 0.0170424, 0.00966186, -0.000151797, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [nan, -nan, -nan, 0.0356175, -0.0574283, 0.00677392, 0.0170424, 0.00966186, -0.000151797, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:42:59.376] [thread: 43815] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = nan and UBG[0] = nan..\n", + "ERROR:root:MPC failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + " 5 20 -4.143 1961.02 3.8 0.400111\n", + "[DEBUG] [2023-12-18 10:42:59.541] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:59.541] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:59.541] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:59.541] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:59.541] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:59.541] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:59.541] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:42:59.541] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:42:59.548] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:42:59.548] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:42:59.548] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:42:59.548] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:42:59.548] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:42:59.548] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:42:59.548] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:59.548] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:59.548] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:42:59.548] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:42:59.637] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:59.637] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:42:59.637] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:42:59.637] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:01.160] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:01.176] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:01.192] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:01.208] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:01.224] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + " 5 20 -3.173 1972.18 2.96 0.402366\n", + "[DEBUG] [2023-12-18 10:43:01.388] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:01.388] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:01.388] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:01.388] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:01.388] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:01.388] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:01.388] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:01.388] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:01.395] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:01.395] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:01.395] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:01.395] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:01.395] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:01.395] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:01.395] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:01.395] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:01.395] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:01.395] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:01.535] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:01.535] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:01.535] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:01.535] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:01.986] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:01.996] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:02.011] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:02.030] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:02.048] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:02.474] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:02.812] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:02.829] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:02.845] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:03.036] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:03.052] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:03.219] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:03.220] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:03.222] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:03.223] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:03.223] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:03.223] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:03.223] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:03.223] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:03.235] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:03.235] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:03.236] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:03.236] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:03.236] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:03.236] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:03.236] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:03.236] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:03.236] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:03.236] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:03.341] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:03.341] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:03.341] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:03.341] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:03.731] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:03.751] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:03.774] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:03.795] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:03.818] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:03.853] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:03.879] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:04.090] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:04.113] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:04.135] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:04.364] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:04.364] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:04.364] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:04.364] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:04.364] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:04.364] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:04.364] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:04.364] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:04.371] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:04.371] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:04.371] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:04.371] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:04.371] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:04.371] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:04.371] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:04.371] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:04.371] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:04.371] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:04.452] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:04.452] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:04.452] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:04.452] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:04.660] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:04.670] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:04.679] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:04.688] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:04.697] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:43:04.865] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:04.875] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:04.885] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:04.901] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:04.873] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:04.873] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:04.873] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:04.873] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:04.873] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:04.873] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:04.873] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:04.873] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:04.879] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:04.879] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:04.879] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:04.879] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:04.879] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:04.879] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:04.879] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:04.879] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:04.879] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:04.879] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:04.976] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:04.976] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:04.976] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:04.976] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:05.075] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.094] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.108] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.124] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.139] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.154] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:05.332] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:05.334] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:05.335] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:05.335] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:05.335] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:05.335] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:05.335] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:05.335] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:05.347] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:05.347] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:05.347] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:05.347] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:05.347] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:05.347] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:05.347] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:05.348] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:05.348] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:05.348] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:05.429] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:05.430] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:05.430] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:05.430] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:05.792] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:05.816] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.837] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.859] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.871] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.884] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.896] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:05.911] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:06.079] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:06.079] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:06.079] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:06.079] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:06.079] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:06.079] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:06.079] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:06.079] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:06.085] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:06.085] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:06.085] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:06.085] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:06.085] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:06.085] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:06.085] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:06.085] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:06.085] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:06.085] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:06.193] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:06.193] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:06.193] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:06.193] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:07.432] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:07.441] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:07.450] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:MultibodyPlant's discrete update solver failed to converge at simulation time = 1.05 with discrete update period = 0.001. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:\n", + " 1. reduce the discrete update period set at construction,\n", + " 2. decrease the high gains in your controller whenever possible,\n", + " 3. switch to a continuous model (discrete update period is zero), though this might affect the simulation run time.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:07.840] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:07.840] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:07.840] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:07.840] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:07.840] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:07.840] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:07.840] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:07.840] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:07.853] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:07.853] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:07.853] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:07.853] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:07.853] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:07.853] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:07.853] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:07.853] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:07.853] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:07.853] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:07.940] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:07.940] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:07.940] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:07.940] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:07.931] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:08.430] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:08.445] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:08.465] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:08.738] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:08.753] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.280] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.304] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.536] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.562] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.578] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.593] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.606] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.773] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.791] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.990] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.992] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:09.999] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:10.001] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:10.008] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:10.010] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:10.017] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:10.026] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:43:10.153] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:10.162] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:10.171] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:10.180] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:10.189] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:10.183] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:10.183] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:10.183] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:10.183] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:10.184] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:10.184] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:10.184] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:10.184] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:10.190] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:10.190] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:10.190] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:10.190] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:10.190] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:10.190] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:10.190] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:10.190] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:10.190] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:10.190] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:10.264] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:10.264] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:10.264] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:10.264] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:10.336] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:10.336] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:10.336] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:10.336] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:10.336] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:10.337] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:10.337] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:10.337] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:10.349] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:10.349] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:10.349] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:10.349] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:10.349] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:10.349] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:10.349] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:10.349] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:10.349] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:10.349] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:10.461] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:10.461] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:10.461] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:10.461] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:11.015] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.044] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.067] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.089] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.113] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.126] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.139] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.338] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.358] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.374] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.387] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.405] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:11.427] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:11.599] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:11.599] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:11.599] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:11.599] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:11.599] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:11.599] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:11.599] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:11.599] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:11.631] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:11.631] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:11.631] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:11.631] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:11.631] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:11.631] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:11.631] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:11.631] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:11.631] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:11.631] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:11.718] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:11.718] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:11.718] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:11.718] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:12.886] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:43:12.890] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:12.890] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:43:12.894] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:12.894] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55fea6db1420)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, -nan, -nan, -nan, 0.0112518, -0.00549093, -0.0124642, -0.00299012, -0.00146881, 1.00895e-05, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [-nan, -nan, -nan, 0.0112518, -0.00549093, -0.0124642, -0.00299012, -0.00146881, 1.00895e-05, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [-nan, -nan, -nan, 0.0112518, -0.00549093, -0.0124642, -0.00299012, -0.00146881, 1.00895e-05, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:43:12.907] [thread: 43818] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = -nan and UBG[0] = -nan..\n", + "ERROR:root:MPC failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:13.089] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:13.089] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:13.089] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:13.089] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:13.089] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:13.089] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:13.090] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:13.090] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:13.102] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:13.102] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:13.102] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:13.102] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:13.102] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:13.102] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:13.102] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:13.102] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:13.102] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:13.102] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:13.199] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:13.199] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:13.199] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:13.199] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:15.958] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:15.973] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:15.988] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:16.004] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:16.020] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:16.174] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:16.174] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:16.174] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:16.174] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:16.174] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:16.174] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:16.174] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:16.174] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:16.187] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:16.187] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:16.187] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:16.187] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:16.187] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:16.187] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:16.187] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:16.187] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:16.187] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:16.187] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:16.301] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:16.301] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:16.301] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:16.301] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:18.603] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:43:18.610] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:18.610] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:43:18.614] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:18.614] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:43:18.618] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:18.618] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:43:18.622] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:18.622] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "ERROR:root:Controller failed\n", + "[ERROR] [2023-12-18 10:43:18.710] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:18.726] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:18.743] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:18.759] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "[WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "ERRORWARNING: Geom with duplicate name '] [2023-12-18 10:43:18.776] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "ERROR:root:Controller failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "[DEBUG] [2023-12-18 10:43:18.790] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:18.791] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:18.791] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:18.791] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:18.791] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:18.791] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:18.791] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:18.791] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:18.797] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:18.797] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:18.797] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:18.797] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:18.797] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:18.797] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:18.797] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:18.797] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:18.797] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:18.797] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:18.873] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:18.873] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:18.873] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:18.873] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:18.955] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:18.955] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:18.955] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:18.955] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:18.956] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:18.956] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:18.956] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:18.956] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:18.979] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:18.979] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:18.979] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:18.979] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:18.979] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:18.979] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:18.979] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:18.979] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:18.979] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:18.979] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:19.102] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:19.102] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:19.102] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:19.102] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:43:21.875] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:21.875] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:43:21.882] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:21.882] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55df8c029b20)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, -nan, -nan, -nan, 0.0387606, -0.0845169, 0.00864665, 0.0341917, 0.0123763, -0.000209217, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [-nan, -nan, -nan, 0.0387606, -0.0845169, 0.00864665, 0.0341917, 0.0123763, -0.000209217, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [-nan, -nan, -nan, 0.0387606, -0.0845169, 0.00864665, 0.0341917, 0.0123763, -0.000209217, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:43:21.910] [thread: 43815] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = -nan and UBG[0] = -nan..\n", + "ERROR:root:MPC failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:22.088] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:22.088] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:22.088] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:22.088] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:22.089] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:22.089] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:22.089] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:22.089] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:22.095] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:22.095] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:22.095] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:22.095] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:22.095] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:22.095] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:22.095] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:22.095] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:22.095] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:22.095] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:22.169] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:22.169] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:22.169] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:22.169] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:23.139] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:43:23.160] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:43:23.170] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:23.372] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:23.391] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:23.420] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:23.435] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:23.451] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:23.467] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:23.482] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:23.381] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:43:23.404] [thread: 43818] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "[DEBUG] [2023-12-18 10:43:23.644] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:23.644] [thread: 43818] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:23.645] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:23.645] [thread: 43818] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:23.645] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:23.645] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:23.645] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:23.645] [thread: 43818] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:23.651] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:23.651] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:23.651] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:23.651] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:23.651] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:23.651] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:23.651] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:23.651] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:23.651] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:23.651] [thread: 43818] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:23.760] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:23.760] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:23.760] [thread: 43818] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:23.760] [thread: 43818] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:26.310] [thread: 43818] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:43:26.321] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:26.321] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:43:26.328] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:26.328] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:43:26.334] [thread: 43818] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:26.334] [thread: 43818] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55fea89ae3e0)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, -nan, -nan, -nan, 0.0512464, -0.131929, 0.0263877, 0.0685611, 0.0211191, -0.000464222, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [-nan, -nan, -nan, 0.0512464, -0.131929, 0.0263877, 0.0685611, 0.0211191, -0.000464222, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [-nan, -nan, -nan, 0.0512464, -0.131929, 0.0263877, 0.0685611, 0.0211191, -0.000464222, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:43:26.353] [thread: 43818] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = -nan and UBG[0] = -nan..\n", + "ERROR:root:MPC failed\n", + "[ERROR] [2023-12-18 10:43:26.477] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:26.485] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:26.495] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:26.504] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "Exit condition -- generations = 5\n", + "[DEBUG] [2023-12-18 10:43:26.468] [thread: 43815] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:26.513] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:26.634] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:26.634] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:26.634] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:26.634] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:26.634] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:26.634] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:26.634] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:26.634] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:26.640] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:26.640] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:26.640] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:26.640] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:26.640] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:26.640] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:26.640] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:26.640] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:26.640] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:26.640] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:26.714] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:26.714] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:26.714] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:26.714] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "[DEBUG] [2023-12-18 10:43:28.819] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:28.819] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:28.819] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:28.819] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:28.819] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:28.819] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:28.819] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:28.819] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:28.826] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:28.826] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:28.826] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:28.826] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:28.826] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:28.826] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:28.826] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:28.826] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:28.826] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:28.826] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:43:28.676] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:28.676] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:43:28.680] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:28.680] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:43:28.684] [thread: 43815] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:43:28.684] [thread: 43815] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "Function solver (0x55df8df0af70)\n", + "Input 0 (x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 1 (p): [1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0.119656, 0.0666417, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, 0.000472964, -0.066792, 0, -nan, -nan, -nan, 0.0410073, -0.130664, 0.0165652, 0.0674663, 0.0150885, -0.000331253, -0.000197771, -3.61032e-05, 0.58242, 0.0035286, -3.89292e-05, 0.58242, 0.00755331, -4.19343e-05, 0.58242, 0.0118414, -4.50809e-05, 0.58242, 0.0163542, -4.83284e-05, 0.58242, 0.0210503, -5.16345e-05, 0.58242, 0.0258864, -5.49556e-05, 0.58242, 0.0308183, -5.82479e-05, 0.58242, 0.0358019, -6.14685e-05, 0.58242, 0.0407939, -6.45758e-05, 0.58242, 0.0457529, -6.7531e-05, 0.58242, 0.0506401, -7.02984e-05, 0.58242, 0.0554207, -7.28467e-05, 0.58242, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 2 (lbx): [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 3 (ubx): [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]\n", + "Input 4 (lbg): [-nan, -nan, -nan, 0.0410073, -0.130664, 0.0165652, 0.0674663, 0.0150885, -0.000331253, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]\n", + "Input 5 (ubg): [-nan, -nan, -nan, 0.0410073, -0.130664, 0.0165652, 0.0674663, 0.0150885, -0.000331253, 0, 0, 0, 0.000472964, -0.066792, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 6 (lam_x0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "Input 7 (lam_g0): [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n", + "[ERROR] [2023-12-18 10:43:28.694] [thread: 43815] [blf] [CentroidalMPC::advance] Unable to solve the problem. The following exception has been thrown Error in Function::call for 'controller' [MXFunction] at .../casadi/core/function.cpp:330:\n", + "Error in Function::operator() for 'solver' [IpoptInterface] at .../casadi/core/function.cpp:1482:\n", + ".../casadi/core/nlpsol.cpp:620: Assertion \"lb <= ub && lb!=inf && ub!=-inf\" failed:\n", + "Ill-posed problem detected: LBG[0] <= UBG[0] was violated. Got LBG[0] = -nan and UBG[0] = -nan..\n", + "ERROR:root:MPC failed\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n", + "WARNING: Geom with duplicate name '' encountered in URDF, creating an unnamed geom.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:28.899] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:28.899] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:28.899] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:28.899] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:29.197] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:30.010] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:30.621] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:30.771] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:30.966] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:30.976] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:31.185] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:31.195] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:31.204] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:31.437] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:31.794] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:31.803] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:31.812] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:31.821] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:32.042] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:32.052] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:32.061] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:32.072] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:32.082] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:32.255] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:32.264] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:32.274] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:32.283] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:43:32.410] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:32.410] [thread: 43815] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:32.410] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:32.410] [thread: 43815] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:32.410] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:32.410] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:32.410] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:43:32.410] [thread: 43815] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:43:32.416] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:43:32.416] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:43:32.416] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:43:32.416] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:43:32.416] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:43:32.416] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:43:32.416] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:32.416] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:32.416] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:43:32.416] [thread: 43815] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:43:32.491] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:32.491] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:43:32.491] [thread: 43815] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:43:32.491] [thread: 43815] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:32.999] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.157] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.175] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.192] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.209] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.226] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.397] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.411] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.426] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.440] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.454] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:33.747] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.040] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.051] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.069] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.226] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.250] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.429] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.606] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.619] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.637] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.649] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.661] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "[ERROR] [2023-12-18 10:43:34.672] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Exit condition -- generations = 5\n", + "Number of islands: 2\n", + "Topology: Unconnected\n", + "Migration type: point-to-point\n", + "Migrant handling policy: preserve\n", + "Status: idle\n", + "\n", + "Islands summaries:\n", + "\n", + "\t# Type Algo Prob Size Status \n", + "\t-------------------------------------------------------------------------------------------------------------------------------------------\n", + "\t0 Multiprocessing island CMA-ES: Covariance Matrix Adaptation Evolutionary Strategy 5 idle \n", + "\t1 Multiprocessing island CMA-ES: Covariance Matrix Adaptation Evolutionary Strategy 5 idle \n", + " None\n", + "{'res': [array([-4.143]), array([-4.103])], 'x': [array([ 701.26484076, 715.81850736, 617.20277594, 434.35466999,\n", + " 361.82303085, 1000. , 917.28287072, 570.11267893,\n", + " 259.85317513, 0. , 497.37569308, 1000. ,\n", + " 152.69507148, 732.34554372, 277.48788585, 1000. ,\n", + " 562.77929379, 0. , 463.25670956, 897.65152491,\n", + " 1000. , 71.86361876, 1000. , 1000. ,\n", + " 335.08114475, 436.56888739, 9.27556022, 0. ]), array([1000. , 304.21555722, 1000. , 0. ,\n", + " 0. , 546.81109553, 762.40716246, 0. ,\n", + " 80.35714398, 0. , 600.4050813 , 0. ,\n", + " 1000. , 765.47953586, 897.43666459, 524.50277115,\n", + " 964.93947906, 764.36151298, 393.32454481, 1000. ,\n", + " 1000. , 1000. , 220.10450288, 268.02808405,\n", + " 14.21060112, 791.30421121, 0. , 0. ])], 'num_workers': 2, 'population': 5, 'generations': 5, 'ret_flag': None}\n", + "CPU times: user 1min 40s, sys: 3min 33s, total: 5min 14s\n", + "Wall time: 2min 44s\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [2023-12-18 10:43:34.844] [thread: 43815] [blf] [QPTSID::advance] osqp was not able to find a feasible solution.\n", + "ERROR:root:Controller failed\n" + ] + } + ], + "source": [ + "%%time\n", + "\n", + "# [Warning] \n", + "# This cell can take a long time to run.\n", + "# Choose the number of workers, population and generations based on the machine you are using.\n", + "num_workers = 2\n", + "population = 5\n", + "generations = 5\n", + "\n", + "prob = pg.problem(GainTuningProblem())\n", + "algo = pg.algorithm(pg.cmaes(gen=generations, force_bounds=True))\n", + "algo.set_verbosity(1)\n", + "archi = pg.archipelago(\n", + " n=num_workers,\n", + " algo=algo,\n", + " prob=prob,\n", + " pop_size=population,\n", + ")\n", + "archi.evolve()\n", + "print(archi)\n", + "ret_flag = archi.wait_check()\n", + "res = archi.get_champions_f()\n", + "x = archi.get_champions_x()\n", + "print(archi, ret_flag)\n", + "\n", + "dict_log = {}\n", + "dict_log[\"res\"] = res\n", + "dict_log[\"x\"] = x\n", + "dict_log[\"num_workers\"] = num_workers\n", + "dict_log[\"population\"] = population\n", + "dict_log[\"generations\"] = generations\n", + "dict_log[\"ret_flag\"] = ret_flag\n", + "\n", + "print(dict_log)\n", + "\n", + "\n", + "# {'res': [array([-7.433]), array([-5.343])], 'x': [array([ 0. , 0. , 488.57760043, 0. ,\n", + "# 0. , 805.59521357, 1000. , 950.62855616,\n", + "# 0. , 305.82151331, 366.00847486, 0. ,\n", + "# 515.2402816 , 0. , 925.6375872 , 0. ,\n", + "# 1000. , 1000. , 1000. , 1000. ,\n", + "# 396.757885 , 115.08573757, 56.49303801, 558.17879614,\n", + "# 795.93093513, 485.46698371, 286.37351307, 54.53056333]), array([ 371.12645591, 1000. , 916.51730098, 165.7734585 ,\n", + "# 0. , 170.92760917, 1000. , 0. ,\n", + "# 372.54686492, 753.15044182, 97.68243388, 0. ,\n", + "# 380.84166921, 0. , 572.90921079, 717.58449249,\n", + "# 847.8886871 , 417.78861249, 0. , 812.82806723,\n", + "# 213.01939079, 216.13608247, 0. , 721.50288941,\n", + "# 121.38006704, 655.37170984, 203.759172 , 93.28056376])], 'num_workers': 2, 'population': 5, 'generations': 5, 'ret_flag': None}\n" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "INFO:drake:Meshcat listening for connections at http://localhost:7001\n", + "WARNING:drake:Meshcat does not display HalfSpace geometry (yet).\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "This is Ipopt version 3.14.12, running with linear solver MUMPS 5.2.1.\n", + "\n", + "Number of nonzeros in equality constraint Jacobian...: 126\n", + "Number of nonzeros in inequality constraint Jacobian.: 0\n", + "Number of nonzeros in Lagrangian Hessian.............: 142\n", + "\n", + "Total number of variables............................: 27\n", + " variables with only lower bounds: 0\n", + " variables with lower and upper bounds: 0\n", + " variables with only upper bounds: 0\n", + "Total number of equality constraints.................: 21\n", + "Total number of inequality constraints...............: 0\n", + " inequality constraints with only lower bounds: 0\n", + " inequality constraints with lower and upper bounds: 0\n", + " inequality constraints with only upper bounds: 0\n", + "\n", + "iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls\n", + " 0 5.2479115e-03 1.00e+00 1.95e-01 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0\n", + " 1 3.8856380e+00 7.52e-02 1.66e+00 -1.7 1.00e+00 0.0 1.00e+00 1.00e+00h 1\n", + " 2 3.8855317e+00 1.62e-06 1.60e+00 -1.7 7.52e-02 1.3 1.00e+00 1.00e+00h 1\n", + " 3 3.8854301e+00 5.92e-08 2.34e-02 -1.7 3.29e-03 0.9 1.00e+00 1.00e+00h 1\n", + " 4 3.8854066e+00 5.21e-09 1.95e-02 -3.8 1.03e-03 1.3 1.00e+00 1.00e+00h 1\n", + " 5 3.8853711e+00 4.67e-08 1.24e-02 -3.8 1.97e-03 0.8 1.00e+00 1.00e+00h 1\n", + " 6 3.8853627e+00 6.31e-09 1.03e-02 -3.8 6.10e-04 1.2 1.00e+00 1.00e+00h 1\n", + " 7 3.8853494e+00 4.33e-08 6.47e-03 -3.8 1.15e-03 0.7 1.00e+00 1.00e+00h 1\n", + " 8 3.8853461e+00 5.34e-09 5.33e-03 -3.8 3.56e-04 1.2 1.00e+00 1.00e+00h 1\n", + " 9 3.8853408e+00 3.08e-08 3.34e-03 -3.8 6.69e-04 0.7 1.00e+00 1.00e+00h 1\n", + "iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls\n", + " 10 3.8853395e+00 3.56e-09 2.75e-03 -3.8 2.07e-04 1.1 1.00e+00 1.00e+00h 1\n", + " 11 3.8853373e+00 1.80e-08 1.73e-03 -3.8 3.89e-04 0.6 1.00e+00 1.00e+00h 1\n", + " 12 3.8853368e+00 1.98e-09 1.42e-03 -3.8 1.20e-04 1.1 1.00e+00 1.00e+00h 1\n", + " 13 3.8853366e+00 2.53e-10 1.32e-03 -5.7 4.18e-05 1.5 1.00e+00 1.00e+00h 1\n", + " 14 3.8853362e+00 1.71e-09 1.07e-03 -5.7 1.02e-04 1.0 1.00e+00 1.00e+00h 1\n", + " 15 3.8853361e+00 2.15e-10 9.92e-04 -5.7 3.54e-05 1.4 1.00e+00 1.00e+00h 1\n", + " 16 3.8853358e+00 1.40e-09 8.00e-04 -5.7 8.55e-05 1.0 1.00e+00 1.00e+00h 1\n", + " 17 3.8853357e+00 1.73e-10 7.34e-04 -5.7 2.94e-05 1.4 1.00e+00 1.00e+00h 1\n", + " 18 3.8853355e+00 1.08e-09 5.85e-04 -5.7 7.03e-05 0.9 1.00e+00 1.00e+00h 1\n", + " 19 3.8853355e+00 1.30e-10 5.34e-04 -5.7 2.41e-05 1.3 1.00e+00 1.00e+00h 1\n", + "iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls\n", + " 20 3.8853353e+00 7.73e-10 4.19e-04 -5.7 5.67e-05 0.9 1.00e+00 1.00e+00h 1\n", + " 21 3.8853353e+00 9.19e-11 3.80e-04 -5.7 1.93e-05 1.3 1.00e+00 1.00e+00h 1\n", + " 22 3.8853352e+00 5.17e-10 2.93e-04 -5.7 4.46e-05 0.8 1.00e+00 1.00e+00h 1\n", + " 23 3.8853352e+00 6.01e-11 2.64e-04 -5.7 1.51e-05 1.2 1.00e+00 1.00e+00h 1\n", + " 24 3.8853352e+00 3.21e-10 2.01e-04 -5.7 3.44e-05 0.8 1.00e+00 1.00e+00h 1\n", + " 25 3.8853352e+00 3.64e-11 1.80e-04 -5.7 1.16e-05 1.2 1.00e+00 1.00e+00h 1\n", + " 26 3.8853352e+00 1.83e-10 1.35e-04 -5.7 2.60e-05 0.7 1.00e+00 1.00e+00h 1\n", + " 27 3.8853352e+00 2.02e-11 1.20e-04 -5.7 8.64e-06 1.1 1.00e+00 1.00e+00h 1\n", + " 28 3.8853351e+00 9.50e-11 8.66e-05 -5.7 1.88e-05 0.7 1.00e+00 1.00e+00h 1\n", + " 29 3.8853351e+00 1.02e-11 7.57e-05 -5.7 6.15e-06 1.1 1.00e+00 1.00e+00h 1\n", + "iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls\n", + " 30 3.8853351e+00 4.48e-11 5.29e-05 -5.7 1.29e-05 0.6 1.00e+00 1.00e+00h 1\n", + " 31 3.8853351e+00 4.66e-12 4.55e-05 -5.7 4.16e-06 1.0 1.00e+00 1.00e+00h 1\n", + " 32 3.8853351e+00 5.78e-13 4.29e-05 -5.7 1.47e-06 1.5 1.00e+00 1.00e+00h 1\n", + " 33 3.8853351e+00 3.75e-12 3.63e-05 -5.7 3.74e-06 1.0 1.00e+00 1.00e+00h 1\n", + " 34 3.8853351e+00 4.58e-13 3.40e-05 -5.7 1.31e-06 1.4 1.00e+00 1.00e+00h 1\n", + " 35 3.8853351e+00 2.86e-12 2.82e-05 -5.7 3.26e-06 0.9 1.00e+00 1.00e+00h 1\n", + " 36 3.8853351e+00 3.44e-13 2.62e-05 -5.7 1.14e-06 1.4 1.00e+00 1.00e+00h 1\n", + " 37 3.8853351e+00 2.06e-12 2.13e-05 -5.7 2.77e-06 0.9 1.00e+00 1.00e+00h 1\n", + " 38 3.8853351e+00 2.43e-13 1.96e-05 -5.7 9.56e-07 1.3 1.00e+00 1.00e+00h 1\n", + " 39 3.8853351e+00 1.39e-12 1.56e-05 -5.7 2.28e-06 0.8 1.00e+00 1.00e+00h 1\n", + "iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls\n", + " 40 3.8853351e+00 1.63e-13 1.42e-05 -8.6 7.78e-07 1.3 1.00e+00 1.00e+00h 1\n", + " 41 3.8853351e+00 8.78e-13 1.10e-05 -8.6 1.81e-06 0.8 1.00e+00 1.00e+00h 1\n", + " 42 3.8853351e+00 1.00e-13 9.89e-06 -8.6 6.11e-07 1.2 1.00e+00 1.00e+00h 1\n", + " 43 3.8853351e+00 5.11e-13 7.44e-06 -8.6 1.38e-06 0.7 1.00e+00 1.00e+00h 1\n", + " 44 3.8853351e+00 5.67e-14 6.62e-06 -8.6 4.60e-07 1.2 1.00e+00 1.00e+00h 1\n", + " 45 3.8853351e+00 2.73e-13 4.84e-06 -8.6 1.01e-06 0.7 1.00e+00 1.00e+00h 1\n", + " 46 3.8853351e+00 2.93e-14 4.25e-06 -8.6 3.32e-07 1.1 1.00e+00 1.00e+00h 1\n", + " 47 3.8853351e+00 1.33e-13 3.00e-06 -8.6 7.04e-07 0.6 1.00e+00 1.00e+00h 1\n", + " 48 3.8853351e+00 1.38e-14 2.59e-06 -8.6 2.28e-07 1.1 1.00e+00 1.00e+00h 1\n", + " 49 3.8853351e+00 1.71e-15 2.45e-06 -8.6 8.09e-08 1.5 1.00e+00 1.00e+00h 1\n", + "iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls\n", + " 50 3.8853351e+00 1.13e-14 2.08e-06 -8.6 2.06e-07 1.0 1.00e+00 1.00e+00h 1\n", + " 51 3.8853351e+00 1.20e-15 1.96e-06 -8.6 7.26e-08 1.4 1.00e+00 1.00e+00h 1\n", + " 52 3.8853351e+00 8.67e-15 1.63e-06 -8.6 1.82e-07 1.0 1.00e+00 1.00e+00h 1\n", + " 53 3.8853351e+00 1.11e-15 1.52e-06 -8.6 6.35e-08 1.4 1.00e+00 1.00e+00h 1\n", + " 54 3.8853351e+00 6.48e-15 1.24e-06 -8.6 1.56e-07 0.9 1.00e+00 1.00e+00h 1\n", + " 55 3.8853351e+00 7.39e-16 1.15e-06 -8.6 5.40e-08 1.3 1.00e+00 1.00e+00h 1\n", + " 56 3.8853351e+00 4.55e-15 9.19e-07 -8.6 1.30e-07 0.9 1.00e+00 1.00e+00h 1\n", + " 57 3.8853351e+00 4.92e-16 8.40e-07 -8.6 4.44e-08 1.3 1.00e+00 1.00e+00h 1\n", + " 58 3.8853351e+00 2.93e-15 6.56e-07 -8.6 1.04e-07 0.8 1.00e+00 1.00e+00h 1\n", + " 59 3.8853351e+00 4.16e-16 5.93e-07 -8.6 3.53e-08 1.2 1.00e+00 1.00e+00h 1\n", + "iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls\n", + " 60 3.8853351e+00 1.64e-15 4.51e-07 -8.6 8.04e-08 0.7 1.00e+00 1.00e+00h 1\n", + " 61 3.8853351e+00 2.01e-16 4.03e-07 -8.6 2.69e-08 1.2 1.00e+00 1.00e+00h 1\n", + " 62 3.8853351e+00 9.71e-16 2.97e-07 -8.6 5.96e-08 0.7 1.00e+00 1.00e+00h 1\n", + " 63 3.8853351e+00 5.55e-17 2.62e-07 -8.6 1.97e-08 1.1 1.00e+00 1.00e+00h 1\n", + " 64 3.8853351e+00 6.91e-16 1.87e-07 -8.6 4.23e-08 0.6 1.00e+00 1.00e+00h 1\n", + " 65 3.8853351e+00 6.94e-17 1.63e-07 -8.6 1.38e-08 1.1 1.00e+00 1.00e+00h 1\n", + " 66 3.8853351e+00 1.28e-16 1.54e-07 -8.6 4.89e-09 1.5 1.00e+00 1.00e+00h 1\n", + " 67 3.8853351e+00 1.63e-16 1.32e-07 -8.6 1.26e-08 1.0 1.00e+00 1.00e+00h 1\n", + " 68 3.8853351e+00 5.22e-17 1.24e-07 -8.6 4.43e-09 1.4 1.00e+00 1.00e+00h 1\n", + " 69 3.8853351e+00 6.61e-17 1.04e-07 -8.6 1.12e-08 1.0 1.00e+00 1.00e+00h 1\n", + "iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls\n", + " 70 3.8853351e+00 8.66e-17 9.72e-08 -8.6 3.91e-09 1.4 1.00e+00 1.00e+00h 1\n", + "\n", + "Number of Iterations....: 70\n", + "\n", + " (scaled) (unscaled)\n", + "Objective...............: 3.8853351325000243e+00 3.8853351325000243e+00\n", + "Dual infeasibility......: 9.7240255736430470e-08 9.7240255736430470e-08\n", + "Constraint violation....: 8.6576856794806649e-17 8.6576856794806649e-17\n", + "Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00\n", + "Complementarity.........: 0.0000000000000000e+00 0.0000000000000000e+00\n", + "Overall NLP error.......: 9.7240255736430470e-08 9.7240255736430470e-08\n", + "\n", + "\n", + "Number of objective function evaluations = 71\n", + "Number of objective gradient evaluations = 71\n", + "Number of equality constraint evaluations = 71\n", + "Number of inequality constraint evaluations = 0\n", + "Number of equality constraint Jacobian evaluations = 71\n", + "Number of inequality constraint Jacobian evaluations = 0\n", + "Number of Lagrangian Hessian evaluations = 70\n", + "Total seconds in IPOPT = 0.049\n", + "\n", + "EXIT: Solved To Acceptable Level.\n", + " solver : t_proc (avg) t_wall (avg) n_eval\n", + " nlp_f | 267.00us ( 3.76us) 273.42us ( 3.85us) 71\n", + " nlp_g | 700.00us ( 9.86us) 649.22us ( 9.14us) 71\n", + " nlp_grad_f | 753.00us ( 10.46us) 753.89us ( 10.47us) 72\n", + " nlp_hess_l | 14.18ms (202.54us) 14.20ms (202.89us) 70\n", + " nlp_jac_g | 4.52ms ( 62.82us) 4.54ms ( 63.12us) 72\n", + " total | 49.22ms ( 49.22ms) 49.10ms ( 49.10ms) 1\n", + "[DEBUG] [2023-12-18 10:49:27.032] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'verbosity' not found.\n", + "[INFO] [2023-12-18 10:49:27.032] [thread: 43757] [blf] [QPTSID::initialize] 'verbosity' not found. The following parameter will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:49:27.032] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:49:27.032] [thread: 43757] [blf] [CoMTask::initialize] [CoMTask Task.] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:49:27.033] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:49:27.033] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: l_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:49:27.033] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'mask' not found.\n", + "[INFO] [2023-12-18 10:49:27.033] [thread: 43757] [blf] [SE3Task::initialize] [SE3Task Optimal Control Element - Frame name: r_sole] Unable to find the mask parameter. The default value is used: true true true.\n", + "[DEBUG] [2023-12-18 10:49:27.039] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_tolerance' not found.\n", + "[INFO] [2023-12-18 10:49:27.039] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_tolerance'. The default one will be used '1e-08'.\n", + "[DEBUG] [2023-12-18 10:49:27.039] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'ipopt_max_iteration' not found.\n", + "[INFO] [2023-12-18 10:49:27.039] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'ipopt_max_iteration'. The default one will be used '3000'.\n", + "[DEBUG] [2023-12-18 10:49:27.039] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'solver_verbosity' not found.\n", + "[INFO] [2023-12-18 10:49:27.039] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'solver_verbosity'. The default one will be used '0'.\n", + "[DEBUG] [2023-12-18 10:49:27.039] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_warm_start_enabled' not found.\n", + "[INFO] [2023-12-18 10:49:27.039] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_warm_start_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:49:27.039] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'is_cse_enabled' not found.\n", + "[INFO] [2023-12-18 10:49:27.039] [thread: 43757] [blf] [CentroidalMPC::Impl::loadParameters] Unable to load the parameter named 'is_cse_enabled'. The default one will be used 'false'.\n", + "[DEBUG] [2023-12-18 10:49:27.113] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:49:27.113] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n", + "[DEBUG] [2023-12-18 10:49:27.113] [thread: 43757] [blf] [StdImplementation::getParameterPrivate] Parameter named 'interpolation_method' not found.\n", + "[INFO] [2023-12-18 10:49:27.113] [thread: 43757] [blf] [SwingFootPlanner::initialize] The parameter named 'interpolation_method' not found. The 'min_acceleration' method will be used.\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[OsqpEigen::Solver::initSolver] Unable to setup the workspace.\n", + "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the solver.\n", + "[ERROR] [2023-12-18 10:49:35.064] [thread: 43757] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:49:35.064] [thread: 43757] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:49:35.068] [thread: 43757] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:49:35.068] [thread: 43757] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:49:35.073] [thread: 43757] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:49:35.073] [thread: 43757] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:49:35.078] [thread: 43757] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:49:35.078] [thread: 43757] [blf] [QPTSID::advance] Unable to update the QP solver.\n", + "ERROR:root:Controller failed\n", + "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized.\n", + "[ERROR] [2023-12-18 10:49:35.082] [thread: 43757] [blf] [QPTSID::Impl::updateSolver] Unable to set the hessian matrix.\n", + "[ERROR] [2023-12-18 10:49:35.082] [thread: 43757] [blf] [QPTSID::advance] Unable to update the QP solver.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] [2023-12-18 10:49:35.058] [thread: 43757] [blf] [QPTSID::advance] The solver found an inaccurate feasible solution.\n", + "ERROR in LDL_factor: Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex\n", + "ERROR in osqp_setup: KKT matrix factorization.\n", + "The problem seems to be non-convex.\n", + "CPU times: user 33.2 s, sys: 1min 12s, total: 1min 45s\n", + "Wall time: 12.1 s\n" + ] + }, + { + "data": { + "text/plain": [ + "4.790999999999942" + ] + }, + "execution_count": 15, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "%%time\n", + "# extract the best solution\n", + "best_x_k = np.array(x[np.argmin(res)])\n", + "\n", + "# or load a known solution\n", + "# best_x_k = np.array([684.25282492, 648.05228734, 569.96763936, 350.65899653,\n", + "# 949.98434623, 515.09198331, 845.83335962, 17.95951342,\n", + "# 314.5705753 , 676.403354 , 440.72680377, 467.56525625,\n", + "# 681.69608946, 97.4551466 , 715.55867788, 416.80935819,\n", + "# 470.72979393, 680.49815841, 876.67111456, 654.65448001,\n", + "# 329.59585922, 65.34583639, 975.68181074, 342.1368993 ,\n", + "# 237.81553041, 606.40982421, 32.24741521, 10.83276846])\n", + "\n", + "# or use default values\n", + "best_x_k = None\n", + "\n", + "# run the cost function\n", + "cost_function(best_x_k, simulator=\"drake\", visualise_flag = True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "comododev", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.13" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/examples/drake_walking.ipynb b/examples/drake_walking.ipynb new file mode 100644 index 0000000..d247a6e --- /dev/null +++ b/examples/drake_walking.ipynb @@ -0,0 +1,333 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Comodo Exmaple \n", + "This examples, load a basic robot model (i.e. composed only of basic shapes), modifies the links of such a robot model by elongating the legs, define instances of the TSID (Task Based Inverse Dynamics) and Centroidal MPC controller and simulate the behavior of the robot using drake. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Comodo import\n", + "from comodo.drakeSimulator.drakeSimulator import DrakeSimulator\n", + "from comodo.robotModel.robotModel import RobotModel\n", + "from comodo.robotModel.createUrdf import createUrdf\n", + "from comodo.centroidalMPC.centroidalMPC import CentroidalMPC\n", + "from comodo.centroidalMPC.mpcParameterTuning import MPCParameterTuning\n", + "from comodo.TSIDController.TSIDParameterTuning import TSIDParameterTuning\n", + "from comodo.TSIDController.TSIDController import TSIDController" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# General import \n", + "import xml.etree.ElementTree as ET\n", + "import numpy as np\n", + "import tempfile\n", + "import urllib.request\n", + "import logging" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# set the logging level\n", + "logging.getLogger().setLevel(logging.INFO)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Getting stickbot urdf file and convert it to string \n", + "urdf_robot_file = tempfile.NamedTemporaryFile(mode=\"w+\")\n", + "url = 'https://raw.githubusercontent.com/icub-tech-iit/ergocub-gazebo-simulations/master/models/stickBot/model.urdf'\n", + "urllib.request.urlretrieve(url, urdf_robot_file.name)\n", + "# Load the URDF file\n", + "tree = ET.parse(urdf_robot_file.name)\n", + "root = tree.getroot()\n", + "\n", + "# Convert the XML tree to a string\n", + "robot_urdf_string_original = ET.tostring(root)\n", + "\n", + "create_urdf_instance = createUrdf(\n", + " original_urdf_path=urdf_robot_file.name, save_gazebo_plugin=False\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define parametric links and controlled joints \n", + "legs_link_names = [\"hip_3\", \"lower_leg\"]\n", + "joint_name_list = [\n", + " \"r_shoulder_pitch\",\n", + " \"r_shoulder_roll\",\n", + " \"r_shoulder_yaw\",\n", + " \"r_elbow\",\n", + " \"l_shoulder_pitch\",\n", + " \"l_shoulder_roll\",\n", + " \"l_shoulder_yaw\",\n", + " \"l_elbow\",\n", + " \"r_hip_pitch\",\n", + " \"r_hip_roll\",\n", + " \"r_hip_yaw\",\n", + " \"r_knee\",\n", + " \"r_ankle_pitch\",\n", + " \"r_ankle_roll\",\n", + " \"l_hip_pitch\",\n", + " \"l_hip_roll\",\n", + " \"l_hip_yaw\",\n", + " \"l_knee\",\n", + " \"l_ankle_pitch\",\n", + " \"l_ankle_roll\",\n", + "]" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define the robot modifications\n", + "modifications = {}\n", + "for item in legs_link_names:\n", + " left_leg_item = \"l_\" + item\n", + " right_leg_item = \"r_\" + item\n", + " modifications.update({left_leg_item: 1.2})\n", + " modifications.update({right_leg_item: 1.2})\n", + "# Motors Parameters \n", + "Im_arms = 1e-3*np.ones(4) # from 0-4\n", + "Im_legs = 1e-3*np.ones(6) # from 5-10\n", + "kv_arms = 0.001*np.ones(4) # from 11-14\n", + "kv_legs = 0.001*np.ones(6) #from 20\n", + "\n", + "Im = np.concatenate((Im_arms, Im_arms, Im_legs, Im_legs))\n", + "kv = np.concatenate((kv_arms, kv_arms, kv_legs, kv_legs))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Modify the robot model and initialize\n", + "create_urdf_instance.modify_lengths(modifications)\n", + "urdf_robot_string = create_urdf_instance.write_urdf_to_file()\n", + "create_urdf_instance.reset_modifications()\n", + "robot_model_init = RobotModel(urdf_robot_string, \"stickBot\", joint_name_list)\n", + "s_des, xyz_rpy, H_b = robot_model_init.compute_desired_position_walking()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define simulator and initial position\n", + "drake_instance = DrakeSimulator()\n", + "drake_instance.set_visualize_robot_flag(True)\n", + "drake_instance.load_model(robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im)\n", + "s, ds, tau = drake_instance.get_state()\n", + "t = drake_instance.get_simulation_time()\n", + "H_b = drake_instance.get_base()\n", + "# beware of the velocity ordering\n", + "w_b = drake_instance.get_base_velocity()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define the controller parameters and instantiate the controller\n", + "# Controller Parameters\n", + "tsid_parameter = TSIDParameterTuning()\n", + "mpc_parameters = MPCParameterTuning()\n", + "\n", + "# TSID Instance\n", + "TSID_controller_instance = TSIDController(frequency=0.01, robot_model=robot_model_init)\n", + "TSID_controller_instance.define_tasks(tsid_parameter)\n", + "TSID_controller_instance.set_state_with_base(s, ds, H_b, w_b, t)\n", + "\n", + "# MPC Instance\n", + "step_lenght = 0.1\n", + "mpc = CentroidalMPC(robot_model=robot_model_init, step_length=step_lenght)\n", + "mpc.intialize_mpc(mpc_parameters=mpc_parameters)\n", + "\n", + "# Set desired quantities\n", + "mpc.configure(s_init=s_des, H_b_init=H_b)\n", + "TSID_controller_instance.compute_com_position()\n", + "mpc.define_test_com_traj(TSID_controller_instance.COM.toNumPy())\n", + "\n", + "# TODO: Set initial robot state and plan trajectories \n", + "drake_instance.step(1)\n", + "\n", + "# Reading the state \n", + "s, ds, tau = drake_instance.get_state()\n", + "H_b = drake_instance.get_base()\n", + "w_b = drake_instance.get_base_velocity()\n", + "t = drake_instance.get_simulation_time()\n", + "\n", + "# MPC\n", + "mpc.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + "mpc.initialize_centroidal_integrator(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + "mpc_output = mpc.plan_trajectory()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Set loop variables \n", + "TIME_TH = 20.0\n", + "\n", + "# Define number of steps\n", + "n_step = int(\n", + " TSID_controller_instance.frequency / drake_instance.get_simulation_frequency()\n", + ")\n", + "n_step_mpc_tsid = int(mpc.get_frequency_seconds() / TSID_controller_instance.frequency)\n", + "\n", + "counter = 0\n", + "mpc_success = True\n", + "energy_tot = 0.0\n", + "succeded_controller = True" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Simulation-control loop\n", + "if drake_instance.visualize_robot_flag:\n", + " drake_instance.meshcat.StartRecording()\n", + "while t < TIME_TH:\n", + " \n", + " # Reading robot state from simulator\n", + " s, ds, tau = drake_instance.get_state()\n", + " energy_i = np.linalg.norm(tau)\n", + " H_b = drake_instance.get_base()\n", + " w_b = drake_instance.get_base_velocity()\n", + " t = drake_instance.get_simulation_time()\n", + "\n", + " # Update TSID\n", + " TSID_controller_instance.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + "\n", + " # MPC plan \n", + " if counter == 0:\n", + " mpc.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + " mpc.update_references()\n", + " mpc_success = mpc.plan_trajectory()\n", + " mpc.contact_planner.advance_swing_foot_planner()\n", + " if not (mpc_success):\n", + " logging.error(\"MPC failed\")\n", + " break\n", + "\n", + " # Reading new references\n", + " com, dcom, forces_left, forces_right = mpc.get_references()\n", + " left_foot, right_foot = mpc.contact_planner.get_references_swing_foot_planner()\n", + "\n", + " # Update references TSID \n", + " TSID_controller_instance.update_task_references_mpc(\n", + " com=com,\n", + " dcom=dcom,\n", + " ddcom=np.zeros(3),\n", + " left_foot_desired=left_foot,\n", + " right_foot_desired=right_foot,\n", + " s_desired=np.array(s_des),\n", + " wrenches_left=forces_left,\n", + " wrenches_right=forces_right,\n", + " )\n", + "\n", + " # Run control \n", + " succeded_controller = TSID_controller_instance.run()\n", + "\n", + " if not (succeded_controller):\n", + " logging.error(\"Controller failed\")\n", + " break\n", + "\n", + " tau = TSID_controller_instance.get_torque()\n", + "\n", + " # Step the simulator\n", + " drake_instance.set_input(tau)\n", + " # drake_instance.step_with_motors(n_step=n_step, torque=tau)\n", + " try:\n", + " drake_instance.step(n_step=n_step)\n", + " except RuntimeError as e:\n", + " logging.error(e)\n", + " break\n", + " counter = counter + 1\n", + "\n", + " if counter == n_step_mpc_tsid:\n", + " counter = 0\n", + "if drake_instance.visualize_robot_flag:\n", + " drake_instance.meshcat.StopRecording()\n", + " drake_instance.meshcat.PublishRecording()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Closing visualization\n", + "drake_instance.close_visualization()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "walk", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.13" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/src/comodo/TSIDController/TSIDParameterTuning.py b/src/comodo/TSIDController/TSIDParameterTuning.py index fe71651..38a0d3a 100644 --- a/src/comodo/TSIDController/TSIDParameterTuning.py +++ b/src/comodo/TSIDController/TSIDParameterTuning.py @@ -5,44 +5,54 @@ class TSIDParameterTuning: def __init__(self) -> None: self.CoM_Kp = 15.0 self.CoM_Kd = 7.0 - self.postural_Kp = ( - np.array( - [ - 130, - 80, - 20, - 60, - 130, - 80, - 20, - 60, - 150, - 20, - 20, - 180, - 140, - 20, - 150, - 20, - 20, - 180, - 140, - 20, - ] - ) - * 2.8 - ) # TODO symmetry - self.postural_weight = 10 * np.ones(len(self.postural_Kp)) - self.foot_tracking_task_kp_lin = 30.0 + # we shall keep the gains symmetric + # --------------------------------------------- + # postural gains + leg = np.array([130, 80, 20, 60]) * 2.8 + arm = np.array([150, 20, 20, 180, 140, 20]) * 2.8 + self.postural_Kp = np.concatenate((leg, leg, arm, arm)) + + leg_kd = np.power(leg, 1 / 2) / 10 + arm_kd = np.power(arm, 1 / 2) / 10 + self.postural_Kd = np.concatenate((leg_kd, leg_kd, arm_kd, arm_kd)) + # --------------------------------------------- + # foot tracking gains + self.foot_tracking_task_kp_lin = 100.0 self.foot_tracking_task_kd_lin = 7.0 self.foot_tracking_task_kp_ang = 300.0 self.foot_tracking_task_kd_ang = 10.0 - self.root_tracking_task_weight = 10 * np.ones(3) + # --------------------------------------------- + # root link tracking gains self.root_link_kp_ang = 20.0 self.root_link_kd_ang = 10.0 + # --------------------------------------------- + # task weights + self.postural_weight = 1e1 * np.ones(len(self.postural_Kp)) + self.root_tracking_task_weight = 1e1 * np.ones(3) + self.x_k_init = np.concatenate( + ( + arm, + leg, + arm_kd, + leg_kd, + np.asarray( + [ + self.foot_tracking_task_kp_lin, + self.foot_tracking_task_kd_lin, + self.foot_tracking_task_kp_ang, + self.foot_tracking_task_kd_ang, + self.root_link_kp_ang, + self.root_link_kd_ang, + self.CoM_Kp, + self.CoM_Kd, + ] + ), + ) + ) - def set_postural_gain(self, arm, leg): - self.postural_Kp = np.concatenate([arm, arm, leg, leg]) + def set_postural_gain(self, arm, leg, arm_kd, leg_kd): + self.postural_Kp = np.concatenate([leg, leg, arm, arm]) + self.postural_kd = np.concatenate([leg_kd, leg_kd, arm_kd, arm_kd]) def set_foot_task(self, kp_lin, kd_lin, kp_ang, kd_ang): self.foot_tracking_task_kp_lin = kp_lin @@ -50,20 +60,20 @@ def set_foot_task(self, kp_lin, kd_lin, kp_ang, kd_ang): self.foot_tracking_task_kp_ang = kp_ang self.foot_tracking_task_kd_ang = kd_ang - def set_weights(self, postural_weight, root_weight): - self.postural_weight = postural_weight - self.root_tracking_task_weight = root_weight - def set_root_task(self, kp_ang, kd_ang): self.root_link_kp_ang = kp_ang self.root_link_kd_ang = kd_ang + def set_weights(self, postural_weight, root_weight): + self.postural_weight = postural_weight + self.root_tracking_task_weight = root_weight + def set_com_task(self, kp_com, kd_com): self.CoM_Kd = kd_com self.CoM_Kp = kp_com def set_from_x_k(self, x_k): - self.set_postural_gain(x_k[:4], x_k[4:10]) - self.set_foot_task(x_k[10], x_k[11], x_k[12], x_k[13]) - self.set_root_task(x_k[14], x_k[15]) - self.set_com_task(x_k[16], x_k[17]) + self.set_postural_gain(x_k[:4], x_k[4:10], x_k[10:14], x_k[14:20]) + self.set_foot_task(x_k[20], x_k[21], x_k[22], x_k[23]) + self.set_root_task(x_k[24], x_k[25]) + self.set_com_task(x_k[26], x_k[27]) diff --git a/src/comodo/drakeSimulator/__init__.py b/src/comodo/drakeSimulator/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/comodo/drakeSimulator/drakeSimulator.py b/src/comodo/drakeSimulator/drakeSimulator.py new file mode 100644 index 0000000..c0f718e --- /dev/null +++ b/src/comodo/drakeSimulator/drakeSimulator.py @@ -0,0 +1,243 @@ +import logging +import math + +import numpy as np +from odio_urdf import xml_to_odio +from pydrake.common.eigen_geometry import Quaternion +from pydrake.geometry import MeshcatVisualizer, StartMeshcat +from pydrake.math import RigidTransform +from pydrake.multibody.meshcat import ContactVisualizer, ContactVisualizerParams +from pydrake.multibody.parsing import Parser +from pydrake.multibody.plant import AddMultibodyPlantSceneGraph +from pydrake.systems.analysis import Simulator, SimulatorStatus +from pydrake.systems.framework import ( + Context, + DiagramBuilder, + InputPort, + InputPortIndex, + OutputPortIndex, + PortDataType, + System, +) + +from comodo.abstractClasses.simulator import Simulator as SimulatorAbstract +from comodo.drakeSimulator.utils import DrakeURDFHelper + +# the style of wrapping the simulator is inspired by the approach by DrakeGymEnv: +# https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/gym/_drake_gym_env.py#L177 + + +class DrakeSimulator(SimulatorAbstract): + def __init__(self) -> None: + self.duh = DrakeURDFHelper() + self.active_meshcat = False + super().__init__() + + def load_model(self, robot_model, s, xyz_rpy, kv_motors=None, Im=None): + # load the robot model construct the diagram and store the simulator + self.robot_model = robot_model + self.urdf_string = robot_model.urdf_string + + # convert the urdf string to be drake compatible + self.duh.load_urdf(urdf_string=self.urdf_string) + self.duh.remove_all_collisions() + self.duh.fix_not_in_joint_list(self.robot_model.joint_name_list) + self.duh.convert_xml_to_odio() + self.duh.add_acutation_tags() + self.urdf_string = self.duh.get_urdf_string() + + builder = DiagramBuilder() + self.time_step = 1e-3 + plant, scene_graph = AddMultibodyPlantSceneGraph( + builder, time_step=self.time_step + ) + parser = Parser(plant, scene_graph) + # TODO: Handle urdfs with mesh packages + # parser.package_map().Add("ergoCub", mesh_path) + robot_model_sim = parser.AddModels( + file_contents=self.urdf_string, + file_type="urdf", + )[0] + + # TODO: Use Im and Km to add motor params here + # ignoring those arguments for now + + # add the ground and the feet + self.duh.add_ground_with_friction(plant) + + # configure feet collisions + xMinMax = [-0.1, 0.1] + yMinMax = [-0.05, 0.05] + self.duh.add_soft_feet_collisions(plant, xMinMax=xMinMax, yMinMax=yMinMax) + + plant.Finalize() + builder.ExportInput(plant.get_actuation_input_port(), "control_input") + builder.ExportOutput(plant.get_state_output_port(), "state_output") + + # useful simulator details + self.nq = plant.num_positions() + self.nv = plant.num_velocities() + self.na = plant.num_actuators() + self.sim_joint_order = self.duh.get_sim_joint_order(plant, robot_model_sim) + # check if the joint ordering is the same + # logging.info( + # "Need joint mapping: ", not (sim_joint_order == robot_model.joint_name_list) + # ) + + # create useful variables + self.qpos = np.zeros(self.nq) + self.joint_torques = np.zeros(self.na) + + if self.visualize_robot_flag: + MeshcatVisualizer.AddToBuilder(builder, scene_graph, self.meshcat) + ContactVisualizer.AddToBuilder( + builder, + plant, + self.meshcat, + ContactVisualizerParams( + newtons_per_meter=1e3, newton_meters_per_meter=1e1 + ), + ) + + diagram = builder.Build() + self.simulator = Simulator(diagram) + + # now perform the setup + self._setup(s, xyz_rpy) + + def _setup(self, s, xyz_rpy): + # similar to DrakeGymEnv we need to expose the required ports + system = self.simulator.get_system() + self.state_output_port = system.GetOutputPort("state_output") + self.control_input_port = system.GetInputPort("control_input") + + self.context = self.simulator.get_mutable_context() + self.context.SetTime(0) + if not self.simulator.Initialize(): + logging.error("Drake simulator instance failed to initialize") + + self.simulator.get_system().SetDefaultContext(self.context) + + self.diagram = self.simulator.get_system() + plant = self.diagram.GetSubsystemByName("plant") + plant_context = self.diagram.GetMutableSubsystemContext(plant, self.context) + + self.set_base_pose_in_drake(xyz_rpy) + self.set_joint_vector_in_drake(s) + # set initial pose of the robot + plant.SetPositions(plant_context, self.qpos) + if self.visualize_robot_flag: + self.diagram.ForcedPublish(self.context) + + def reset(self): + logging.error("Drake simulator reset not implemented, recreate drake_instance") + pass + + def set_visualize_robot_flag(self, visualize_robot): + # pass meshcat to visualise the robot + self.visualize_robot_flag = visualize_robot + if self.visualize_robot_flag and not self.active_meshcat: + self.meshcat = StartMeshcat() + self.active_meshcat = True + pass + + # does this need to simulator specific? + def set_base_pose_in_drake(self, xyz_rpy): + base_xyz_quat = np.zeros(7) + # order -- quaternion+xyz + base_xyz_quat[:4] = self.RPY_to_quat(xyz_rpy[3], xyz_rpy[4], xyz_rpy[5]) + base_xyz_quat[4:] = xyz_rpy[:3] + self.qpos[:7] = base_xyz_quat + + # set initial positions of the joints + def set_joint_vector_in_drake(self, pos): + self.qpos[7:] = pos + pass + + def set_input(self, input): + # expose the acutation output port of the system to accept control + # inputs from the TSID and CentroidalMPC and step drake simulator + self.control_input_port.FixValue(self.context, input) + pass + + def step(self, n_step=1, visualize=True): + # TODO: does nothing currently + # self.control_input_port.FixValue(self.context, self.joint_torques) + self.simulator.AdvanceTo(self.context.get_time() + n_step * self.time_step) + if visualize and self.visualize_robot_flag: + self.diagram.ForcedPublish(self.context) + pass + + def step_with_motors(self, n_step, torque): + # implementation of motor level control inputs? + pass + + def get_base(self): + # this is for the state output return + # quat + pos + base_pos = self.state_output_port.Eval(self.context)[:7] + # normalise unnormalised quaternion + base_quat_wxyz = base_pos[:4] + H_b = RigidTransform( + Quaternion(base_quat_wxyz / (np.linalg.norm(base_quat_wxyz))), base_pos[4:] + ).GetAsMatrix4() + return H_b + + def get_base_velocity(self): + # this is for the state output return + base_vel = self.state_output_port.Eval(self.context)[self.nq : self.nq + 6] + # order -- linar_vel + angular_vel + return np.concatenate((base_vel[3:], base_vel[:3])) + + # TODO: Change from get_state to get_joint_state? + def get_state(self): + # this is for the state output return + robot_state = self.state_output_port.Eval(self.context) + robot_pos = robot_state[: self.nq] + robot_vel = robot_state[self.nq : self.nq + self.nv] + # return joint state to be coherent with the MuJoCo API + return ( + robot_pos[7:], + robot_vel[6:], + self.joint_torques, + ) + + def close(self): + pass + + def visualize_robot(self): + self.viewer.render() + + def get_simulation_time(self): + # get time from the simulator context + return self.context.get_time() + + def get_simulation_frequency(self): + # return the simulator timestep + return self.time_step + + # we will need similar stuff but with manifpy + def RPY_to_quat(self, roll, pitch, yaw): + cr = math.cos(roll / 2) + cp = math.cos(pitch / 2) + cy = math.cos(yaw / 2) + sr = math.sin(roll / 2) + sp = math.sin(pitch / 2) + sy = math.sin(yaw / 2) + + qw = cr * cp * cy + sr * sp * sy + qx = sr * cp * cy - cr * sp * sy + qy = cr * sp * cy + sr * cp * sy + qz = cr * cp * sy - sr * sp * cy + + # Note: The order for drake is different as compared to mujoco + return [qw, qx, qy, qz] + + def close_visualization(self): + self.meshcat.Delete() + logging.info( + "Drake uses meshcat for visualization. Close browser tab to close \ + the visualisation." + ) + self.reset() + pass diff --git a/src/comodo/drakeSimulator/utils.py b/src/comodo/drakeSimulator/utils.py new file mode 100644 index 0000000..88c22f5 --- /dev/null +++ b/src/comodo/drakeSimulator/utils.py @@ -0,0 +1,209 @@ +# some useful functions to load robot models +# into the drake simulator + +import logging +import os +import sys +import tempfile +import xml.etree.ElementTree as ET +from pathlib import Path + +import meshio +import numpy as np +from odio_urdf import * +from pydrake.geometry import ( + AddCompliantHydroelasticProperties, + AddContactMaterial, + AddRigidHydroelasticProperties, +) +from pydrake.geometry import Box as BoxDrake +from pydrake.geometry import HalfSpace, ProximityProperties +from pydrake.math import RigidTransform +from pydrake.multibody.plant import CoulombFriction +from tqdm import tqdm + + +class DrakeURDFHelper: + """A simple class to make a given URDF drake compatible""" + + def __init__(self): + pass + + def load_urdf(self, urdf_path=None, urdf_string=None, mesh_path=None): + # read the urdf + self.urdf_path = str(urdf_path) + # save the string temporarily to parse the xml + # TODO: This function blows up if not careful during an optimisation + if urdf_string is not None: + tmpfile = tempfile.NamedTemporaryFile(mode="w+") + tmpfile.write(urdf_string) + self.urdf_path = tmpfile.name + + self.mesh_path = mesh_path + self.urdf_out_path = os.getcwd() + "/urdfs" + + # load the xml from the urdf_string + tree = ET.parse(self.urdf_path) + self.root = tree.getroot() + + def convert_meshes_to_obj(self): + """Currently drake only loads .obj files and not .stl files. + This is a simple routine to allow this conversion""" + # read the meshes and convert them from .stl to .obj so that they can be loaded to Drake + logging.info("Converting all the meshes") + for child in tqdm( + self.root.findall("./link/visual/geometry/mesh") + or self.root.findall("./link/collision/geometry/mesh") + ): + # extract mesh location and name associated with the urdf + path = child.attrib["filename"] + child_mesh_path = self.mesh_path + path.split("package://")[1] + # convert the mesh and save it in the same folder + child_mesh_name = child_mesh_path.replace("stl", "obj") + if not Path(child_mesh_name).is_file(): + temp_mesh = meshio.read(child_mesh_path) + temp_mesh.write(child_mesh_name) + # replace the urdf to load .obj files + child.set("filename", path.replace("stl", "obj")) + + # # if the same, will be ignored else converted + # logging.info("Converting all the collision meshes") + # for child in tqdm(self.root.findall("./link/collision/geometry/mesh")): + # # extract mesh location and name associated with the urdf + # path = child.attrib["filename"] + # child_mesh_path = self.mesh_path + path.split("package://")[1] + # # convert the mesh and save it in the same folder + # child_mesh_name = child_mesh_path.replace("stl", "obj") + # if not Path(child_mesh_name).is_file(): + # # temp_mesh = pymesh.load_mesh(child_mesh_path) + # # pymesh.save_mesh(child_mesh_name, temp_mesh) + # temp_mesh = meshio.read(child_mesh_path) + # temp_mesh.write(child_mesh_name) + # # replace the urdf to load .obj files + # child.set("filename", path.replace("stl", "obj")) + + logging.info("Converted all the meshes from .stl to .obj") + + def remove_all_collisions(self): + """Removes all the collision tags from the URDF""" + list = [] + for link in self.root.findall("./link"): + if link.attrib["name"] not in list: + for col in link.findall("./collision"): + link.remove(col) + + def fix_not_in_joint_list(self, red_joint_name_list): + """Converts any joint not in the + reduced joint name list (red_joint_name_list) into fixed joints""" + joints = self.root.findall("./joint") + for j in joints: + if j.attrib["name"] not in red_joint_name_list: + j.attrib["type"] = "fixed" + + def convert_xml_to_odio(self): + """Converts the loaded xml to odio urdf format""" + # convert the xml to odio urdf format + self.odio_robot_dsl = xml_to_odio(self.root) + # blockPrint() + self.odio_robot = eval(self.odio_robot_dsl) + # enablePrint() + + def add_acutation_tags(self): + """Adds the actuation tags to the odio urdf""" + # extract all the non-fixed joints + joints = self.root.findall("./joint") + joint_names = [] + for j in joints: + if j.attrib["type"] != "fixed": + joint_names.append([j.attrib["name"], j.attrib["type"]]) + + # add actuation tags to the odio urdf + for j in joint_names: + actuator_name = "actuator_" + j[0] + transmission_name = "transmission" + j[0] + temp_trans = Transmission( + Type("SimpleTransmission"), + Actuator(Mechanicalreduction("1"), name=actuator_name), + Transjoint(Hardwareinterface("EffortJointInterface"), j[0]), + name=transmission_name, + ) + # Add the transmission to the robot URDF + self.odio_robot(temp_trans) + + def write_to_file(self, urdf_name): + """Writes the odio urdf to a file""" + # create a folder in the current directory called urdfs + if not os.path.exists(self.urdf_out_path): + os.makedirs(self.urdf_out_path) + filename = self.urdf_out_path + "/" + urdf_name + ".urdf" + with open(filename, "w") as f: + print(self.odio_robot, file=f) + + logging.info("Saved the urdf to {}".format(filename)) + + def get_urdf_string(self): + """Returns the urdf string""" + return str(self.odio_robot) + + def get_sim_joint_order(self, plant, robot_model_sim): + sim_joint_order = [] + for ii in plant.GetJointIndices(robot_model_sim): + jj = plant.get_joint(ii) + if jj.type_name() == "revolute": + sim_joint_order.append(jj.name()) + return sim_joint_order + + # add ground with friction + def add_ground_with_friction(self, plant): + surface_friction_ground = CoulombFriction( + static_friction=1.0, dynamic_friction=1.0 + ) + proximity_properties_ground = ProximityProperties() + AddContactMaterial( + 1e4, 1e7, surface_friction_ground, proximity_properties_ground + ) + AddRigidHydroelasticProperties(0.01, proximity_properties_ground) + + plant.RegisterCollisionGeometry( + plant.world_body(), + RigidTransform(), + HalfSpace(), + "ground_collision", + proximity_properties_ground, + ) + plant.RegisterVisualGeometry( + plant.world_body(), + RigidTransform(), + HalfSpace(), + "ground_visual", + np.array([0.5, 0.5, 0.5, 0.0]), + ) + + def add_soft_feet_collisions(self, plant, xMinMax, yMinMax): + surface_friction_feet = CoulombFriction( + static_friction=1.0, dynamic_friction=1.0 + ) + proximity_properties_feet = ProximityProperties() + AddContactMaterial(1e4, 1e7, surface_friction_feet, proximity_properties_feet) + AddCompliantHydroelasticProperties(0.01, 1e8, proximity_properties_feet) + for ii in ["l_foot_front", "l_foot_rear", "r_foot_front", "r_foot_rear"]: + # for ii in ["r_foot", "l_foot"]: + # for collision + plant.RegisterCollisionGeometry( + plant.GetBodyByName(ii), + RigidTransform(np.array([0, 0, 0])), + # RigidTransform(), + BoxDrake((xMinMax[1] - xMinMax[0]) / 2, yMinMax[1] - yMinMax[0], 2e-3), + ii + "_collision", + proximity_properties_feet, + ) + + # for visual + plant.RegisterVisualGeometry( + plant.GetBodyByName(ii), + RigidTransform(np.array([0, 0, 0])), + # RigidTransform(), + BoxDrake((xMinMax[1] - xMinMax[0]) / 2, yMinMax[1] - yMinMax[0], 2e-3), + ii + "_collision", + np.array([1.0, 1.0, 1.0, 1]), + )