EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
Go2Py_SIM/examples/09-crododdyl.ipynb

4906 lines
459 KiB
Plaintext
Raw Blame History

This file contains invisible Unicode characters

This file contains invisible Unicode characters that are indistinguishable to humans but may be processed differently by a computer. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## OfflineTrajectory Optimization"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import pinocchio as pin\n",
"import crocoddyl\n",
"import pinocchio\n",
"import numpy as np\n",
"urdf_root_path = '/home/Go2py/Go2Py/assets'\n",
"urdf_path = '/home/Go2py/Go2Py/assets/urdf/go2_with_arm.urdf'\n",
"robot = pin.RobotWrapper.BuildFromURDF(\n",
"urdf_path, urdf_root_path, pin.JointModelFreeFlyer())"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import sys\n",
"import mim_solvers\n",
"pinRef = pin.LOCAL_WORLD_ALIGNED\n",
"FRICTION_CSTR = True\n",
"MU = 0.8 # friction coefficient"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"ee_frame_names = ['FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT', 'Link6']\n",
"rmodel = robot.model\n",
"rdata = robot.data\n",
"# # set contact frame_names and_indices\n",
"lfFootId = rmodel.getFrameId(ee_frame_names[0])\n",
"rfFootId = rmodel.getFrameId(ee_frame_names[1])\n",
"lhFootId = rmodel.getFrameId(ee_frame_names[2])\n",
"rhFootId = rmodel.getFrameId(ee_frame_names[3])\n",
"efId = rmodel.getFrameId(ee_frame_names[4])"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"q0 = np.array([0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 1.0] \n",
" +4*[0.0, 0.77832842, -1.56065452] + 8*[0.0]\n",
" )\n",
"x0 = np.concatenate([q0, np.zeros(rmodel.nv)])"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"q0 = np.array([0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 1.0] \n",
" +4*[0.0, 0.77832842, -1.56065452] + 8*[0.0]\n",
" )\n",
"x0 = np.concatenate([q0, np.zeros(rmodel.nv)])\n",
"\n",
"pinocchio.forwardKinematics(rmodel, rdata, q0)\n",
"pinocchio.updateFramePlacements(rmodel, rdata)\n",
"rfFootPos0 = rdata.oMf[rfFootId].translation\n",
"rhFootPos0 = rdata.oMf[rhFootId].translation\n",
"lfFootPos0 = rdata.oMf[lfFootId].translation\n",
"lhFootPos0 = rdata.oMf[lhFootId].translation \n",
"efPos0 = rdata.oMf[efId].translation"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4\n",
"comRef[2] = pinocchio.centerOfMass(rmodel, rdata, q0)[2].item() \n",
"print(f'The desired CoM position is: {comRef}')"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"supportFeetIds = [lfFootId, rfFootId, lhFootId, rhFootId]\n",
"supportFeePos = [lfFootPos0, rfFootPos0, lhFootPos0, rhFootPos0]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"state = crocoddyl.StateMultibody(rmodel)\n",
"actuation = crocoddyl.ActuationModelFloatingBase(state)\n",
"nu = actuation.nu"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"comDes = []\n",
"\n",
"N_ocp = 250 #100\n",
"dt = 0.02\n",
"T = N_ocp * dt\n",
"radius = 0.065\n",
"for t in range(N_ocp+1):\n",
" comDes_t = comRef.copy()\n",
" w = (2 * np.pi) * 0.2 # / T\n",
" comDes_t[0] += radius * np.sin(w * t * dt) \n",
" comDes_t[1] += radius * (np.cos(w * t * dt) - 1)\n",
" comDes += [comDes_t]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import friction_utils\n",
"running_models = []\n",
"constraintModels = []\n",
"\n",
"for t in range(N_ocp+1):\n",
" contactModel = crocoddyl.ContactModelMultiple(state, nu)\n",
" costModel = crocoddyl.CostModelSum(state, nu)\n",
"\n",
" # Add contact\n",
" for frame_idx in supportFeetIds:\n",
" support_contact = crocoddyl.ContactModel3D(state, frame_idx, np.array([0., 0., 0.]), pinRef, nu, np.array([0., 0.]))\n",
" # print(\"contact name = \", rmodel.frames[frame_idx].name + \"_contact\")\n",
" contactModel.addContact(rmodel.frames[frame_idx].name + \"_contact\", support_contact) \n",
"\n",
" # Add state/control reg costs\n",
"\n",
" state_reg_weight, control_reg_weight = 1e-1, 1e-3\n",
"\n",
" freeFlyerQWeight = [0.]*3 + [500.]*3\n",
" freeFlyerVWeight = [10.]*6\n",
" legsQWeight = [0.01]*(rmodel.nv - 6)\n",
" legsWWeights = [1.]*(rmodel.nv - 6)\n",
" stateWeights = np.array(freeFlyerQWeight + legsQWeight + freeFlyerVWeight + legsWWeights) \n",
"\n",
"\n",
" stateResidual = crocoddyl.ResidualModelState(state, x0, nu)\n",
" stateActivation = crocoddyl.ActivationModelWeightedQuad(stateWeights**2)\n",
" stateReg = crocoddyl.CostModelResidual(state, stateActivation, stateResidual)\n",
"\n",
" if t == N_ocp:\n",
" costModel.addCost(\"stateReg\", stateReg, state_reg_weight*dt)\n",
" else:\n",
" costModel.addCost(\"stateReg\", stateReg, state_reg_weight)\n",
"\n",
" if t != N_ocp:\n",
" ctrlResidual = crocoddyl.ResidualModelControl(state, nu)\n",
" ctrlReg = crocoddyl.CostModelResidual(state, ctrlResidual)\n",
" costModel.addCost(\"ctrlReg\", ctrlReg, control_reg_weight) \n",
"\n",
"\n",
" # Add COM task\n",
" com_residual = crocoddyl.ResidualModelCoMPosition(state, comDes[t], nu)\n",
" com_activation = crocoddyl.ActivationModelWeightedQuad(np.array([1., 1., 1.]))\n",
" com_track = crocoddyl.CostModelResidual(state, com_activation, com_residual) # What does it correspond to exactly?\n",
" if t == N_ocp:\n",
" costModel.addCost(\"comTrack\", com_track, 1e5)\n",
" else:\n",
" costModel.addCost(\"comTrack\", com_track, 1e5)\n",
"\n",
" # End Effecor Position Task\n",
" ef_residual = crocoddyl.ResidualModelFrameTranslation(state, efId, efPos0, nu)\n",
" ef_activation = crocoddyl.ActivationModelWeightedQuad(np.array([1., 1., 1.]))\n",
" ef_track = crocoddyl.CostModelResidual(state, ef_activation, ef_residual)\n",
" if t == N_ocp:\n",
" costModel.addCost(\"efTrack\", ef_track, 1e5)\n",
" else:\n",
" costModel.addCost(\"efTrack\", ef_track, 1e5)\n",
" \n",
"\n",
" constraintModelManager = crocoddyl.ConstraintModelManager(state, actuation.nu)\n",
" if(FRICTION_CSTR):\n",
" if(t != N_ocp):\n",
" for frame_idx in supportFeetIds:\n",
" name = rmodel.frames[frame_idx].name + \"_contact\"\n",
" residualFriction = friction_utils.ResidualFrictionCone(state, name, MU, actuation.nu)\n",
" constraintFriction = crocoddyl.ConstraintModelResidual(state, residualFriction, np.array([0.]), np.array([np.inf]))\n",
" constraintModelManager.addConstraint(name + \"friction\", constraintFriction)\n",
"\n",
" dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(state, actuation, contactModel, costModel, constraintModelManager, 0., True)\n",
" model = crocoddyl.IntegratedActionModelEuler(dmodel, dt)\n",
"\n",
" running_models += [model]\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Create shooting problem\n",
"ocp = crocoddyl.ShootingProblem(x0, running_models[:-1], running_models[-1])\n",
"\n",
"solver = mim_solvers.SolverCSQP(ocp)\n",
"solver.max_qp_iters = 1000\n",
"max_iter = 500\n",
"solver.with_callbacks = True\n",
"solver.use_filter_line_search = False\n",
"solver.termination_tolerance = 1e-4\n",
"solver.eps_abs = 1e-6\n",
"solver.eps_rel = 1e-6"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"xs = [x0]*(solver.problem.T + 1)\n",
"us = solver.problem.quasiStatic([x0]*solver.problem.T) \n",
"solver.solve(xs, us, max_iter) "
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"nq, nv, N = rmodel.nq, rmodel.nv, len(xs) \n",
"jointPos_sol = []\n",
"jointVel_sol = []\n",
"jointAcc_sol = []\n",
"jointTorques_sol = []\n",
"centroidal_sol = []\n",
"xs, us = solver.xs, solver.us\n",
"x = []\n",
"for time_idx in range (N):\n",
" q, v = xs[time_idx][:nq], xs[time_idx][nq:]\n",
" pin.framesForwardKinematics(rmodel, rdata, q)\n",
" pin.computeCentroidalMomentum(rmodel, rdata, q, v)\n",
" centroidal_sol += [\n",
" np.concatenate(\n",
" [pin.centerOfMass(rmodel, rdata, q, v), np.array(rdata.hg.linear), np.array(rdata.hg.angular)]\n",
" )\n",
" ]\n",
" jointPos_sol += [q]\n",
" jointVel_sol += [v]\n",
" x += [xs[time_idx]]\n",
" if time_idx < N-1:\n",
" jointAcc_sol += [solver.problem.runningDatas[time_idx].xnext[nq::]] \n",
" jointTorques_sol += [us[time_idx]]\n",
"\n",
"\n",
"\n",
"\n",
"sol = {'x':x, 'centroidal':centroidal_sol, 'jointPos':jointPos_sol, \n",
" 'jointVel':jointVel_sol, 'jointAcc':jointAcc_sol, \n",
" 'jointTorques':jointTorques_sol} \n",
"\n",
"for frame_idx in supportFeetIds:\n",
" # print('extract foot id ', frame_idx, \"_name = \", rmodel.frames[frame_idx].name)\n",
" ct_frame_name = rmodel.frames[frame_idx].name + \"_contact\"\n",
" datas = [solver.problem.runningDatas[i].differential.multibody.contacts.contacts[ct_frame_name] for i in range(N-1)]\n",
" ee_forces = [datas[k].f.vector for k in range(N-1)] \n",
" sol[ct_frame_name] = [ee_forces[i] for i in range(N-1)] \n",
" "
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import matplotlib.pyplot as plt\n",
"constrained_sol = sol\n",
"time_lin = np.linspace(0, T, solver.problem.T)\n",
"fig, axs = plt.subplots(4, 3, constrained_layout=True)\n",
"for i, frame_idx in enumerate(supportFeetIds):\n",
" ct_frame_name = rmodel.frames[frame_idx].name + \"_contact\"\n",
" forces = np.array(constrained_sol[ct_frame_name])\n",
" axs[i, 0].plot(time_lin, forces[:, 0], label=\"Fx\")\n",
" axs[i, 1].plot(time_lin, forces[:, 1], label=\"Fy\")\n",
" axs[i, 2].plot(time_lin, forces[:, 2], label=\"Fz\")\n",
" # Add friction cone constraints \n",
" Fz_lb = (1./MU)*np.sqrt(forces[:, 0]**2 + forces[:, 1]**2)\n",
" # Fz_ub = np.zeros(time_lin.shape)\n",
" # axs[i, 2].plot(time_lin, Fz_ub, 'k-.', label='ub')\n",
" axs[i, 2].plot(time_lin, Fz_lb, 'k-.', label='lb')\n",
" axs[i, 0].grid()\n",
" axs[i, 1].grid()\n",
" axs[i, 2].grid()\n",
" axs[i, 0].set_ylabel(ct_frame_name)\n",
"axs[0, 0].legend()\n",
"axs[0, 1].legend()\n",
"axs[0, 2].legend()\n",
"\n",
"axs[3, 0].set_xlabel(r\"$F_x$\")\n",
"axs[3, 1].set_xlabel(r\"$F_y$\")\n",
"axs[3, 2].set_xlabel(r\"$F_z$\")\n",
"fig.suptitle('Force', fontsize=16)\n",
"\n",
"\n",
"comDes = np.array(comDes)\n",
"centroidal_sol = np.array(constrained_sol['centroidal'])\n",
"plt.figure()\n",
"plt.plot(comDes[:, 0], comDes[:, 1], \"--\", label=\"reference\")\n",
"plt.plot(centroidal_sol[:, 0], centroidal_sol[:, 1], label=\"solution\")\n",
"plt.legend()\n",
"plt.xlabel(\"x\")\n",
"plt.ylabel(\"y\")\n",
"plt.title(\"COM trajectory\")\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"mujoco_joint_names = \\\n",
"['FL_hip_joint',\n",
" 'FL_thigh_joint',\n",
" 'FL_calf_joint',\n",
" 'FR_hip_joint',\n",
" 'FR_thigh_joint',\n",
" 'FR_calf_joint',\n",
" 'RL_hip_joint',\n",
" 'RL_thigh_joint',\n",
" 'RL_calf_joint',\n",
" 'RR_hip_joint',\n",
" 'RR_thigh_joint',\n",
" 'RR_calf_joint',\n",
" 'Joint1',\n",
" 'Joint2',\n",
" 'Joint3',\n",
" 'Joint4',\n",
" 'Joint5',\n",
" 'Joint6',\n",
" 'Joint7_1',\n",
" 'Joint7_2']\n",
"\n",
"for joint_name in mujoco_joint_names:\n",
" joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)\n",
" print(f'Joint {joint_name} has id {joint_id}')\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"pinocchio_joint_names = \\\n",
"['FL_HAA_joint',\n",
" 'FL_HFE_joint',\n",
" 'FL_KFE_joint',\n",
" 'FR_HAA_joint',\n",
" 'FR_HFE_joint',\n",
" 'FR_KFE_joint',\n",
" 'HL_HAA_joint',\n",
" 'HL_HFE_joint',\n",
" 'HL_KFE_joint',\n",
" 'HR_HAA_joint',\n",
" 'HR_HFE_joint',\n",
" 'HR_KFE_joint',\n",
" 'Joint1',\n",
" 'Joint2',\n",
" 'Joint3',\n",
" 'Joint4',\n",
" 'Joint5',\n",
" 'Joint6',\n",
" 'Joint7_1',\n",
" 'Joint7_2']\n",
"\n",
"\n",
"for joint_name in pinocchio_joint_names:\n",
" joint_id = rmodel.getJointId(joint_name)\n",
" print(f'Joint {joint_name} has id {joint_id}')\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"pin_idx_to_mujoco_idx = []\n",
"for i in range(len(pinocchio_joint_names)):\n",
" joint_name = pinocchio_joint_names[i]\n",
" pin_joint_id = rmodel.getJointId(joint_name)\n",
" mujoco_joint_name = mujoco_joint_names[i]\n",
" mujoco_joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, mujoco_joint_name)\n",
" pin_idx_to_mujoco_idx.append(mujoco_joint_id)\n",
"\n",
"pin_idx_to_mujoco_idx = np.array(pin_idx_to_mujoco_idx) - min(pin_idx_to_mujoco_idx)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import mujoco\n",
"model = mujoco.MjModel.from_xml_path('/home/Go2py/Go2Py/assets/mujoco/go2_with_arm.xml')\n",
"data = mujoco.MjData(model)\n",
"viewer = mujoco.viewer.launch_passive(model, data)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import time\n",
"for k in range(1000):\n",
" i = k % len(xs)\n",
" q0 = xs[i][:27]\n",
" qx = q0[3]\n",
" qy = q0[4]\n",
" qz = q0[5]\n",
" qw = q0[6]\n",
" q0[3:7] = [qw, qx, qy, qz]\n",
" q0[7:] = q0[7:][pin_idx_to_mujoco_idx]\n",
" data.qpos[:] = q0\n",
"\n",
" mujoco.mj_step(model, data)\n",
" viewer.sync()\n",
" time.sleep(1/60)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Online MPC"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"from Go2Py.sim.mujoco import Go2Sim\n",
"import numpy as np\n",
"robot=Go2Sim(with_arm=True)"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"map = np.zeros((1200, 1200))\n",
"# map[:,649:679] = 400\n",
"map[:,653:679] = 400\n",
"robot.updateHeightMap(map)"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"import os\n",
"import sys\n",
"import mim_solvers\n",
"import friction_utils\n",
"import mujoco\n",
"import pinocchio as pin\n",
"import crocoddyl\n",
"import pinocchio\n",
"import numpy as np\n",
"import mujoco.viewer\n",
"\n",
"class Go2MPC:\n",
" def __init__(self, assets_path, HORIZON=250, friction_mu = 0.2, dt = 0.02):\n",
" self.assets_path = assets_path\n",
" self.HORIZON = HORIZON\n",
" self.max_iterations = 500\n",
" self.dt = dt\n",
" self.urdf_path = os.path.join(assets_path, 'urdf/go2_with_arm.urdf')\n",
" self.xml_path = os.path.join(assets_path, 'mujoco/go2_with_arm.xml')\n",
" self.pin_robot = pin.RobotWrapper.BuildFromURDF(self.urdf_path, self.assets_path, pin.JointModelFreeFlyer())\n",
" self.pinRef = pin.LOCAL_WORLD_ALIGNED\n",
" self.friction_mu = friction_mu \n",
" self.rmodel = self.pin_robot.model\n",
" self.rdata = self.pin_robot.data\n",
"\n",
" self.mpc_to_unitree_name_map = \\\n",
" {'FL_HAA_joint':'FL_hip_joint',\n",
" 'FL_HFE_joint':'FL_thigh_joint',\n",
" 'FL_KFE_joint':'FL_calf_joint',\n",
" 'FR_HAA_joint':'FR_hip_joint',\n",
" 'FR_HFE_joint':'FR_thigh_joint',\n",
" 'FR_KFE_joint':'FR_calf_joint',\n",
" 'HL_HAA_joint':'RL_hip_joint',\n",
" 'HL_HFE_joint':'RL_thigh_joint',\n",
" 'HL_KFE_joint':'RL_calf_joint',\n",
" 'HR_HAA_joint':'RR_hip_joint',\n",
" 'HR_HFE_joint': 'RR_thigh_joint',\n",
" 'HR_KFE_joint': 'RR_calf_joint',\n",
" 'Joint1':'Joint1',\n",
" 'Joint2':'Joint2',\n",
" 'Joint3':'Joint3',\n",
" 'Joint4':'Joint4',\n",
" 'Joint5':'Joint5',\n",
" 'Joint6':'Joint6'\n",
" }\n",
" self.unitree_to_mpc_name_map = {val:key for key, val in self.mpc_to_unitree_name_map.items()}\n",
" self.unitree_joint_order = ['FR', 'FL', 'RR', 'RL']\n",
"\n",
" mpc_to_unitree_idx_map = {}\n",
" unitree_id = 0\n",
" for foot_name in self.unitree_joint_order:\n",
" for actuator in ['_hip_joint', '_thigh_joint', '_calf_joint']:\n",
" unitree_actuator_name = foot_name+actuator\n",
" mpc_joint_name = self.unitree_to_mpc_name_map[unitree_actuator_name]\n",
" mpc_joint_id = self.rmodel.getJointId(mpc_joint_name)\n",
" mpc_to_unitree_idx_map[mpc_joint_id-2]=unitree_id\n",
" unitree_id+=1\n",
"\n",
" for unitree_actuator_name in ['Joint1', 'Joint2', 'Joint3', 'Joint4', 'Joint5', 'Joint6']:\n",
" mpc_joint_name = self.unitree_to_mpc_name_map[unitree_actuator_name]\n",
" mpc_joint_id = self.rmodel.getJointId(mpc_joint_name)\n",
" mpc_to_unitree_idx_map[mpc_joint_id-2]=unitree_id\n",
" unitree_id+=1\n",
"\n",
" self.mpc_to_unitree_idx = np.zeros(18).astype(np.int32) # mpc_state[mpc_to_unitree_idx] -> state/command in unitree order \n",
" self.unitree_to_mpc_idx = np.zeros(18).astype(np.int32) # unitree_state[unitree_to_mpc] -> state/command in mpc order\n",
" for mpc_idx, unitree_idx in mpc_to_unitree_idx_map.items():\n",
" self.mpc_to_unitree_idx[mpc_idx] = unitree_idx\n",
" self.unitree_to_mpc_idx[unitree_idx] = mpc_idx\n",
"\n",
" # set contact frame_names and_indices\n",
" ee_frame_names = ['FL_FOOT', 'FR_FOOT', 'HL_FOOT', 'HR_FOOT', 'Link6']\n",
" self.lfFootId = self.rmodel.getFrameId(ee_frame_names[0])\n",
" self.rfFootId = self.rmodel.getFrameId(ee_frame_names[1])\n",
" self.lhFootId = self.rmodel.getFrameId(ee_frame_names[2])\n",
" self.rhFootId = self.rmodel.getFrameId(ee_frame_names[3])\n",
" self.armEEId = self.rmodel.getFrameId(ee_frame_names[4])\n",
" self.running_models = []\n",
" self.constraintModels = []\n",
" \n",
" self.ccdyl_state = crocoddyl.StateMultibody(self.rmodel)\n",
" self.ccdyl_actuation = crocoddyl.ActuationModelFloatingBase(self.ccdyl_state)\n",
" self.nu = self.ccdyl_actuation.nu\n",
"\n",
" def initialize(self, q0=np.array([0.0, 0.0, 0.33, 0.0, 0.0, 0.0, 1.0] \n",
" +4*[0.0, 0.77832842, -1.56065452] + [0.0, 0.3, -0.3, 0.0, 0.0, 0.0]\n",
" )):\n",
" q0[11+2]=0.0\n",
" self.q0 = q0.copy()\n",
" self.x0 = np.concatenate([q0, np.zeros(self.rmodel.nv)])\n",
" pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)\n",
" pinocchio.updateFramePlacements(self.rmodel, self.rdata)\n",
" self.rfFootPos0 = self.rdata.oMf[self.rfFootId].translation\n",
" self.rhFootPos0 = self.rdata.oMf[self.rhFootId].translation\n",
" self.lfFootPos0 = self.rdata.oMf[self.lfFootId].translation\n",
" self.lhFootPos0 = self.rdata.oMf[self.lhFootId].translation \n",
" self.armEEPos0 = self.rdata.oMf[self.armEEId].translation\n",
" self.armEEOri0 = self.rdata.oMf[self.armEEId].rotation\n",
" self.supportFeetIds = [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId]\n",
" # self.supportFeePos = [self.lfFootPos0, self.rfFootPos0, self.lhFootPos0, self.rhFootPos0]\n",
" self.xs = [self.x0]*(self.HORIZON + 1)\n",
" self.createProblem()\n",
" self.createSolver()\n",
" self.us = self.solver.problem.quasiStatic([self.x0]*self.HORIZON) \n",
"\n",
" def createProblem(self):\n",
" #First compute the desired state of the robot\n",
" comRef = (self.rfFootPos0 + self.rhFootPos0 + self.lfFootPos0 + self.lhFootPos0) / 4\n",
" comRef[2] = pinocchio.centerOfMass(self.rmodel, self.rdata, self.q0)[2].item() \n",
" eeDes = self.armEEPos0\n",
" comDes = []\n",
" dt = self.dt\n",
" # radius = 0.065\n",
" radius = 0.0\n",
" for t in range(self.HORIZON+1):\n",
" comDes_t = comRef.copy()\n",
" w = (2 * np.pi) * 0.2 # / T\n",
" comDes_t[0] += radius * np.sin(w * t * dt) \n",
" comDes_t[1] += radius * (np.cos(w * t * dt) - 1)\n",
" comDes += [comDes_t]\n",
" body_com_ref = comDes\n",
" arm_eff_ref = [eeDes]*(self.HORIZON+1)\n",
" # Now define the model\n",
" for t in range(self.HORIZON+1):\n",
" self.contactModel = crocoddyl.ContactModelMultiple(self.ccdyl_state, self.nu)\n",
" costModel = crocoddyl.CostModelSum(self.ccdyl_state, self.nu)\n",
"\n",
" # Add contacts\n",
" for frame_idx in self.supportFeetIds:\n",
" support_contact = crocoddyl.ContactModel3D(self.ccdyl_state, frame_idx, np.array([0., 0., 0.]), self.pinRef, self.nu, np.array([0., 0.]))\n",
" self.contactModel.addContact(self.rmodel.frames[frame_idx].name + \"_contact\", support_contact) \n",
"\n",
" # Contact for the EE\n",
" support_contact = crocoddyl.ContactModel3D(self.ccdyl_state, self.armEEId, np.array([0., 0., 0.0]), pin.LOCAL_WORLD_ALIGNED, self.nu, np.array([0., 0.]))\n",
" self.contactModel.addContact(self.rmodel.frames[self.armEEId].name + \"_contact\", support_contact) \n",
" # Add state/control regularization costs\n",
" state_reg_weight, control_reg_weight = 1e-1, 1e-3\n",
"\n",
" freeFlyerQWeight = [0.]*3 + [500.]*3\n",
" freeFlyerVWeight = [10.]*6\n",
" legsQWeight = [0.01]*(self.rmodel.nv - 6)\n",
" legsWWeights = [1.]*(self.rmodel.nv - 6)\n",
" stateWeights = np.array(freeFlyerQWeight + legsQWeight + freeFlyerVWeight + legsWWeights) \n",
" stateResidual = crocoddyl.ResidualModelState(self.ccdyl_state, self.x0, self.nu)\n",
"\n",
" stateActivation = crocoddyl.ActivationModelWeightedQuad(stateWeights**2)\n",
" stateReg = crocoddyl.CostModelResidual(self.ccdyl_state, stateActivation, stateResidual)\n",
"\n",
" if t == self.HORIZON:\n",
" costModel.addCost(\"stateReg\", stateReg, state_reg_weight*self.dt)\n",
" else:\n",
" costModel.addCost(\"stateReg\", stateReg, state_reg_weight)\n",
"\n",
" if t != self.HORIZON:\n",
" ctrlResidual = crocoddyl.ResidualModelControl(self.ccdyl_state, self.nu)\n",
" ctrlReg = crocoddyl.CostModelResidual(self.ccdyl_state, ctrlResidual)\n",
" costModel.addCost(\"ctrlReg\", ctrlReg, control_reg_weight) \n",
"\n",
"\n",
" # Body COM Tracking Cost\n",
" com_residual = crocoddyl.ResidualModelCoMPosition(self.ccdyl_state, body_com_ref[t], self.nu)\n",
" com_activation = crocoddyl.ActivationModelWeightedQuad(np.array([1., 1., 1.]))\n",
" com_track = crocoddyl.CostModelResidual(self.ccdyl_state, com_activation, com_residual) # What does it correspond to exactly?\n",
" # if t == self.HORIZON:\n",
" # # costModel.addCost(\"comTrack\", com_track, 1e5)\n",
" # costModel.addCost(\"comTrack\", com_track, 1e5)\n",
" # else:\n",
" # costModel.addCost(\"comTrack\", com_track, 1e1)\n",
"\n",
" # End Effecor Position Tracking Cost\n",
" # ef_residual = crocoddyl.ResidualModelFrameTranslation(self.ccdyl_state, self.armEEId, arm_eff_ref[t], self.nu) # Check this cost term \n",
" # ef_activation = crocoddyl.ActivationModelWeightedQuad(np.array([1., 1., 1.]))\n",
" # ef_track = crocoddyl.CostModelResidual(self.ccdyl_state, ef_activation, ef_residual)\n",
"\n",
" ef_rotation_residual = crocoddyl.ResidualModelFrameRotation(self.ccdyl_state, self.armEEId, self.armEEOri0, self.nu)\n",
" ef_rot_activation = crocoddyl.ActivationModelWeightedQuad(np.array([1., 1., 1.]))\n",
" ef_rot_track = crocoddyl.CostModelResidual(self.ccdyl_state, ef_rot_activation, ef_rotation_residual)\n",
" if t == self.HORIZON:\n",
" # costModel.addCost(\"efTrack\", ef_track, 1e5)\n",
" costModel.addCost(\"efRotTrack\", ef_rot_track, 1e5)\n",
" \n",
" else:\n",
" # costModel.addCost(\"efTrack\", ef_track, 1e1)\n",
" costModel.addCost(\"efRotTrack\", ef_rot_track, 1e1)\n",
" # Force tracking term\n",
" self.ef_des_force = pin.Force()\n",
" self.ef_des_force.linear[2] = 0\n",
" contact_force_residual = crocoddyl.ResidualModelContactForce(self.ccdyl_state, self.armEEId, self.ef_des_force, 3, self.nu)\n",
" contact_force_activation = crocoddyl.ActivationModelWeightedQuad(np.array([1., 1., 1.]))\n",
" contact_force_track = crocoddyl.CostModelResidual(self.ccdyl_state, contact_force_activation, contact_force_residual)\n",
" if t == self.HORIZON:\n",
" costModel.addCost(\"contact_force_track\", contact_force_track, 1e5)\n",
" else:\n",
" costModel.addCost(\"contact_force_track\", contact_force_track, 1e1)\n",
" \n",
" \n",
" # Friction Cone Constraints\n",
" constraintModelManager = crocoddyl.ConstraintModelManager(self.ccdyl_state, self.ccdyl_actuation.nu)\n",
" if(t != self.HORIZON):\n",
" for frame_idx in self.supportFeetIds:\n",
" name = self.rmodel.frames[frame_idx].name + \"_contact\"\n",
" residualFriction = friction_utils.ResidualFrictionCone(self.ccdyl_state, name, self.friction_mu, self.ccdyl_actuation.nu)\n",
" constraintFriction = crocoddyl.ConstraintModelResidual(self.ccdyl_state, residualFriction, np.array([0.]), np.array([np.inf]))\n",
" constraintModelManager.addConstraint(name + \"friction\", constraintFriction)\n",
" #friction model for the arm\n",
" # name = self.rmodel.frames[self.armEEId].name + \"_contact\"\n",
" # residualFriction = friction_utils.ResidualFrictionCone(self.ccdyl_state, name, self.friction_mu, self.ccdyl_actuation.nu)\n",
" # constraintFriction = crocoddyl.ConstraintModelResidual(self.ccdyl_state, residualFriction, np.array([0.]), np.array([np.inf]))\n",
" # constraintModelManager.addConstraint(name + \"friction\", constraintFriction)\n",
" \n",
" dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.ccdyl_state, self.ccdyl_actuation, self.contactModel, costModel, constraintModelManager, 0., True)\n",
" model = crocoddyl.IntegratedActionModelEuler(dmodel, self.dt)\n",
" self.running_models += [model]\n",
" self.ocp = crocoddyl.ShootingProblem(self.x0, self.running_models[:-1], self.running_models[-1])\n",
" \n",
" def createSolver(self):\n",
" solver = mim_solvers.SolverCSQP(self.ocp)\n",
" solver.max_qp_iters = 25\n",
" solver.with_callbacks = True\n",
" solver.use_filter_line_search = False\n",
" solver.termination_tolerance = 1e-2\n",
" solver.eps_abs = 1e-2\n",
" solver.eps_rel = 0.\n",
" self.solver = solver\n",
"\n",
" def getSolution(self, k=None):\n",
" if k is None: \n",
" x_idx = 1\n",
" u_idx = 0\n",
" else:\n",
" x_idx = k\n",
" u_idx = k\n",
" t = self.xs[x_idx][:3]\n",
" quat = self.xs[x_idx][3:7]\n",
" qx = quat[0]\n",
" qy = quat[1]\n",
" qz = quat[2]\n",
" qw = quat[3]\n",
" q = self.xs[x_idx][7:25]\n",
" eta = self.xs[x_idx][25:25+6]\n",
" dq = self.xs[x_idx][25+6:]\n",
" constraint_norm = self.solver.constraint_norm\n",
" return dict(\n",
" position=t,\n",
" orientation=np.array([qw, qx, qy, qz]), #Mujoco and uniree quaternion order\n",
" velocity = eta[:3],\n",
" omega = eta[3:],\n",
" q = q[self.mpc_to_unitree_idx],\n",
" dq = dq[self.mpc_to_unitree_idx], \n",
" tau = self.us[u_idx][[self.mpc_to_unitree_idx]],\n",
" constraint_norm = constraint_norm\n",
" )\n",
" \n",
" def updateAndSolve(self, t, quat, q, v, omega, dq):\n",
" q_ = np.zeros(self.rmodel.nq)\n",
" dq_ = np.zeros(self.rmodel.nv)\n",
" qw = quat[0]\n",
" qx = quat[1]\n",
" qy = quat[2]\n",
" qz = quat[3]\n",
" q_[:3] = t\n",
" q_[3:7] = np.array([qx, qy, qz, qw])\n",
" q_[7:] = q[self.unitree_to_mpc_idx]\n",
" dq_[:3] = v\n",
" dq_[3:6] = omega\n",
" dq_[6:] = dq[self.unitree_to_mpc_idx]\n",
" pin.framesForwardKinematics(self.rmodel, self.rdata, q_)\n",
" x = np.hstack([q_, dq_])\n",
" self.solver.problem.x0 = x\n",
" self.xs = list(self.solver.xs[1:]) + [self.solver.xs[-1]]\n",
" self.xs[0] = x\n",
" self.us = list(self.us[1:]) + [self.us[-1]] \n",
" self.solver.solve(self.xs, self.us, self.max_iterations)\n",
" self.xs, self.us = self.solver.xs, self.solver.us\n",
" return self.getSolution()\n",
" \n",
" def solve(self):\n",
" self.solver.solve(self.xs, self.us, self.max_iterations)\n",
" self.xs, self.us = self.solver.xs, self.solver.us\n",
" return self.getSolution()"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"assets_path = '/home/Go2py/Go2Py/assets'\n",
"mpc = Go2MPC(assets_path, HORIZON=20, friction_mu=0.1)\n",
"# mpc = Go2MPC(assets_path, HORIZON=20, friction_mu=0.1)"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.4187e+03 1.4187e+03 1.8250e-11 0.0000e+00 1.64565e+01 0.2500 ---- 1\n",
" 2 1.0276e+03 7.9827e+02 2.7192e-01 2.2657e+01 9.00467e+00 1.0000 3.4495e+01 3\n",
" 3 2.3304e+02 3.9179e-02 5.0619e+00 1.8238e+01 4.48684e-01 1.0000 1.8238e+01 3\n",
" 4 2.4402e-01 2.6907e-02 1.5285e-03 2.0183e-02 1.99059e-01 1.0000 2.0183e-02 3\n",
" END 3.6830e-02 2.6903e-02 5.3920e-05 9.3885e-04 ---- ---- 3.3589e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 7.0234e-02 2.6902e-02 3.3943e-03 9.3887e-04 ---- ---- 7.8730e-03 -----\n"
]
},
{
"data": {
"text/plain": [
"{'position': array([7.64245650e-05, 1.19914809e-06, 3.30076878e-01]),\n",
" 'orientation': array([ 1.00000000e+00, -6.81801566e-09, -2.91826441e-07, -3.19038422e-07]),\n",
" 'velocity': array([3.82122935e-03, 5.99585975e-05, 3.84391623e-03]),\n",
" 'omega': array([-6.81801566e-07, -2.91826441e-05, -3.19038422e-05]),\n",
" 'q': array([-3.53840354e-06, 7.78324126e-01, -1.56013948e+00, -3.53840354e-06,\n",
" 7.78326989e-01, -1.56014401e+00, -4.35362308e-06, 7.78324619e-01,\n",
" -1.56014046e+00, -4.35362308e-06, 7.78328001e-01, -1.56014604e+00,\n",
" -2.71600731e-06, 2.99623852e-01, -2.99131121e-01, 6.58396878e-07,\n",
" -3.77311125e-04, 4.55605926e-07]),\n",
" 'dq': array([-1.76920177e-04, -2.14688789e-04, 2.57521924e-02, -1.76920177e-04,\n",
" -7.15256885e-05, 2.55252730e-02, -2.17681154e-04, -1.90062091e-04,\n",
" 2.57028413e-02, -2.17681154e-04, -2.09729748e-05, 2.54239671e-02,\n",
" -1.35800366e-04, -1.88073907e-02, 4.34439336e-02, 3.29198439e-05,\n",
" -1.88655563e-02, 2.27802963e-05]),\n",
" 'tau': array([[ 1.60440927e+00, 5.95906212e-01, 5.80116764e+00,\n",
" -1.59226822e+00, 7.20977830e-01, 5.78112417e+00,\n",
" 1.58779351e+00, 6.06274461e-01, 5.80958688e+00,\n",
" -1.59994801e+00, 5.74307259e-01, 5.85546083e+00,\n",
" 5.84126493e-05, -8.36610161e-01, -5.02686702e-01,\n",
" 3.03338611e-05, -1.14695761e-01, 2.88833109e-06]]),\n",
" 'constraint_norm': 0.0009388668151402157}"
]
},
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"\n",
"mpc.initialize()\n",
"mpc.solve()\n",
"m = list(mpc.solver.problem.runningModels) + [mpc.solver.problem.terminalModel]\n",
"for action_model in m:\n",
" action_model.differential.contacts.changeContactStatus('Link6_contact', False)\n",
"mpc.solve()"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
"import time\n",
"for i in range(mpc.HORIZON):\n",
"# for i in range(1):\n",
" q = np.zeros(27)\n",
" # state = mpc.getSolution(i)\n",
" state = mpc.getSolution(0)\n",
" robot.pos0 = state['position']\n",
" robot.rot0 = state['orientation']\n",
" robot.q0 = state['q']\n",
" robot.reset()\n",
" time.sleep(0.01)"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
"for action_model in m:\n",
" # action_model.differential.costs.costs['contact_force_track'].cost.residual.reference.linear[2] = 0\n",
" # action_model.differential.costs.costs['contact_force_track'].weight =0\n",
" # action_model.differential.costs.costs['efTrack'].active = True\n",
" action_model.differential.contacts.changeContactStatus('Link6_contact', True)\n",
" action_model.differential.costs.costs['contact_force_track'].cost.residual.reference.linear[:] = np.array([-5, 0, 0])"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [],
"source": [
"import mujoco as mj\n",
"def getForceSensor(model, data, site_id):\n",
" site_id = mj.mj_name2id(model,mj.mjtObj.mjOBJ_SITE, 'EF_force_site')\n",
" world_R_sensor = data.xmat[site_id].reshape(3,3).T\n",
" force_in_body = data.sensordata[-3:].reshape(3,1)\n",
" force_in_world = world_R_sensor@force_in_body\n",
" return force_in_world"
]
},
{
"cell_type": "code",
"execution_count": 19,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.2378e+02 9.9770e+01 1.1491e+01 9.1010e-01 4.80228e+00 0.1250 ---- 3\n",
" 2 2.1380e+02 7.6389e+01 9.9404e+00 3.8008e+00 4.14732e+00 0.5000 1.1750e+01 3\n",
" 3 1.5072e+02 1.9077e+01 3.6168e+00 9.5471e+00 2.17564e+00 1.0000 9.5471e+00 3\n",
" 4 6.0622e+01 6.8999e-02 8.2992e-01 5.2253e+00 1.02132e+00 0.2500 5.2253e+00 3\n",
" 5 5.0684e+01 6.8959e-02 6.2229e-01 4.4393e+00 7.74598e-02 1.0000 4.4393e+00 3\n",
" END 7.8577e-02 6.9011e-02 9.5660e-04 0.0000e+00 ---- ---- 4.2526e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.7318e+01 7.4555e-02 2.7244e+00 0.0000e+00 5.60400e-01 0.2500 ---- 1\n",
" 2 2.4399e+01 7.1720e-02 2.0459e+00 3.8688e-01 3.91568e-01 1.0000 5.3273e-01 3\n",
" 3 1.5954e+01 6.9089e-02 3.8903e-02 1.5496e+00 1.50224e-01 0.2500 1.5496e+00 3\n",
" 4 1.2466e+01 6.9085e-02 2.9175e-02 1.2105e+00 4.36357e-02 1.0000 1.2105e+00 3\n",
" 5 3.0983e-01 6.9092e-02 8.2111e-05 2.3992e-02 5.32740e-02 0.0312 2.3992e-02 3\n",
" 6 3.0763e-01 6.9091e-02 7.9556e-05 2.3774e-02 4.63496e-02 0.0312 2.3774e-02 3\n",
" 7 3.0301e-01 6.9091e-02 7.7079e-05 2.3314e-02 4.03711e-02 0.0625 2.3314e-02 3\n",
" 8 3.0110e-01 6.9091e-02 7.2287e-05 2.3129e-02 3.00400e-02 0.1250 2.3129e-02 3\n",
" 9 2.9920e-01 6.9091e-02 6.3309e-05 2.2947e-02 1.57095e-02 0.5000 2.2947e-02 3\n",
" 10 2.5429e-01 6.9091e-02 3.1929e-05 1.8488e-02 1.46063e-02 0.2500 1.8488e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.2972e-01 6.9091e-02 2.3955e-05 1.6039e-02 3.04817e-03 1.0000 1.6039e-02 3\n",
" END 6.9455e-02 6.9091e-02 6.6673e-08 3.6339e-05 ---- ---- 5.7523e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.8912e+01 7.4669e-02 2.7326e+00 1.5112e-01 5.01831e-01 1.0000 ---- 3\n",
" 2 2.5449e+01 6.9187e-02 6.5872e-02 2.4721e+00 1.82263e-01 1.0000 2.4721e+00 3\n",
" 3 4.0605e+00 6.9195e-02 8.6670e-04 3.9826e-01 3.90642e-01 0.1250 3.9826e-01 3\n",
" 4 3.9833e+00 6.9191e-02 7.7174e-04 3.9064e-01 2.00427e-01 0.2500 3.9064e-01 3\n",
" 5 3.1953e+00 6.9190e-02 5.9240e-04 3.1202e-01 2.55341e-02 1.0000 3.1202e-01 3\n",
" 6 5.3825e-01 6.9193e-02 7.6930e-06 4.6898e-02 3.09257e-02 0.2500 4.6898e-02 3\n",
" 7 4.5180e-01 6.9193e-02 6.1625e-06 3.8254e-02 1.10048e-02 1.0000 3.8254e-02 3\n",
" END 7.0237e-02 6.9193e-02 1.0252e-06 1.0331e-04 ---- ---- 7.0933e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.8469e+01 7.4786e-02 2.7491e+00 9.0258e-02 4.99376e-01 1.0000 ---- 3\n",
" 2 2.4287e+01 6.9468e-02 4.6402e-02 2.3753e+00 1.71745e-01 1.0000 2.3753e+00 3\n",
" 3 4.9073e+00 6.9477e-02 8.0726e-04 4.8297e-01 3.53726e-01 0.1250 4.8297e-01 3\n",
" 4 4.6337e+00 6.9473e-02 7.1780e-04 4.5571e-01 1.82195e-01 0.5000 4.5571e-01 3\n",
" 5 4.4099e+00 6.9472e-02 4.0502e-04 4.3364e-01 1.75181e-01 0.2500 4.3364e-01 3\n",
" 6 3.9409e+00 6.9471e-02 3.1433e-04 3.8683e-01 1.16857e-02 1.0000 3.8683e-01 3\n",
" END 6.9484e-02 6.9476e-02 8.3817e-07 0.0000e+00 ---- ---- 7.2187e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.8886e+01 7.5079e-02 2.7796e+00 1.0144e-01 5.01946e-01 0.5000 ---- 3\n",
" 2 2.2101e+01 7.0456e-02 1.3998e+00 8.0323e-01 2.82183e-01 0.5000 8.0323e-01 3\n",
" 3 1.7021e+01 6.9605e-02 7.0231e-01 9.9288e-01 1.88927e-01 0.5000 9.9288e-01 3\n",
" 4 1.4155e+01 6.9544e-02 3.5167e-01 1.0569e+00 1.68780e-01 0.2500 1.0569e+00 3\n",
" 5 1.2074e+01 6.9566e-02 2.6379e-01 9.3669e-01 5.36531e-02 1.0000 9.3669e-01 3\n",
" END 7.7078e-02 6.9738e-02 3.6950e-04 3.6446e-04 ---- ---- 6.5523e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.8125e+01 7.5354e-02 2.7924e+00 1.2530e-02 5.07328e-01 0.5000 ---- 2\n",
" 2 2.1822e+01 7.0716e-02 1.4068e+00 7.6842e-01 2.67245e-01 1.0000 7.6842e-01 3\n",
" 3 3.6621e+00 6.9977e-02 1.1932e-02 3.4728e-01 1.53216e-01 0.0625 3.4728e-01 3\n",
" 4 3.5047e+00 6.9976e-02 1.1186e-02 3.3228e-01 1.16338e-01 0.1250 3.3228e-01 3\n",
" 5 3.2494e+00 6.9975e-02 9.7875e-03 3.0816e-01 5.99643e-02 0.5000 3.0816e-01 3\n",
" 6 2.1185e+00 6.9975e-02 4.8909e-03 1.9996e-01 6.03686e-02 0.2500 1.9996e-01 3\n",
" 7 1.9204e+00 6.9974e-02 3.6685e-03 1.8138e-01 9.85309e-03 1.0000 1.8138e-01 3\n",
" END 7.9289e-02 6.9976e-02 6.6906e-06 9.2462e-04 ---- ---- 9.2462e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.9471e+01 7.5588e-02 2.8118e+00 1.2772e-01 4.96390e-01 0.5000 ---- 3\n",
" 2 2.2177e+01 7.0941e-02 1.4169e+00 7.9363e-01 2.74265e-01 0.5000 7.9363e-01 3\n",
" 3 1.5695e+01 7.0079e-02 7.1109e-01 8.5138e-01 1.75557e-01 0.5000 8.5138e-01 3\n",
" 4 1.2364e+01 7.0011e-02 3.5613e-01 8.7326e-01 1.48230e-01 0.2500 8.7326e-01 3\n",
" 5 1.0484e+01 7.0031e-02 2.6713e-01 7.7423e-01 5.18531e-02 1.0000 7.7423e-01 3\n",
" END 8.7887e-02 7.0196e-02 3.9240e-04 1.3767e-03 ---- ---- 1.3767e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.8702e+01 7.5816e-02 2.8254e+00 3.7228e-02 5.06334e-01 0.5000 ---- 3\n",
" 2 2.2503e+01 7.1157e-02 1.4241e+00 8.1906e-01 2.61765e-01 1.0000 8.1906e-01 3\n",
" 3 3.9116e+00 7.0396e-02 1.2991e-02 3.7112e-01 1.46047e-01 0.1250 3.7112e-01 3\n",
" 4 3.9026e+00 7.0394e-02 1.1366e-02 3.7185e-01 7.53180e-02 0.5000 3.7185e-01 3\n",
" 5 3.4838e+00 7.0394e-02 5.6750e-03 3.3566e-01 7.62246e-02 0.2500 3.3566e-01 3\n",
" 6 3.2013e+00 7.0393e-02 4.2577e-03 3.0884e-01 1.21684e-02 1.0000 3.0884e-01 3\n",
" END 8.3319e-02 7.0396e-02 8.9330e-06 1.2834e-03 ---- ---- 1.2834e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.9449e+01 7.6009e-02 2.8462e+00 9.1100e-02 4.97854e-01 0.5000 ---- 3\n",
" 2 2.2688e+01 7.1344e-02 1.4349e+00 8.2670e-01 2.80240e-01 0.5000 8.2670e-01 3\n",
" 3 1.8416e+01 7.0472e-02 7.2027e-01 1.1143e+00 1.86036e-01 0.5000 1.1143e+00 3\n",
" 4 1.5358e+01 7.0399e-02 3.6077e-01 1.1680e+00 1.57713e-01 0.2500 1.1680e+00 3\n",
" 5 1.3004e+01 7.0418e-02 2.7061e-01 1.0227e+00 5.39454e-02 1.0000 1.0227e+00 3\n",
" END 8.9060e-02 7.0581e-02 4.2063e-04 1.4273e-03 ---- ---- 1.4273e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.8825e+01 7.6200e-02 2.8587e+00 1.6138e-02 5.03559e-01 0.5000 ---- 3\n",
" 2 2.1808e+01 7.1532e-02 1.4413e+00 7.3234e-01 2.58609e-01 1.0000 7.3234e-01 3\n",
" 3 2.3384e+00 7.0759e-02 1.3601e-02 2.1316e-01 1.26433e-01 0.0625 2.1316e-01 3\n",
" 4 2.2565e+00 7.0759e-02 1.2751e-02 2.0583e-01 9.61262e-02 0.1250 2.0583e-01 3\n",
" 5 2.1240e+00 7.0758e-02 1.1156e-02 1.9416e-01 5.04117e-02 0.5000 1.9416e-01 3\n",
" 6 1.4492e+00 7.0758e-02 5.5719e-03 1.3227e-01 4.97616e-02 0.2500 1.3227e-01 3\n",
" 7 1.3198e+00 7.0757e-02 4.1799e-03 1.2072e-01 9.57908e-03 1.0000 1.2072e-01 3\n",
" END 8.3459e-02 7.0758e-02 6.4527e-06 1.2636e-03 ---- ---- 1.2636e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.0517e+01 7.6366e-02 2.8774e+00 1.6669e-01 4.95023e-01 1.0000 ---- 3\n",
" 2 3.0330e+01 7.0924e-02 5.8685e-02 2.9672e+00 2.29460e-01 0.5000 2.9672e+00 3\n",
" 3 2.6785e+01 7.0919e-02 2.9359e-02 2.6421e+00 1.88471e-01 0.5000 2.6421e+00 3\n",
" 4 2.5063e+01 7.0919e-02 1.4676e-02 2.4845e+00 1.89418e-01 0.5000 2.4845e+00 3\n",
" 5 2.4251e+01 7.0918e-02 7.3397e-03 2.4107e+00 1.92212e-01 0.2500 2.4107e+00 3\n",
" 6 1.9523e+01 7.0917e-02 5.5043e-03 1.9397e+00 2.58734e-02 1.0000 1.9397e+00 3\n",
" END 8.1509e-02 7.0938e-02 8.7105e-06 1.0485e-03 ---- ---- 1.0485e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.9246e+01 7.6546e-02 2.9032e+00 1.3807e-02 5.10919e-01 0.5000 ---- 3\n",
" 2 1.8191e+01 7.1880e-02 1.4639e+00 3.4795e-01 2.93328e-01 0.5000 3.9183e-01 3\n",
" 3 1.3083e+01 7.1005e-02 7.3486e-01 5.6634e-01 2.08982e-01 0.2500 5.6634e-01 3\n",
" 4 1.1469e+01 7.0936e-02 5.5130e-01 5.8851e-01 1.01494e-01 1.0000 5.8851e-01 3\n",
" END 1.5310e-01 7.1109e-02 1.8136e-03 6.3855e-03 ---- ---- 6.3855e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.1178e+01 7.6727e-02 2.9054e+00 2.0476e-01 5.12504e-01 0.5000 ---- 3\n",
" 2 2.6221e+01 7.2056e-02 1.4649e+00 1.1500e+00 3.08119e-01 0.5000 1.1500e+00 3\n",
" 3 2.2932e+01 7.1181e-02 7.3534e-01 1.5507e+00 2.18133e-01 0.5000 1.5507e+00 3\n",
" 4 1.9841e+01 7.1107e-02 3.6831e-01 1.6087e+00 1.98030e-01 0.2500 1.6087e+00 3\n",
" 5 1.6638e+01 7.1125e-02 2.7627e-01 1.3804e+00 6.01042e-02 1.0000 1.3804e+00 3\n",
" END 8.9077e-02 7.1290e-02 4.4192e-04 1.3368e-03 ---- ---- 1.3368e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.9549e+01 7.6895e-02 2.9297e+00 1.7483e-02 5.08055e-01 0.5000 ---- 3\n",
" 2 2.0934e+01 7.2235e-02 1.4773e+00 6.0887e-01 2.67302e-01 1.0000 6.0887e-01 3\n",
" 3 5.8174e-01 7.1470e-02 1.4245e-02 3.6782e-02 1.72409e-01 0.0625 3.6782e-02 3\n",
" 4 5.6361e-01 7.1468e-02 1.3354e-02 3.5860e-02 1.27862e-01 0.1250 3.5860e-02 3\n",
" 5 5.5220e-01 7.1467e-02 1.1682e-02 3.6390e-02 6.26220e-02 0.5000 3.6390e-02 3\n",
" 6 4.7038e-01 7.1466e-02 5.8271e-03 3.4065e-02 6.63885e-02 0.2500 3.4065e-02 3\n",
" 7 4.2397e-01 7.1465e-02 4.3723e-03 3.0878e-02 1.08774e-02 1.0000 3.0878e-02 3\n",
" END 8.1627e-02 7.1465e-02 7.1243e-06 1.0090e-03 ---- ---- 1.0090e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.1081e+01 7.7055e-02 2.9485e+00 1.5184e-01 5.04129e-01 0.5000 ---- 3\n",
" 2 2.2221e+01 7.2406e-02 1.4869e+00 7.2806e-01 2.73815e-01 1.0000 7.2806e-01 3\n",
" 3 9.9195e+00 7.1649e-02 1.4659e-02 9.7012e-01 2.33082e-01 0.1250 9.7012e-01 3\n",
" 4 9.0662e+00 7.1645e-02 1.2820e-02 8.8663e-01 1.13510e-01 0.5000 8.8663e-01 3\n",
" 5 6.8059e+00 7.1644e-02 6.3628e-03 6.6706e-01 1.18572e-01 0.2500 6.6706e-01 3\n",
" 6 5.5182e+00 7.1643e-02 4.7767e-03 5.3988e-01 1.61548e-02 1.0000 5.3988e-01 3\n",
" END 8.1512e-02 7.1648e-02 9.7083e-06 9.7672e-04 ---- ---- 9.7672e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.1027e+01 7.7232e-02 2.9693e+00 1.2573e-01 5.13164e-01 0.5000 ---- 3\n",
" 2 2.3602e+01 7.2588e-02 1.4973e+00 8.5565e-01 2.70927e-01 1.0000 8.5565e-01 3\n",
" 3 2.1534e+00 7.1838e-02 1.4559e-02 1.9360e-01 1.59531e-01 0.0625 1.9360e-01 3\n",
" 4 2.0639e+00 7.1837e-02 1.3648e-02 1.8556e-01 1.18804e-01 0.1250 1.8556e-01 3\n",
" 5 1.9103e+00 7.1836e-02 1.1941e-02 1.7190e-01 5.92181e-02 0.5000 1.7190e-01 3\n",
" 6 1.1986e+00 7.1836e-02 5.9609e-03 1.0671e-01 6.21129e-02 0.2500 1.0671e-01 3\n",
" 7 1.0804e+00 7.1835e-02 4.4723e-03 9.6382e-02 1.13711e-02 1.0000 9.6382e-02 3\n",
" END 8.3138e-02 7.1836e-02 7.1988e-06 1.1230e-03 ---- ---- 1.1230e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.1767e+01 7.7414e-02 2.9887e+00 1.8024e-01 5.06975e-01 1.0000 ---- 3\n",
" 2 3.1731e+01 7.2022e-02 6.1901e-02 3.1040e+00 2.42862e-01 0.5000 3.1040e+00 3\n",
" 3 2.2032e+01 7.2021e-02 3.0935e-02 2.1650e+00 2.14356e-01 0.5000 2.1650e+00 3\n",
" 4 2.1665e+01 7.2020e-02 1.5483e-02 2.1438e+00 2.34372e-01 0.2500 2.1438e+00 3\n",
" 5 1.7955e+01 7.2018e-02 1.1611e-02 1.7767e+00 3.15241e-02 1.0000 1.7767e+00 3\n",
" END 7.7197e-02 7.2036e-02 1.7463e-05 4.9859e-04 ---- ---- 5.4142e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.0359e+01 7.7611e-02 3.0146e+00 1.3519e-02 5.28826e-01 0.5000 ---- 3\n",
" 2 2.0980e+01 7.2979e-02 1.5203e+00 5.7043e-01 2.99303e-01 1.0000 5.7043e-01 3\n",
" 3 8.9532e+00 7.2238e-02 1.5260e-02 8.7283e-01 2.78425e-01 0.1250 8.7283e-01 3\n",
" 4 8.7502e+00 7.2234e-02 1.3347e-02 8.5445e-01 1.29925e-01 0.5000 8.5445e-01 3\n",
" 5 7.0320e+00 7.2234e-02 6.6363e-03 6.8934e-01 1.52650e-01 0.2500 6.8934e-01 3\n",
" 6 6.2520e+00 7.2233e-02 4.9828e-03 6.1299e-01 2.17156e-02 1.0000 6.1299e-01 3\n",
" END 7.7880e-02 7.2238e-02 1.2630e-05 5.5156e-04 ---- ---- 5.7289e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.1357e+01 7.7804e-02 3.0277e+00 1.0018e-01 5.44533e-01 0.5000 ---- 3\n",
" 2 2.2141e+01 7.3182e-02 1.5270e+00 6.7972e-01 3.19501e-01 1.0000 6.7972e-01 3\n",
" 3 1.8745e+01 7.2448e-02 1.5804e-02 1.8514e+00 3.90758e-01 0.1250 1.8514e+00 3\n",
" 4 1.7930e+01 7.2441e-02 1.3821e-02 1.7719e+00 1.87014e-01 0.5000 1.7719e+00 3\n",
" 5 1.4926e+01 7.2441e-02 6.8522e-03 1.4785e+00 2.14813e-01 0.2500 1.4785e+00 3\n",
" 6 1.2941e+01 7.2439e-02 5.1470e-03 1.2817e+00 2.85349e-02 1.0000 1.2817e+00 3\n",
" END 8.1690e-02 7.2452e-02 1.6557e-05 9.0730e-04 ---- ---- 9.0730e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.2238e+01 7.8011e-02 3.0522e+00 1.6375e-01 5.55608e-01 0.5000 ---- 3\n",
" 2 2.4976e+01 7.3397e-02 1.5394e+00 9.5089e-01 3.20574e-01 1.0000 9.5089e-01 3\n",
" 3 1.6630e+01 7.2673e-02 1.5861e-02 1.6399e+00 3.55028e-01 0.1250 1.6399e+00 3\n",
" 4 1.6267e+01 7.2665e-02 1.3875e-02 1.6056e+00 1.75153e-01 0.5000 1.6056e+00 3\n",
" 5 1.4417e+01 7.2666e-02 6.9056e-03 1.4276e+00 2.00001e-01 0.2500 1.4276e+00 3\n",
" 6 1.2648e+01 7.2664e-02 5.1854e-03 1.2524e+00 2.80432e-02 1.0000 1.2524e+00 3\n",
" END 8.4540e-02 7.2676e-02 1.6369e-05 1.1700e-03 ---- ---- 1.1700e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.2740e+01 7.8229e-02 3.0750e+00 1.9126e-01 5.54673e-01 0.5000 ---- 3\n",
" 2 2.4174e+01 7.3623e-02 1.5509e+00 8.5919e-01 3.32011e-01 0.5000 8.5919e-01 3\n",
" 3 1.6112e+01 7.2779e-02 7.7849e-01 8.2548e-01 2.49001e-01 0.5000 8.2548e-01 3\n",
" 4 1.5284e+01 7.2718e-02 3.9001e-01 1.1311e+00 2.44414e-01 0.2500 1.1311e+00 3\n",
" 5 1.3680e+01 7.2738e-02 2.9252e-01 1.0682e+00 6.70163e-02 1.0000 1.0682e+00 3\n",
" END 9.2830e-02 7.2909e-02 5.2431e-04 1.4677e-03 ---- ---- 1.4677e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.2576e+01 7.8464e-02 3.0905e+00 1.5930e-01 5.65829e-01 0.5000 ---- 3\n",
" 2 2.7130e+01 7.3864e-02 1.5587e+00 1.1469e+00 3.29670e-01 1.0000 1.1469e+00 3\n",
" 3 1.9633e+01 7.3154e-02 1.6198e-02 1.9398e+00 3.80175e-01 0.1250 1.9398e+00 3\n",
" 4 1.9320e+01 7.3145e-02 1.4171e-02 1.9105e+00 1.92449e-01 0.5000 1.9105e+00 3\n",
" 5 1.8021e+01 7.3146e-02 7.0527e-03 1.7878e+00 2.16386e-01 0.2500 1.7878e+00 3\n",
" 6 1.5670e+01 7.3143e-02 5.2965e-03 1.5544e+00 3.15535e-02 1.0000 1.5544e+00 3\n",
" END 8.8837e-02 7.3159e-02 1.7926e-05 1.5498e-03 ---- ---- 1.5498e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.3041e+01 7.8697e-02 3.1217e+00 1.7453e-01 5.59853e-01 0.5000 ---- 3\n",
" 2 2.4325e+01 7.4111e-02 1.5745e+00 8.5054e-01 3.36468e-01 0.5000 8.5054e-01 3\n",
" 3 1.8271e+01 7.3276e-02 7.9038e-01 1.0294e+00 2.55871e-01 0.5000 1.0294e+00 3\n",
" 4 1.7841e+01 7.3219e-02 3.9597e-01 1.3808e+00 2.54202e-01 0.2500 1.3808e+00 3\n",
" 5 1.5745e+01 7.3240e-02 2.9699e-01 1.2702e+00 6.92447e-02 1.0000 1.2702e+00 3\n",
" END 1.0385e-01 7.3417e-02 5.4189e-04 2.5011e-03 ---- ---- 3.6836e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.2608e+01 7.8958e-02 3.1377e+00 1.1528e-01 5.71466e-01 0.5000 ---- 3\n",
" 2 2.7837e+01 7.4377e-02 1.5826e+00 1.1937e+00 3.34694e-01 1.0000 1.1937e+00 3\n",
" 3 2.3808e+01 7.3687e-02 1.6593e-02 2.3568e+00 3.81474e-01 0.1250 2.3568e+00 3\n",
" 4 2.2935e+01 7.3677e-02 1.4517e-02 2.2716e+00 2.01413e-01 0.5000 2.2716e+00 3\n",
" 5 2.1563e+01 7.3677e-02 7.2267e-03 2.1417e+00 2.20102e-01 0.2500 2.1417e+00 3\n",
" 6 1.8351e+01 7.3675e-02 5.4273e-03 1.8223e+00 3.41210e-02 1.0000 1.8223e+00 3\n",
" END 9.5541e-02 7.3694e-02 1.8991e-05 2.1658e-03 ---- ---- 2.1658e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.3455e+01 7.9217e-02 3.1704e+00 1.6717e-01 5.56030e-01 0.5000 ---- 3\n",
" 2 2.4415e+01 7.4652e-02 1.5992e+00 8.3481e-01 3.30865e-01 0.5000 8.3481e-01 3\n",
" 3 1.8456e+01 7.3828e-02 8.0281e-01 1.0354e+00 2.42841e-01 0.5000 1.0354e+00 3\n",
" 4 1.7594e+01 7.3777e-02 4.0220e-01 1.3498e+00 2.41111e-01 0.2500 1.3498e+00 3\n",
" 5 1.5433e+01 7.3800e-02 3.0167e-01 1.2342e+00 7.01538e-02 1.0000 1.2342e+00 3\n",
" END 1.0127e-01 7.3981e-02 5.5942e-04 2.1694e-03 ---- ---- 2.1694e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.3016e+01 7.9506e-02 3.1864e+00 1.0720e-01 5.74038e-01 0.5000 ---- 3\n",
" 2 2.7720e+01 7.4947e-02 1.6073e+00 1.1572e+00 3.36972e-01 1.0000 1.1572e+00 3\n",
" 3 2.4128e+01 7.4279e-02 1.6920e-02 2.3885e+00 3.79924e-01 0.1250 2.3885e+00 3\n",
" 4 2.3231e+01 7.4268e-02 1.4803e-02 2.3008e+00 2.04829e-01 0.2500 2.3008e+00 3\n",
" 5 1.8760e+01 7.4267e-02 1.1095e-02 1.8575e+00 3.99537e-02 1.0000 1.8575e+00 3\n",
" END 1.1063e-01 7.4286e-02 4.0198e-05 3.5940e-03 ---- ---- 9.0816e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.4044e+01 7.9793e-02 3.2211e+00 1.7541e-01 5.50364e-01 0.5000 ---- 3\n",
" 2 2.4266e+01 7.5252e-02 1.6249e+00 7.9421e-01 3.12698e-01 1.0000 7.9421e-01 3\n",
" 3 1.0819e+01 7.4601e-02 1.6949e-02 1.0575e+00 2.97522e-01 0.0625 1.0575e+00 3\n",
" 4 1.0387e+01 7.4598e-02 1.5889e-02 1.0154e+00 2.25061e-01 0.1250 1.0154e+00 3\n",
" 5 9.7474e+00 7.4595e-02 1.3900e-02 9.5338e-01 1.09917e-01 0.5000 9.5338e-01 3\n",
" 6 7.1558e+00 7.4596e-02 6.9272e-03 7.0119e-01 1.22074e-01 0.2500 7.0119e-01 3\n",
" 7 6.2285e+00 7.4595e-02 5.2012e-03 6.1018e-01 2.00203e-02 1.0000 6.1018e-01 3\n",
" END 9.1745e-02 7.4601e-02 8.5962e-06 1.7058e-03 ---- ---- 1.7058e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.4633e+01 8.0098e-02 3.2451e+00 2.1019e-01 5.44214e-01 0.5000 ---- 3\n",
" 2 2.5065e+01 7.5571e-02 1.6371e+00 8.6183e-01 3.13083e-01 1.0000 8.6183e-01 3\n",
" 3 1.9387e+01 7.4934e-02 1.7263e-02 1.9139e+00 3.02260e-01 0.1250 1.9139e+00 3\n",
" 4 1.8130e+01 7.4928e-02 1.5098e-02 1.7905e+00 1.52441e-01 0.5000 1.7905e+00 3\n",
" 5 1.4858e+01 7.4928e-02 7.4995e-03 1.4708e+00 1.67556e-01 0.2500 1.4708e+00 3\n",
" 6 1.2424e+01 7.4926e-02 5.6297e-03 1.2292e+00 2.85190e-02 1.0000 1.2292e+00 3\n",
" END 9.9152e-02 7.4938e-02 1.4378e-05 2.4070e-03 ---- ---- 2.4070e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.4592e+01 8.0427e-02 3.2734e+00 1.7770e-01 5.56425e-01 0.5000 ---- 3\n",
" 2 2.5919e+01 7.5912e-02 1.6515e+00 9.3281e-01 3.19313e-01 1.0000 9.3281e-01 3\n",
" 3 1.3488e+01 7.5289e-02 1.7393e-02 1.3239e+00 3.03846e-01 0.1250 1.3239e+00 3\n",
" 4 1.3087e+01 7.5284e-02 1.5214e-02 1.2860e+00 1.48770e-01 0.5000 1.2860e+00 3\n",
" 5 1.1803e+01 7.5284e-02 7.5699e-03 1.1652e+00 1.69741e-01 0.2500 1.1652e+00 3\n",
" 6 9.9981e+00 7.5283e-02 5.6841e-03 9.8659e-01 3.36679e-02 1.0000 9.8659e-01 3\n",
" END 9.6143e-02 7.5292e-02 1.6842e-05 2.0682e-03 ---- ---- 2.0682e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.5155e+01 8.0773e-02 3.3012e+00 2.0626e-01 5.84088e-01 0.5000 ---- 3\n",
" 2 2.7146e+01 7.6271e-02 1.6658e+00 1.0412e+00 3.67811e-01 0.5000 1.0412e+00 3\n",
" 3 1.9385e+01 7.5481e-02 8.3622e-01 1.0948e+00 2.78172e-01 0.2500 1.0948e+00 3\n",
" 4 1.6674e+01 7.5432e-02 6.2732e-01 1.0325e+00 1.23580e-01 1.0000 1.0325e+00 3\n",
" END 1.7862e-01 7.5666e-02 2.3551e-03 7.9399e-03 ---- ---- 7.9399e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.5168e+01 8.1163e-02 3.3141e+00 1.9457e-01 6.29926e-01 0.5000 ---- 3\n",
" 2 3.4899e+01 7.6660e-02 1.6723e+00 1.8099e+00 4.41190e-01 0.2500 1.8099e+00 3\n",
" 3 2.9456e+01 7.6141e-02 1.2549e+00 1.6830e+00 2.20691e-01 1.0000 1.6830e+00 3\n",
" 4 1.0005e+00 7.6064e-02 9.6675e-03 8.2774e-02 1.69957e-01 0.0625 8.2774e-02 3\n",
" 5 9.8081e-01 7.6063e-02 9.0628e-03 8.1412e-02 1.20149e-01 0.1250 8.1412e-02 3\n",
" 6 9.3136e-01 7.6062e-02 7.9287e-03 7.7601e-02 5.03565e-02 0.5000 7.7601e-02 3\n",
" 7 5.3898e-01 7.6061e-02 3.9577e-03 4.2334e-02 6.56102e-02 0.1250 4.2334e-02 3\n",
" 8 5.0915e-01 7.6061e-02 3.4632e-03 3.9845e-02 2.64027e-02 0.5000 3.9845e-02 3\n",
" 9 3.3715e-01 7.6060e-02 1.7330e-03 2.4376e-02 3.61891e-02 0.2500 2.4376e-02 2\n",
" 10 3.0956e-01 7.6060e-02 1.2995e-03 2.2050e-02 9.89474e-03 1.0000 2.2050e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 7.6731e-02 7.6060e-02 1.5763e-06 6.5481e-05 ---- ---- 5.5215e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.5402e+01 8.1522e-02 3.3574e+00 1.7472e-01 5.53924e-01 0.5000 ---- 3\n",
" 2 2.6329e+01 7.7051e-02 1.6940e+00 9.3118e-01 3.14159e-01 1.0000 9.3118e-01 3\n",
" 3 1.1359e+01 7.6475e-02 1.8079e-02 1.1102e+00 3.18147e-01 0.1250 1.1102e+00 3\n",
" 4 1.1273e+01 7.6469e-02 1.5813e-02 1.1039e+00 1.43277e-01 0.5000 1.1039e+00 3\n",
" 5 8.7650e+00 7.6470e-02 7.8715e-03 8.6098e-01 1.77069e-01 0.2500 8.6098e-01 3\n",
" 6 8.1369e+00 7.6468e-02 5.9106e-03 8.0013e-01 3.39875e-02 1.0000 8.0013e-01 3\n",
" END 1.5267e-01 7.6475e-02 1.8996e-05 7.6008e-03 ---- ---- 7.6008e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.5746e+01 8.1905e-02 3.3589e+00 2.0749e-01 6.04788e-01 0.5000 ---- 3\n",
" 2 3.2962e+01 7.6659e-02 1.7180e+00 1.5704e+00 3.94359e-01 0.5000 1.5704e+00 3\n",
" 3 2.8911e+01 7.5489e-02 8.6807e-01 2.0155e+00 3.13082e-01 0.2500 2.0155e+00 3\n",
" 4 2.5570e+01 7.5344e-02 6.5153e-01 1.8980e+00 1.29096e-01 1.0000 1.8980e+00 3\n",
" 5 3.4844e-01 7.5306e-02 6.1216e-03 2.1192e-02 1.40676e-01 0.0312 2.1192e-02 2\n",
" 6 3.4180e-01 7.5306e-02 5.9303e-03 2.0720e-02 1.19401e-01 0.0625 2.0720e-02 2\n",
" 7 3.3564e-01 7.5305e-02 5.5595e-03 2.0474e-02 8.28993e-02 0.1250 2.0474e-02 2\n",
" 8 3.2010e-01 7.5304e-02 4.8643e-03 1.9615e-02 3.33377e-02 0.5000 1.9615e-02 2\n",
" 9 2.3738e-01 7.5303e-02 2.4305e-03 1.3777e-02 4.47056e-02 0.1250 1.3777e-02 2\n",
" 10 2.3556e-01 7.5303e-02 2.1267e-03 1.3899e-02 1.67941e-02 0.5000 1.3899e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 1.7882e-01 7.5303e-02 1.0637e-03 9.2880e-03 ---- ---- 9.2880e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.5431e+01 8.0722e-02 3.3207e+00 2.1433e-01 5.27107e-01 1.0000 ---- 3\n",
" 2 3.4341e+01 7.4493e-02 1.4906e-01 3.2776e+00 2.14514e-01 1.0000 3.2776e+00 3\n",
" 3 2.4168e+01 7.4496e-02 1.0723e-03 2.4083e+00 5.28090e-01 0.1250 2.4083e+00 3\n",
" 4 2.3960e+01 7.4481e-02 9.4302e-04 2.3876e+00 2.63915e-01 0.2500 2.3876e+00 3\n",
" 5 1.9367e+01 7.4479e-02 7.0886e-04 1.9285e+00 5.42267e-02 1.0000 1.9285e+00 3\n",
" 6 3.8623e-01 7.4499e-02 1.6590e-05 3.1156e-02 1.30926e-01 0.1250 3.1156e-02 3\n",
" 7 3.7830e-01 7.4498e-02 1.4488e-05 3.0366e-02 5.24230e-02 0.5000 3.0366e-02 3\n",
" 8 2.6462e-01 7.4497e-02 9.0034e-06 1.9004e-02 6.91239e-02 0.5000 1.9004e-02 2\n",
" 9 2.5072e-01 7.4496e-02 4.1211e-06 1.7619e-02 1.04971e-01 0.1250 1.7619e-02 2\n",
" 10 2.4169e-01 7.4495e-02 3.8200e-06 1.6715e-02 3.84981e-02 0.5000 1.6715e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 1.3445e-01 7.4495e-02 2.4573e-06 5.9928e-03 ---- ---- 5.9928e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.4548e+01 7.9902e-02 3.2906e+00 1.5619e-01 5.48858e-01 0.5000 ---- 3\n",
" 2 2.5384e+01 7.5010e-02 1.6726e+00 8.5829e-01 3.10491e-01 1.0000 8.5829e-01 3\n",
" 3 1.5610e+01 7.4004e-02 3.1778e-02 1.5218e+00 3.49126e-01 0.1250 1.5218e+00 3\n",
" 4 1.5470e+01 7.3996e-02 2.7803e-02 1.5118e+00 1.67229e-01 0.5000 1.5118e+00 3\n",
" 5 1.3676e+01 7.3995e-02 1.3876e-02 1.3463e+00 2.07311e-01 0.2500 1.3463e+00 3\n",
" 6 1.2777e+01 7.3992e-02 1.0414e-02 1.2599e+00 4.00579e-02 1.0000 1.2599e+00 3\n",
" 7 3.2307e-01 7.4004e-02 2.5761e-05 2.4881e-02 1.39440e-01 0.0156 2.4881e-02 3\n",
" 8 3.2138e-01 7.4004e-02 2.5359e-05 2.4712e-02 1.28542e-01 0.0156 2.4712e-02 3\n",
" 9 3.1908e-01 7.4004e-02 2.4963e-05 2.4482e-02 1.18463e-01 0.0312 2.4482e-02 3\n",
" 10 3.1904e-01 7.4004e-02 2.4181e-05 2.4479e-02 9.98247e-02 0.0312 2.4479e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.1563e-01 7.4003e-02 2.3423e-05 2.4139e-02 8.41331e-02 0.0625 2.4139e-02 3\n",
" 12 3.1522e-01 7.4003e-02 2.1947e-05 2.4100e-02 5.76512e-02 0.1250 2.4100e-02 3\n",
" 13 3.0997e-01 7.4003e-02 1.9166e-05 2.3578e-02 2.20722e-02 1.0000 2.3578e-02 3\n",
" 14 2.7678e-01 7.4003e-02 2.5319e-06 2.0275e-02 8.20590e-02 0.0625 2.0275e-02 2\n",
" 15 2.7183e-01 7.4003e-02 2.4077e-06 1.9780e-02 5.58822e-02 0.1250 1.9780e-02 2\n",
" 16 2.5841e-01 7.4002e-02 2.1766e-06 1.8438e-02 2.06296e-02 1.0000 1.8438e-02 2\n",
" 17 1.9382e-01 7.4002e-02 1.5790e-06 1.1980e-02 8.15436e-02 0.0312 1.1980e-02 3\n",
" 18 1.9302e-01 7.4002e-02 1.5370e-06 1.1900e-02 6.86623e-02 0.0312 1.1900e-02 3\n",
" 19 1.9078e-01 7.4002e-02 1.4948e-06 1.1677e-02 5.78024e-02 0.0625 1.1677e-02 3\n",
" 20 1.8919e-01 7.4002e-02 1.4198e-06 1.1517e-02 3.95068e-02 0.1250 1.1517e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.8536e-01 7.4002e-02 1.2841e-06 1.1135e-02 1.45888e-02 1.0000 1.1135e-02 2\n",
" END 1.1358e-01 7.4002e-02 6.8796e-07 3.9576e-03 ---- ---- 3.9576e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.4218e+01 7.9405e-02 3.2783e+00 1.3551e-01 5.29921e-01 1.0000 ---- 3\n",
" 2 3.4119e+01 7.3659e-02 1.1119e-01 3.2934e+00 3.36442e-01 0.5000 3.2934e+00 3\n",
" 3 3.2454e+01 7.3650e-02 5.5522e-02 3.1825e+00 3.73345e-01 0.2500 3.1825e+00 3\n",
" 4 2.9567e+01 7.3644e-02 4.1654e-02 2.9076e+00 7.57484e-02 1.0000 2.9076e+00 3\n",
" 5 5.1998e-01 7.3672e-02 1.2387e-04 4.4507e-02 1.98294e-01 0.0156 4.4507e-02 3\n",
" 6 5.1506e-01 7.3672e-02 1.2196e-04 4.4017e-02 1.83098e-01 0.0312 4.4017e-02 3\n",
" 7 5.1370e-01 7.3671e-02 1.1822e-04 4.3885e-02 1.55071e-01 0.0312 4.3885e-02 3\n",
" 8 5.0644e-01 7.3671e-02 1.1458e-04 4.3163e-02 1.31511e-01 0.0625 4.3163e-02 3\n",
" 9 5.0372e-01 7.3670e-02 1.0757e-04 4.2897e-02 9.17573e-02 0.1250 4.2897e-02 3\n",
" 10 4.9273e-01 7.3670e-02 9.4589e-05 4.1812e-02 3.75981e-02 0.5000 4.1812e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.1974e-01 7.3670e-02 4.9553e-05 2.4557e-02 4.69050e-02 0.2500 2.4557e-02 2\n",
" 12 2.9493e-01 7.3670e-02 3.7236e-05 2.2089e-02 1.40031e-02 1.0000 2.2089e-02 2\n",
" END 7.7370e-02 7.3670e-02 9.2891e-07 3.6906e-04 ---- ---- 5.7995e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.3995e+01 7.9067e-02 3.2748e+00 1.1670e-01 5.23120e-01 1.0000 ---- 3\n",
" 2 3.2864e+01 7.3367e-02 1.0540e-01 3.1736e+00 3.39529e-01 0.5000 3.1736e+00 3\n",
" 3 3.1054e+01 7.3359e-02 5.2609e-02 3.0454e+00 3.90523e-01 0.2500 3.0454e+00 3\n",
" 4 3.0864e+01 7.3350e-02 3.9474e-02 3.0396e+00 8.57127e-02 1.0000 3.0396e+00 3\n",
" 5 8.6993e-01 7.3380e-02 1.2325e-04 7.9531e-02 2.78859e-01 0.0312 7.9531e-02 3\n",
" 6 8.5861e-01 7.3379e-02 1.1966e-04 7.8403e-02 2.36180e-01 0.0625 7.8403e-02 3\n",
" 7 8.5759e-01 7.3377e-02 1.1292e-04 7.8309e-02 1.61742e-01 0.0625 7.8309e-02 3\n",
" 8 8.2284e-01 7.3376e-02 1.0623e-04 7.4841e-02 1.10300e-01 0.1250 7.4841e-02 3\n",
" 9 7.8648e-01 7.3375e-02 9.3742e-05 7.1217e-02 4.15842e-02 0.5000 7.1217e-02 3\n",
" 10 4.8998e-01 7.3375e-02 4.9683e-05 4.1611e-02 5.99191e-02 0.1250 4.1611e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 4.5513e-01 7.3375e-02 4.3499e-05 3.8132e-02 2.06946e-02 1.0000 3.8132e-02 3\n",
" 12 2.0903e-01 7.3375e-02 1.0127e-06 1.3565e-02 8.63308e-02 0.0312 1.3565e-02 3\n",
" 13 2.0863e-01 7.3375e-02 9.9020e-07 1.3525e-02 7.22526e-02 0.0312 1.3525e-02 3\n",
" 14 2.0646e-01 7.3375e-02 9.6577e-07 1.3307e-02 6.04599e-02 0.0625 1.3307e-02 3\n",
" 15 2.0524e-01 7.3375e-02 9.2401e-07 1.3186e-02 4.07340e-02 0.1250 1.3186e-02 3\n",
" 16 2.0060e-01 7.3375e-02 8.4442e-07 1.2721e-02 1.42267e-02 1.0000 1.2721e-02 2\n",
" END 9.9904e-02 7.3375e-02 3.8950e-07 2.6526e-03 ---- ---- 2.6526e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.3955e+01 7.8768e-02 3.2680e+00 1.1964e-01 5.23105e-01 1.0000 ---- 3\n",
" 2 3.3381e+01 7.3155e-02 9.5432e-02 3.2353e+00 3.36525e-01 0.5000 3.2353e+00 3\n",
" 3 3.0389e+01 7.3148e-02 4.7622e-02 2.9839e+00 3.96202e-01 0.1250 2.9839e+00 3\n",
" 4 2.7477e+01 7.3143e-02 4.1673e-02 2.6987e+00 1.69444e-01 1.0000 2.6987e+00 3\n",
" 5 2.0966e+01 7.3165e-02 2.7166e-04 2.0890e+00 5.62118e-01 0.0625 2.0890e+00 3\n",
" 6 2.0710e+01 7.3155e-02 2.5620e-04 2.0634e+00 4.25616e-01 0.1250 2.0634e+00 3\n",
" 7 2.0570e+01 7.3147e-02 2.2626e-04 2.0494e+00 1.86098e-01 0.5000 2.0494e+00 3\n",
" 8 1.5786e+01 7.3151e-02 1.0940e-04 1.5712e+00 2.63967e-01 0.1250 1.5712e+00 3\n",
" 9 1.4268e+01 7.3149e-02 9.5826e-05 1.4194e+00 1.03686e-01 1.0000 1.4194e+00 3\n",
" 10 7.8767e+00 7.3163e-02 1.5835e-05 7.8034e-01 3.87594e-01 0.0312 7.8034e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 7.7497e+00 7.3160e-02 1.5525e-05 7.6764e-01 3.36085e-01 0.0625 7.6764e-01 3\n",
" 12 7.7462e+00 7.3156e-02 1.5012e-05 7.6729e-01 2.40350e-01 0.1250 7.6729e-01 3\n",
" 13 7.6880e+00 7.3153e-02 1.3793e-05 7.6147e-01 9.39891e-02 0.5000 7.6147e-01 3\n",
" 14 4.8147e+00 7.3156e-02 1.0047e-05 4.7414e-01 1.39379e-01 0.1250 4.7414e-01 3\n",
" 15 4.3944e+00 7.3155e-02 9.1852e-06 4.3212e-01 5.08579e-02 1.0000 4.3212e-01 3\n",
" 16 1.7233e+00 7.3159e-02 3.4537e-06 1.6501e-01 2.04097e-01 0.0312 1.6501e-01 3\n",
" 17 1.7186e+00 7.3159e-02 3.3864e-06 1.6454e-01 1.72968e-01 0.0312 1.6454e-01 3\n",
" 18 1.6916e+00 7.3158e-02 3.3075e-06 1.6184e-01 1.46198e-01 0.0625 1.6184e-01 3\n",
" 19 1.6823e+00 7.3157e-02 3.1725e-06 1.6091e-01 1.00275e-01 0.1250 1.6091e-01 3\n",
" 20 1.6335e+00 7.3157e-02 2.9019e-06 1.5603e-01 3.66545e-02 1.0000 1.5603e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.1075e+00 7.3158e-02 2.1768e-06 1.0343e-01 1.47478e-01 0.0312 1.0343e-01 3\n",
" 22 1.0920e+00 7.3158e-02 2.1455e-06 1.0188e-01 1.24187e-01 0.0625 1.0188e-01 3\n",
" 23 1.0897e+00 7.3158e-02 2.1117e-06 1.0165e-01 8.47755e-02 0.1250 1.0165e-01 3\n",
" 24 1.0667e+00 7.3157e-02 2.0214e-06 9.9351e-02 3.08867e-02 1.0000 9.9351e-02 3\n",
" 25 8.2796e-01 7.3158e-02 1.3146e-06 7.5479e-02 1.23225e-01 0.0312 7.5479e-02 3\n",
" 26 8.1574e-01 7.3158e-02 1.2942e-06 7.4256e-02 1.03970e-01 0.0625 7.4256e-02 3\n",
" 27 8.1064e-01 7.3158e-02 1.2739e-06 7.3747e-02 7.13444e-02 0.1250 7.3747e-02 3\n",
" 28 7.8749e-01 7.3157e-02 1.2344e-06 7.1432e-02 2.64889e-02 1.0000 7.1432e-02 3\n",
" 29 5.6854e-01 7.3158e-02 1.2030e-06 4.9537e-02 1.04857e-01 0.0312 4.9537e-02 3\n",
" 30 5.6071e-01 7.3158e-02 1.1825e-06 4.8754e-02 8.83891e-02 0.0625 4.8754e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 5.5758e-01 7.3158e-02 1.1554e-06 4.8441e-02 6.06139e-02 0.1250 4.8441e-02 3\n",
" 32 5.4426e-01 7.3157e-02 1.0930e-06 4.7109e-02 2.26506e-02 1.0000 4.7109e-02 3\n",
" 33 4.5209e-01 7.3158e-02 6.3653e-07 3.7893e-02 8.86480e-02 0.0312 3.7893e-02 3\n",
" 34 4.4547e-01 7.3158e-02 6.2750e-07 3.7230e-02 7.49063e-02 0.0625 3.7230e-02 3\n",
" 35 4.4099e-01 7.3158e-02 6.2024e-07 3.6782e-02 5.16637e-02 0.1250 3.6782e-02 3\n",
" 36 4.2761e-01 7.3157e-02 6.0526e-07 3.5444e-02 1.96999e-02 1.0000 3.5444e-02 3\n",
" 37 3.4360e-01 7.3158e-02 6.2451e-07 2.7044e-02 7.62059e-02 0.0625 2.7044e-02 2\n",
" 38 3.3621e-01 7.3158e-02 6.1964e-07 2.6304e-02 5.26354e-02 0.1250 2.6304e-02 2\n",
" 39 3.1688e-01 7.3158e-02 6.0367e-07 2.4371e-02 2.02493e-02 1.0000 2.4371e-02 2\n",
" 40 2.5472e-01 7.3158e-02 5.1835e-07 1.8156e-02 7.73828e-02 0.0312 1.8156e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 2.4957e-01 7.3158e-02 5.1052e-07 1.7640e-02 6.55712e-02 0.0625 1.7640e-02 2\n",
" 42 2.4530e-01 7.3158e-02 5.0297e-07 1.7214e-02 4.55761e-02 0.1250 1.7214e-02 2\n",
" 43 2.3538e-01 7.3158e-02 4.8834e-07 1.6222e-02 1.79141e-02 0.5000 1.6222e-02 2\n",
" END 1.5274e-01 7.3158e-02 3.7024e-07 7.9583e-03 ---- ---- 7.9583e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.5014e+01 7.8541e-02 3.2765e+00 2.1709e-01 5.18066e-01 0.5000 ---- 3\n",
" 2 2.6868e+01 7.4056e-02 1.6487e+00 1.0307e+00 2.94740e-01 1.0000 1.0307e+00 3\n",
" 3 6.6132e+00 7.3485e-02 1.2481e-02 6.4149e-01 2.72724e-01 0.1250 6.4149e-01 3\n",
" 4 6.1471e+00 7.3482e-02 1.0917e-02 5.9644e-01 1.18031e-01 0.5000 5.9644e-01 3\n",
" 5 4.1295e+00 7.3482e-02 5.4317e-03 4.0017e-01 1.45074e-01 0.2500 4.0017e-01 3\n",
" 6 3.6844e+00 7.3481e-02 4.0799e-03 3.5701e-01 3.02340e-02 1.0000 3.5701e-01 3\n",
" 7 3.0448e-01 7.3483e-02 1.4961e-05 2.3085e-02 1.06217e-01 0.0156 2.3085e-02 3\n",
" 8 3.0303e-01 7.3483e-02 1.4721e-05 2.2940e-02 9.78052e-02 0.0156 2.2940e-02 3\n",
" 9 3.0091e-01 7.3483e-02 1.4486e-05 2.2728e-02 9.00482e-02 0.0156 2.2728e-02 3\n",
" 10 2.9832e-01 7.3483e-02 1.4255e-05 2.2469e-02 8.28976e-02 0.0312 2.2469e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.9691e-01 7.3483e-02 1.3794e-05 2.2329e-02 6.97061e-02 0.0312 2.2329e-02 3\n",
" 12 2.9274e-01 7.3483e-02 1.3351e-05 2.1913e-02 5.86221e-02 0.0625 2.1913e-02 2\n",
" 13 2.8250e-01 7.3483e-02 1.2482e-05 2.0890e-02 3.99333e-02 0.1250 2.0890e-02 2\n",
" 14 2.6116e-01 7.3483e-02 1.0852e-05 1.8757e-02 1.45556e-02 1.0000 1.8757e-02 2\n",
" END 1.7310e-01 7.3483e-02 8.5855e-07 9.9606e-03 ---- ---- 9.9606e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.5297e+01 7.8871e-02 3.3030e+00 2.1878e-01 5.44392e-01 0.5000 ---- 3\n",
" 2 2.7586e+01 7.4390e-02 1.6623e+00 1.0889e+00 3.41365e-01 1.0000 1.0889e+00 3\n",
" 3 1.8835e+01 7.3821e-02 1.3373e-02 1.8627e+00 4.95568e-01 0.1250 1.8627e+00 3\n",
" 4 1.8434e+01 7.3812e-02 1.1697e-02 1.8243e+00 2.12897e-01 0.5000 1.8243e+00 3\n",
" 5 1.3984e+01 7.3814e-02 5.8010e-03 1.3852e+00 2.78746e-01 0.2500 1.3852e+00 3\n",
" 6 1.3573e+01 7.3811e-02 4.3684e-03 1.3456e+00 5.77402e-02 1.0000 1.3456e+00 3\n",
" 7 9.2860e-01 7.3824e-02 2.8669e-05 8.5449e-02 2.24105e-01 0.0156 8.5449e-02 3\n",
" 8 9.2349e-01 7.3823e-02 2.8210e-05 8.4938e-02 2.06798e-01 0.0156 8.4938e-02 3\n",
" 9 9.1593e-01 7.3823e-02 2.7759e-05 8.4183e-02 1.90691e-01 0.0156 8.4183e-02 3\n",
" 10 9.0640e-01 7.3823e-02 2.7316e-05 8.3231e-02 1.75736e-01 0.0312 8.3231e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 9.0306e-01 7.3822e-02 2.6428e-05 8.2898e-02 1.47922e-01 0.0312 8.2898e-02 3\n",
" 12 8.8817e-01 7.3822e-02 2.5576e-05 8.1410e-02 1.24341e-01 0.0625 8.1410e-02 3\n",
" 13 8.7950e-01 7.3821e-02 2.3898e-05 8.0544e-02 8.44198e-02 0.1250 8.0544e-02 3\n",
" 14 8.4491e-01 7.3821e-02 2.0746e-05 7.7088e-02 3.04202e-02 1.0000 7.7088e-02 3\n",
" 15 4.9686e-01 7.3822e-02 2.2378e-06 4.2301e-02 1.22302e-01 0.0312 4.2301e-02 3\n",
" 16 4.9319e-01 7.3821e-02 2.1875e-06 4.1935e-02 1.02733e-01 0.0312 4.1935e-02 3\n",
" 17 4.8460e-01 7.3821e-02 2.1332e-06 4.1075e-02 8.62699e-02 0.0625 4.1075e-02 3\n",
" 18 4.7698e-01 7.3821e-02 2.0418e-06 4.0314e-02 5.85936e-02 0.1250 4.0314e-02 3\n",
" 19 4.5577e-01 7.3821e-02 1.8772e-06 3.8193e-02 2.11635e-02 1.0000 3.8193e-02 3\n",
" 20 2.7131e-01 7.3821e-02 1.5368e-06 1.9747e-02 8.50271e-02 0.0312 1.9747e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.6615e-01 7.3821e-02 1.5066e-06 1.9232e-02 7.14927e-02 0.0625 1.9232e-02 2\n",
" 22 2.6123e-01 7.3821e-02 1.4648e-06 1.8739e-02 4.87325e-02 0.1250 1.8739e-02 2\n",
" 23 2.4777e-01 7.3821e-02 1.3860e-06 1.7393e-02 1.77517e-02 1.0000 1.7393e-02 2\n",
" END 1.5358e-01 7.3821e-02 1.1530e-06 7.9752e-03 ---- ---- 7.9752e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.5209e+01 7.9211e-02 3.3281e+00 1.8484e-01 5.45417e-01 0.5000 ---- 3\n",
" 2 2.6702e+01 7.4736e-02 1.6752e+00 9.8759e-01 3.57164e-01 1.0000 9.8759e-01 3\n",
" 3 2.1488e+01 7.4172e-02 1.3778e-02 2.1276e+00 5.31268e-01 0.1250 2.1276e+00 3\n",
" 4 2.1303e+01 7.4162e-02 1.2050e-02 2.1109e+00 2.32116e-01 0.5000 2.1109e+00 3\n",
" 5 1.6986e+01 7.4164e-02 5.9636e-03 1.6853e+00 3.02784e-01 0.2500 1.6853e+00 3\n",
" 6 1.6314e+01 7.4160e-02 4.4928e-03 1.6195e+00 6.33425e-02 1.0000 1.6195e+00 3\n",
" 7 4.4257e-01 7.4177e-02 3.0995e-05 3.6808e-02 2.41674e-01 0.0312 3.6808e-02 3\n",
" 8 4.4122e-01 7.4176e-02 2.9917e-05 3.6674e-02 2.04048e-01 0.0312 3.6674e-02 3\n",
" 9 4.3471e-01 7.4175e-02 2.8899e-05 3.6025e-02 1.71848e-01 0.0625 3.6025e-02 3\n",
" 10 4.3088e-01 7.4174e-02 2.6848e-05 3.5644e-02 1.16920e-01 0.1250 3.5644e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 4.1651e-01 7.4173e-02 2.3026e-05 3.4211e-02 4.22428e-02 0.5000 3.4211e-02 3\n",
" 12 2.5175e-01 7.4173e-02 1.0805e-05 1.7746e-02 6.44953e-02 0.2500 1.7746e-02 2\n",
" 13 2.5054e-01 7.4173e-02 7.5440e-06 1.7629e-02 1.94727e-02 0.5000 1.7629e-02 2\n",
" END 1.5970e-01 7.4173e-02 3.8103e-06 8.5492e-03 ---- ---- 8.5492e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.5437e+01 7.9565e-02 3.3560e+00 1.7970e-01 5.34841e-01 0.5000 ---- 3\n",
" 2 2.5547e+01 7.5098e-02 1.6894e+00 8.5785e-01 3.07700e-01 1.0000 8.5785e-01 3\n",
" 3 9.7676e+00 7.4541e-02 1.3665e-02 9.5564e-01 3.60244e-01 0.1250 9.5564e-01 3\n",
" 4 9.5296e+00 7.4537e-02 1.1952e-02 9.3356e-01 1.50941e-01 0.5000 9.3356e-01 3\n",
" 5 6.7147e+00 7.4538e-02 5.9418e-03 6.5808e-01 1.97928e-01 0.2500 6.5808e-01 3\n",
" 6 6.3684e+00 7.4536e-02 4.4667e-03 6.2492e-01 4.59324e-02 1.0000 6.2492e-01 3\n",
" 7 7.9576e-01 7.4542e-02 2.5017e-05 7.2097e-02 1.74027e-01 0.0156 7.2097e-02 3\n",
" 8 7.8743e-01 7.4541e-02 2.4619e-05 7.1264e-02 1.60228e-01 0.0312 7.1264e-02 3\n",
" 9 7.8369e-01 7.4541e-02 2.3827e-05 7.0891e-02 1.34662e-01 0.0312 7.0891e-02 3\n",
" 10 7.7062e-01 7.4541e-02 2.3065e-05 6.9585e-02 1.13070e-01 0.0625 6.9585e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 7.6224e-01 7.4540e-02 2.1570e-05 6.8748e-02 7.66262e-02 0.1250 6.8748e-02 3\n",
" 12 7.3239e-01 7.4540e-02 1.8766e-05 6.5766e-02 2.72113e-02 1.0000 6.5766e-02 3\n",
" 13 4.6369e-01 7.4540e-02 1.5309e-06 3.8914e-02 1.11397e-01 0.0312 3.8914e-02 3\n",
" 14 4.6168e-01 7.4540e-02 1.5098e-06 3.8712e-02 9.35063e-02 0.0312 3.8712e-02 3\n",
" 15 4.5474e-01 7.4540e-02 1.4815e-06 3.8019e-02 7.84719e-02 0.0625 3.8019e-02 3\n",
" 16 4.4992e-01 7.4540e-02 1.4424e-06 3.7537e-02 5.32449e-02 0.1250 3.7537e-02 3\n",
" 17 4.3654e-01 7.4540e-02 1.3615e-06 3.6198e-02 1.90604e-02 1.0000 3.6198e-02 3\n",
" 18 3.4011e-01 7.4540e-02 8.5442e-07 2.6556e-02 7.77549e-02 0.0312 2.6556e-02 3\n",
" 19 3.3669e-01 7.4540e-02 8.4130e-07 2.6214e-02 6.53374e-02 0.0625 2.6214e-02 2\n",
" 20 3.2539e-01 7.4540e-02 8.2827e-07 2.5084e-02 4.44223e-02 0.1250 2.5084e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.9816e-01 7.4540e-02 8.0066e-07 2.2361e-02 1.60369e-02 1.0000 2.2361e-02 2\n",
" END 1.2917e-01 7.4540e-02 7.4330e-07 5.4619e-03 ---- ---- 5.4619e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6149e+01 7.9933e-02 3.3838e+00 2.2311e-01 5.47203e-01 0.5000 ---- 3\n",
" 2 2.7828e+01 7.5475e-02 1.7036e+00 1.0716e+00 3.51383e-01 1.0000 1.0716e+00 3\n",
" 3 1.8395e+01 7.4929e-02 1.4401e-02 1.8176e+00 5.00760e-01 0.1250 1.8176e+00 3\n",
" 4 1.8262e+01 7.4921e-02 1.2595e-02 1.8062e+00 2.15111e-01 0.5000 1.8062e+00 3\n",
" 5 1.4010e+01 7.4923e-02 6.2440e-03 1.3873e+00 2.85848e-01 0.2500 1.3873e+00 3\n",
" 6 1.3449e+01 7.4920e-02 4.7014e-03 1.3327e+00 6.45976e-02 1.0000 1.3327e+00 3\n",
" 7 5.7461e-01 7.4933e-02 3.1226e-05 4.9937e-02 2.44477e-01 0.0078 4.9937e-02 3\n",
" 8 5.7211e-01 7.4933e-02 3.0974e-05 4.9687e-02 2.34925e-01 0.0078 4.9687e-02 3\n",
" 9 5.6934e-01 7.4932e-02 3.0725e-05 4.9410e-02 2.24855e-01 0.0156 4.9410e-02 3\n",
" 10 5.6923e-01 7.4932e-02 3.0216e-05 4.9399e-02 2.07401e-01 0.0156 4.9399e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 5.6715e-01 7.4932e-02 2.9720e-05 4.9192e-02 1.91225e-01 0.0156 4.9192e-02 3\n",
" 12 5.6348e-01 7.4931e-02 2.9234e-05 4.8825e-02 1.76243e-01 0.0156 4.8825e-02 3\n",
" 13 5.5852e-01 7.4931e-02 2.8759e-05 4.8330e-02 1.62386e-01 0.0312 4.8330e-02 3\n",
" 14 5.5820e-01 7.4931e-02 2.7798e-05 4.8299e-02 1.36758e-01 0.0312 4.8299e-02 3\n",
" 15 5.5057e-01 7.4931e-02 2.6884e-05 4.7537e-02 1.15112e-01 0.0625 4.7537e-02 3\n",
" 16 5.4801e-01 7.4930e-02 2.5073e-05 4.7283e-02 7.86386e-02 0.1250 4.7283e-02 3\n",
" 17 5.3288e-01 7.4930e-02 2.1692e-05 4.5773e-02 2.93230e-02 1.0000 4.5773e-02 3\n",
" 18 4.5553e-01 7.4930e-02 2.0864e-06 3.8058e-02 1.14830e-01 0.0312 3.8058e-02 3\n",
" 19 4.4860e-01 7.4930e-02 2.0547e-06 3.7365e-02 9.64103e-02 0.0625 3.7365e-02 3\n",
" 20 4.4435e-01 7.4930e-02 2.0281e-06 3.6940e-02 6.54805e-02 0.1250 3.6940e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.3266e-01 7.4930e-02 1.9952e-06 3.5771e-02 2.38681e-02 1.0000 3.5771e-02 3\n",
" 22 3.7805e-01 7.4930e-02 2.2893e-06 3.0310e-02 9.59177e-02 0.0312 3.0310e-02 3\n",
" 23 3.7361e-01 7.4930e-02 2.2462e-06 2.9866e-02 8.06839e-02 0.0625 2.9866e-02 3\n",
" 24 3.7152e-01 7.4930e-02 2.1882e-06 2.9657e-02 5.50248e-02 0.1250 2.9657e-02 3\n",
" 25 3.6254e-01 7.4929e-02 2.0757e-06 2.8759e-02 2.00933e-02 1.0000 2.8759e-02 3\n",
" 26 2.8152e-01 7.4930e-02 1.5967e-06 2.0658e-02 8.09923e-02 0.0625 2.0658e-02 2\n",
" 27 2.7672e-01 7.4930e-02 1.5860e-06 2.0177e-02 5.52501e-02 0.1250 2.0177e-02 2\n",
" 28 2.6192e-01 7.4929e-02 1.5534e-06 1.8697e-02 2.02387e-02 1.0000 1.8697e-02 2\n",
" 29 1.9885e-01 7.4930e-02 1.4551e-06 1.2391e-02 8.14008e-02 0.0312 1.2391e-02 2\n",
" 30 1.9581e-01 7.4930e-02 1.4315e-06 1.2086e-02 6.92381e-02 0.0020 1.2086e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.9581e-01 7.4930e-02 1.4315e-06 1.2086e-02 6.92551e-02 0.0312 1.2086e-02 1\n",
" 32 1.9531e-01 7.4930e-02 1.4519e-06 1.2037e-02 5.84474e-02 0.0156 1.2037e-02 1\n",
" 33 1.9528e-01 7.4930e-02 1.4421e-06 1.2034e-02 5.39321e-02 0.0312 1.2034e-02 1\n",
" 34 1.9443e-01 7.4929e-02 1.4433e-06 1.1949e-02 4.55802e-02 0.0312 1.1949e-02 1\n",
" 35 1.9394e-01 7.4929e-02 1.4349e-06 1.1899e-02 3.85679e-02 0.0625 1.1899e-02 1\n",
" 36 1.9298e-01 7.4929e-02 1.4626e-06 1.1803e-02 2.67825e-02 0.1250 1.1803e-02 1\n",
" 37 1.9171e-01 7.4929e-02 1.5679e-06 1.1676e-02 1.09801e-02 1.0000 1.1676e-02 1\n",
" 38 1.7922e-01 7.4929e-02 5.6396e-06 1.0423e-02 3.86465e-02 0.0312 1.0423e-02 1\n",
" 39 1.7825e-01 7.4929e-02 5.4692e-06 1.0327e-02 3.26059e-02 0.0625 1.0327e-02 1\n",
" 40 1.7726e-01 7.4929e-02 5.1442e-06 1.0228e-02 2.24250e-02 0.1250 1.0228e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 1.7486e-01 7.4929e-02 4.5333e-06 9.9883e-03 ---- ---- 9.9883e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6122e+01 8.0322e-02 3.4094e+00 1.9479e-01 5.33277e-01 0.5000 ---- 3\n",
" 2 2.6116e+01 7.5874e-02 1.7167e+00 8.8726e-01 2.90007e-01 1.0000 8.8726e-01 3\n",
" 3 5.2594e+00 7.5339e-02 1.4139e-02 5.0427e-01 2.44587e-01 0.1250 5.0427e-01 3\n",
" 4 4.7197e+00 7.5338e-02 1.2368e-02 4.5207e-01 1.09958e-01 0.5000 4.5207e-01 3\n",
" 5 2.9098e+00 7.5337e-02 6.1538e-03 2.7729e-01 1.28215e-01 0.2500 2.7729e-01 3\n",
" 6 2.5273e+00 7.5336e-02 4.6232e-03 2.4057e-01 2.55562e-02 1.0000 2.4057e-01 3\n",
" END 9.4560e-02 7.5338e-02 1.8545e-05 1.9037e-03 ---- ---- 1.9037e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6605e+01 8.0731e-02 3.4405e+00 2.1196e-01 5.71544e-01 0.5000 ---- 3\n",
" 2 2.8668e+01 7.6296e-02 1.7327e+00 1.1265e+00 3.30095e-01 1.0000 1.1265e+00 3\n",
" 3 9.0331e+00 7.5774e-02 1.4642e-02 8.8109e-01 4.15563e-01 0.0625 8.8109e-01 3\n",
" 4 8.6821e+00 7.5771e-02 1.3725e-02 8.4691e-01 2.93621e-01 0.1250 8.4691e-01 3\n",
" 5 7.9710e+00 7.5769e-02 1.2003e-02 7.7752e-01 1.20309e-01 1.0000 7.7752e-01 3\n",
" 6 4.7988e+00 7.5775e-02 3.1318e-04 4.7199e-01 4.32737e-01 0.0312 4.7199e-01 3\n",
" 7 4.6912e+00 7.5774e-02 3.0405e-04 4.6124e-01 3.68738e-01 0.0625 4.6124e-01 3\n",
" 8 4.5986e+00 7.5772e-02 2.8688e-04 4.5200e-01 2.58863e-01 0.1250 4.5200e-01 3\n",
" 9 4.4134e+00 7.5770e-02 2.5433e-04 4.3351e-01 1.03826e-01 0.5000 4.3351e-01 3\n",
" 10 2.7401e+00 7.5771e-02 1.3366e-04 2.6630e-01 1.44928e-01 0.2500 2.6630e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.6619e+00 7.5771e-02 1.0451e-04 2.5851e-01 3.30397e-02 1.0000 2.5851e-01 3\n",
" 12 2.0828e-01 7.5773e-02 3.6947e-06 1.3247e-02 1.35896e-01 0.0156 1.3247e-02 2\n",
" 13 2.0701e-01 7.5773e-02 3.6540e-06 1.3120e-02 1.25091e-01 0.0156 1.3120e-02 2\n",
" 14 2.0530e-01 7.5773e-02 3.6115e-06 1.2949e-02 1.15137e-01 0.0156 1.2949e-02 2\n",
" 15 2.0324e-01 7.5773e-02 3.5675e-06 1.2743e-02 1.05966e-01 0.0312 1.2743e-02 2\n",
" 16 2.0209e-01 7.5773e-02 3.4986e-06 1.2628e-02 8.90626e-02 0.0312 1.2628e-02 2\n",
" 17 1.9926e-01 7.5773e-02 3.4199e-06 1.2345e-02 7.48572e-02 0.0625 1.2345e-02 2\n",
" 18 1.9788e-01 7.5773e-02 3.2940e-06 1.2207e-02 5.10659e-02 0.1250 1.2207e-02 2\n",
" 19 1.9546e-01 7.5773e-02 3.0502e-06 1.1966e-02 1.87427e-02 0.5000 1.1966e-02 2\n",
" END 1.3313e-01 7.5773e-02 1.9181e-06 5.7337e-03 ---- ---- 5.7337e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6841e+01 8.1164e-02 3.4696e+00 2.0635e-01 5.44204e-01 0.5000 ---- 3\n",
" 2 2.8476e+01 7.6742e-02 1.7475e+00 1.0924e+00 3.11945e-01 1.0000 1.0924e+00 3\n",
" 3 7.2015e+00 7.6236e-02 1.5174e-02 6.9735e-01 3.32451e-01 0.1250 6.9735e-01 3\n",
" 4 6.9449e+00 7.6233e-02 1.3272e-02 6.7359e-01 1.38139e-01 0.5000 6.7359e-01 3\n",
" 5 4.4946e+00 7.6233e-02 6.5947e-03 4.3525e-01 1.79548e-01 0.2500 4.3525e-01 3\n",
" 6 4.2426e+00 7.6232e-02 4.9604e-03 4.1168e-01 4.55392e-02 1.0000 4.1168e-01 3\n",
" 7 6.9881e-01 7.6235e-02 2.5712e-05 6.2231e-02 1.74101e-01 0.0156 6.2231e-02 3\n",
" 8 6.9299e-01 7.6235e-02 2.5282e-05 6.1651e-02 1.60064e-01 0.0156 6.1651e-02 3\n",
" 9 6.8574e-01 7.6235e-02 2.4862e-05 6.0926e-02 1.47130e-01 0.0312 6.0926e-02 3\n",
" 10 6.8192e-01 7.6235e-02 2.3999e-05 6.0545e-02 1.23337e-01 0.0312 6.0545e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 6.7018e-01 7.6234e-02 2.3187e-05 5.9371e-02 1.03397e-01 0.0625 5.9371e-02 3\n",
" 12 6.6089e-01 7.6234e-02 2.1558e-05 5.8444e-02 6.98451e-02 0.1250 5.8444e-02 3\n",
" 13 6.3089e-01 7.6234e-02 1.8512e-05 5.5447e-02 2.47093e-02 1.0000 5.5447e-02 3\n",
" 14 3.7960e-01 7.6234e-02 3.6041e-06 3.0333e-02 1.02581e-01 0.0020 3.0333e-02 1\n",
" 15 3.7960e-01 7.6234e-02 3.6041e-06 3.0333e-02 1.02503e-01 0.0156 3.0333e-02 1\n",
" 16 3.7735e-01 7.6234e-02 3.5391e-06 3.0108e-02 9.43761e-02 0.0078 3.0108e-02 1\n",
" 17 3.7700e-01 7.6234e-02 3.5092e-06 3.0073e-02 9.02272e-02 0.0156 3.0073e-02 1\n",
" 18 3.7540e-01 7.6234e-02 3.4459e-06 2.9913e-02 8.31397e-02 0.0156 2.9913e-02 1\n",
" 19 3.7458e-01 7.6234e-02 3.3837e-06 2.9831e-02 7.66287e-02 0.0312 2.9831e-02 1\n",
" 20 3.7400e-01 7.6234e-02 3.2456e-06 2.9773e-02 6.47492e-02 0.0312 2.9773e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.7246e-01 7.6234e-02 3.1145e-06 2.9620e-02 5.48199e-02 0.0625 2.9620e-02 1\n",
" 22 3.7162e-01 7.6234e-02 2.8139e-06 2.9536e-02 3.82051e-02 0.1250 2.9536e-02 1\n",
" 23 3.6885e-01 7.6234e-02 2.1462e-06 2.9260e-02 1.58982e-02 1.0000 2.9260e-02 1\n",
" 24 3.2795e-01 7.6234e-02 8.5334e-06 2.5163e-02 5.21276e-02 0.0625 2.5163e-02 1\n",
" 25 3.2721e-01 7.6234e-02 7.9482e-06 2.5090e-02 3.84039e-02 0.2500 2.5090e-02 2\n",
" 26 2.9376e-01 7.6234e-02 7.6006e-06 2.1745e-02 9.97581e-03 0.0020 2.1745e-02 1\n",
" 27 2.9376e-01 7.6234e-02 7.6006e-06 2.1745e-02 9.87294e-03 1.0000 2.1745e-02 1\n",
" END 1.7443e-01 7.6234e-02 1.4062e-06 9.8181e-03 ---- ---- 9.8181e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7051e+01 8.1612e-02 3.4725e+00 2.2450e-01 5.49595e-01 0.5000 ---- 3\n",
" 2 2.7881e+01 7.6559e-02 1.7691e+00 1.0113e+00 3.20543e-01 1.0000 1.0113e+00 3\n",
" 3 1.0827e+01 7.5413e-02 3.8108e-02 1.0370e+00 3.76119e-01 0.0625 1.0370e+00 3\n",
" 4 1.0425e+01 7.5410e-02 3.5724e-02 9.9921e-01 2.63597e-01 0.1250 9.9921e-01 3\n",
" 5 9.5668e+00 7.5407e-02 3.1255e-02 9.1788e-01 1.02487e-01 1.0000 9.1788e-01 3\n",
" 6 3.9967e+00 7.5413e-02 2.0019e-04 3.9192e-01 3.72811e-01 0.0312 3.9192e-01 3\n",
" 7 3.9466e+00 7.5411e-02 1.9413e-04 3.8693e-01 3.15074e-01 0.0312 3.8693e-01 3\n",
" 8 3.8571e+00 7.5410e-02 1.8819e-04 3.7798e-01 2.65821e-01 0.0625 3.7798e-01 3\n",
" 9 3.7717e+00 7.5408e-02 1.7677e-04 3.6945e-01 1.82027e-01 0.1250 3.6945e-01 3\n",
" 10 3.5714e+00 7.5407e-02 1.5519e-04 3.4945e-01 6.71296e-02 1.0000 3.4945e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.8416e+00 7.5410e-02 7.0781e-06 2.7661e-01 2.62038e-01 0.0312 2.7661e-01 3\n",
" 12 2.7798e+00 7.5409e-02 6.9174e-06 2.7043e-01 2.21803e-01 0.0625 2.7043e-01 3\n",
" 13 2.7272e+00 7.5408e-02 6.6753e-06 2.6517e-01 1.53023e-01 0.1250 2.6517e-01 3\n",
" 14 2.6035e+00 7.5408e-02 6.2680e-06 2.5280e-01 5.81461e-02 1.0000 2.5280e-01 3\n",
" 15 2.3244e+00 7.5410e-02 6.5683e-06 2.2490e-01 2.21100e-01 0.0625 2.2490e-01 3\n",
" 16 2.3090e+00 7.5409e-02 6.4377e-06 2.2335e-01 1.53564e-01 0.1250 2.2335e-01 3\n",
" 17 2.2697e+00 7.5408e-02 6.1572e-06 2.1942e-01 6.01192e-02 0.5000 2.1942e-01 3\n",
" 18 1.5945e+00 7.5408e-02 4.4209e-06 1.5191e-01 8.32270e-02 0.2500 1.5191e-01 3\n",
" 19 1.4793e+00 7.5408e-02 3.8873e-06 1.4039e-01 1.97288e-02 1.0000 1.4039e-01 3\n",
" 20 3.6577e-01 7.5409e-02 6.7510e-07 2.9036e-02 8.57764e-02 0.0312 2.9036e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.6303e-01 7.5409e-02 6.6604e-07 2.8761e-02 7.13066e-02 0.0312 2.8761e-02 3\n",
" 22 3.5699e-01 7.5409e-02 6.5385e-07 2.8158e-02 5.92736e-02 0.0625 2.8158e-02 3\n",
" 23 3.5028e-01 7.5409e-02 6.3756e-07 2.7486e-02 3.92522e-02 0.1250 2.7486e-02 3\n",
" 24 3.3133e-01 7.5409e-02 6.0345e-07 2.5592e-02 1.28335e-02 1.0000 2.5592e-02 3\n",
" END 1.5660e-01 7.5409e-02 3.9221e-07 8.1189e-03 ---- ---- 8.1189e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6767e+01 8.0769e-02 3.4426e+00 2.2597e-01 5.36426e-01 0.5000 ---- 3\n",
" 2 2.8558e+01 7.5851e-02 1.7493e+00 1.0989e+00 3.23555e-01 1.0000 1.0989e+00 3\n",
" 3 1.5477e+01 7.4839e-02 3.3222e-02 1.5070e+00 4.10162e-01 0.0625 1.5070e+00 3\n",
" 4 1.4868e+01 7.4834e-02 3.1145e-02 1.4482e+00 2.86922e-01 0.1250 1.4482e+00 3\n",
" 5 1.3705e+01 7.4831e-02 2.7248e-02 1.3357e+00 1.09673e-01 1.0000 1.3357e+00 3\n",
" 6 8.7734e+00 7.4841e-02 1.7141e-04 8.6969e-01 3.74896e-01 0.0312 8.6969e-01 3\n",
" 7 8.6043e+00 7.4838e-02 1.6607e-04 8.5278e-01 3.26866e-01 0.0625 8.5278e-01 3\n",
" 8 8.5179e+00 7.4834e-02 1.5564e-04 8.4415e-01 2.35031e-01 0.1250 8.4415e-01 3\n",
" 9 8.2612e+00 7.4831e-02 1.3562e-04 8.1850e-01 9.20398e-02 1.0000 8.1850e-01 3\n",
" 10 6.9052e+00 7.4839e-02 2.4019e-05 6.8302e-01 3.58866e-01 0.0312 6.8302e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 6.7620e+00 7.4836e-02 2.3671e-05 6.6869e-01 3.03983e-01 0.0625 6.6869e-01 3\n",
" 12 6.6646e+00 7.4834e-02 2.3291e-05 6.5895e-01 2.08654e-01 0.1250 6.5895e-01 3\n",
" 13 6.4519e+00 7.4832e-02 2.2207e-05 6.3768e-01 7.52786e-02 1.0000 6.3768e-01 3\n",
" 14 6.3013e+00 7.4838e-02 1.1157e-05 6.2263e-01 2.80019e-01 0.0312 6.2263e-01 3\n",
" 15 6.1520e+00 7.4836e-02 1.0910e-05 6.0771e-01 2.43066e-01 0.0625 6.0771e-01 3\n",
" 16 6.0008e+00 7.4834e-02 1.0589e-05 5.9259e-01 1.73771e-01 0.1250 5.9259e-01 3\n",
" 17 5.6443e+00 7.4832e-02 1.0241e-05 5.5694e-01 6.90985e-02 1.0000 5.5694e-01 3\n",
" 18 3.7554e+00 7.4838e-02 1.4449e-05 3.6804e-01 2.64763e-01 0.0312 3.6804e-01 3\n",
" 19 3.6838e+00 7.4836e-02 1.4220e-05 3.6089e-01 2.24179e-01 0.0625 3.6089e-01 3\n",
" 20 3.6450e+00 7.4835e-02 1.3943e-05 3.5700e-01 1.55104e-01 0.1250 3.5700e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.5485e+00 7.4834e-02 1.3252e-05 3.4735e-01 5.97887e-02 1.0000 3.4735e-01 3\n",
" 22 3.5469e+00 7.4837e-02 7.4955e-06 3.4720e-01 2.21434e-01 0.0312 3.4720e-01 3\n",
" 23 3.4625e+00 7.4836e-02 7.3527e-06 3.3876e-01 1.90290e-01 0.0625 3.3876e-01 3\n",
" 24 3.3709e+00 7.4835e-02 7.1854e-06 3.2960e-01 1.35027e-01 0.1250 3.2960e-01 3\n",
" 25 3.1598e+00 7.4834e-02 6.9472e-06 3.0849e-01 5.48203e-02 1.0000 3.0849e-01 3\n",
" 26 2.2526e+00 7.4837e-02 7.8881e-06 2.1777e-01 2.04879e-01 0.0312 2.1777e-01 3\n",
" 27 2.2039e+00 7.4836e-02 7.7471e-06 2.1290e-01 1.74255e-01 0.0625 2.1290e-01 3\n",
" 28 2.1615e+00 7.4835e-02 7.5631e-06 2.0866e-01 1.21886e-01 0.1250 2.0866e-01 3\n",
" 29 2.0737e+00 7.4835e-02 7.1831e-06 1.9988e-01 4.83922e-02 0.5000 1.9988e-01 3\n",
" 30 1.3052e+00 7.4835e-02 4.9810e-06 1.2303e-01 6.72726e-02 0.2500 1.2303e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.2258e+00 7.4835e-02 4.4029e-06 1.1509e-01 1.48185e-02 1.0000 1.1509e-01 3\n",
" 32 3.9093e-01 7.4836e-02 9.1298e-07 3.1609e-02 6.26685e-02 0.0312 3.1609e-02 3\n",
" 33 3.8919e-01 7.4836e-02 9.0126e-07 3.1435e-02 5.23335e-02 0.0312 3.1435e-02 3\n",
" 34 3.8337e-01 7.4836e-02 8.8538e-07 3.0852e-02 4.36888e-02 0.0625 3.0852e-02 3\n",
" 35 3.7817e-01 7.4836e-02 8.6552e-07 3.0333e-02 2.93103e-02 0.1250 3.0333e-02 3\n",
" 36 3.6079e-01 7.4836e-02 8.2447e-07 2.8594e-02 1.03752e-02 1.0000 2.8594e-02 3\n",
" END 1.6162e-01 7.4836e-02 5.4182e-07 8.6777e-03 ---- ---- 8.6777e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6575e+01 8.0183e-02 3.4202e+00 2.2926e-01 5.27399e-01 0.5000 ---- 3\n",
" 2 2.8616e+01 7.5357e-02 1.7345e+00 1.1195e+00 3.20947e-01 1.0000 1.1195e+00 3\n",
" 3 1.8531e+01 7.4438e-02 2.9382e-02 1.8163e+00 3.94644e-01 0.1250 1.8163e+00 3\n",
" 4 1.8208e+01 7.4429e-02 2.5709e-02 1.7876e+00 1.74680e-01 0.5000 1.7876e+00 3\n",
" 5 1.5924e+01 7.4430e-02 1.2817e-02 1.5722e+00 2.20056e-01 0.2500 1.5722e+00 3\n",
" 6 1.5326e+01 7.4426e-02 9.6281e-03 1.5155e+00 5.31285e-02 1.0000 1.5155e+00 3\n",
" 7 7.0651e-01 7.4442e-02 2.1702e-05 6.3185e-02 2.12736e-01 0.0312 6.3185e-02 3\n",
" 8 7.0495e-01 7.4441e-02 2.0989e-05 6.3029e-02 1.80267e-01 0.0312 6.3029e-02 3\n",
" 9 6.9421e-01 7.4440e-02 2.0318e-05 6.1957e-02 1.51566e-01 0.0625 6.1957e-02 3\n",
" 10 6.8785e-01 7.4439e-02 1.9025e-05 6.1322e-02 1.00912e-01 0.1250 6.1322e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 6.5792e-01 7.4438e-02 1.6668e-05 5.8331e-02 3.22421e-02 1.0000 5.8331e-02 3\n",
" 12 3.2936e-01 7.4439e-02 4.5175e-06 2.5488e-02 1.34832e-01 0.0625 2.5488e-02 2\n",
" 13 3.2825e-01 7.4438e-02 4.5283e-06 2.5377e-02 8.95054e-02 0.1250 2.5377e-02 2\n",
" 14 3.1767e-01 7.4438e-02 4.4758e-06 2.4319e-02 3.01203e-02 0.5000 2.4319e-02 2\n",
" END 1.7008e-01 7.4438e-02 3.2245e-06 9.5612e-03 ---- ---- 9.5612e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6232e+01 7.9775e-02 3.4040e+00 2.1129e-01 5.28521e-01 0.5000 ---- 3\n",
" 2 2.7339e+01 7.5013e-02 1.7239e+00 1.0025e+00 3.04964e-01 1.0000 1.0025e+00 3\n",
" 3 1.3538e+01 7.4158e-02 2.5534e-02 1.3209e+00 3.55599e-01 0.0625 1.3209e+00 3\n",
" 4 1.2977e+01 7.4154e-02 2.3937e-02 1.2663e+00 2.57736e-01 0.1250 1.2663e+00 3\n",
" 5 1.1928e+01 7.4151e-02 2.0942e-02 1.1645e+00 1.04742e-01 1.0000 1.1645e+00 3\n",
" 6 7.8855e+00 7.4160e-02 1.7538e-04 7.8096e-01 3.84697e-01 0.0312 7.8096e-01 3\n",
" 7 7.7185e+00 7.4157e-02 1.7017e-04 7.6427e-01 3.27180e-01 0.0625 7.6427e-01 3\n",
" 8 7.6074e+00 7.4154e-02 1.6024e-04 7.5317e-01 2.26601e-01 0.1250 7.5317e-01 3\n",
" 9 7.3821e+00 7.4151e-02 1.4127e-04 7.3065e-01 8.38181e-02 0.5000 7.3065e-01 3\n",
" 10 4.5703e+00 7.4154e-02 7.1267e-05 4.4955e-01 1.26221e-01 0.1250 4.4955e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 4.1731e+00 7.4153e-02 6.2645e-05 4.0983e-01 4.73092e-02 1.0000 4.0983e-01 3\n",
" 12 1.1720e+00 7.4157e-02 5.0450e-06 1.0978e-01 1.85967e-01 0.0156 1.0978e-01 3\n",
" 13 1.1612e+00 7.4157e-02 4.9849e-06 1.0870e-01 1.71190e-01 0.0156 1.0870e-01 3\n",
" 14 1.1483e+00 7.4156e-02 4.9223e-06 1.0741e-01 1.57538e-01 0.0312 1.0741e-01 3\n",
" 15 1.1414e+00 7.4156e-02 4.8184e-06 1.0672e-01 1.32312e-01 0.0312 1.0672e-01 3\n",
" 16 1.1216e+00 7.4156e-02 4.7005e-06 1.0474e-01 1.11070e-01 0.0625 1.0474e-01 3\n",
" 17 1.1092e+00 7.4155e-02 4.4921e-06 1.0350e-01 7.54425e-02 0.1250 1.0350e-01 3\n",
" 18 1.0702e+00 7.4155e-02 4.0638e-06 9.9605e-02 2.78839e-02 1.0000 9.9605e-02 3\n",
" 19 8.1511e-01 7.4156e-02 8.1255e-07 7.4095e-02 1.08519e-01 0.0312 7.4095e-02 3\n",
" 20 8.0022e-01 7.4156e-02 7.9993e-07 7.2606e-02 9.19303e-02 0.0625 7.2606e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 7.8730e-01 7.4155e-02 7.8713e-07 7.1313e-02 6.36409e-02 0.1250 7.1313e-02 3\n",
" 22 7.5290e-01 7.4155e-02 7.5845e-07 6.7874e-02 2.44288e-02 1.0000 6.7874e-02 3\n",
" 23 6.0331e-01 7.4156e-02 6.9250e-07 5.2915e-02 9.31881e-02 0.0312 5.2915e-02 3\n",
" 24 5.9178e-01 7.4156e-02 6.8221e-07 5.1762e-02 7.89028e-02 0.0625 5.1762e-02 3\n",
" 25 5.8059e-01 7.4155e-02 6.7113e-07 5.0643e-02 5.47590e-02 0.1250 5.0643e-02 3\n",
" 26 5.5698e-01 7.4155e-02 6.4432e-07 4.8282e-02 2.15450e-02 1.0000 4.8282e-02 3\n",
" 27 5.4068e-01 7.4156e-02 5.4095e-07 4.6652e-02 8.06007e-02 0.0625 4.6652e-02 2\n",
" 28 5.1865e-01 7.4155e-02 5.3625e-07 4.4449e-02 5.62537e-02 0.1250 4.4449e-02 2\n",
" 29 4.7578e-01 7.4155e-02 5.2783e-07 4.0162e-02 2.28060e-02 1.0000 4.0162e-02 2\n",
" 30 4.3661e-01 7.4156e-02 6.5763e-07 3.6245e-02 8.30819e-02 0.0625 3.6245e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 4.2728e-01 7.4156e-02 6.4806e-07 3.5312e-02 5.86802e-02 0.1250 3.5312e-02 2\n",
" 32 4.1213e-01 7.4155e-02 6.3169e-07 3.3797e-02 2.43377e-02 0.5000 3.3797e-02 2\n",
" 33 2.9509e-01 7.4155e-02 5.1422e-07 2.2093e-02 3.35909e-02 0.0020 2.2093e-02 1\n",
" 34 2.9509e-01 7.4155e-02 5.1422e-07 2.2093e-02 3.35639e-02 0.1250 2.2093e-02 1\n",
" 35 2.8791e-01 7.4155e-02 6.3494e-07 2.1375e-02 1.53671e-02 0.2500 2.1375e-02 1\n",
" 36 2.8168e-01 7.4155e-02 1.0394e-06 2.0752e-02 5.23444e-03 1.0000 2.0752e-02 1\n",
" END 1.2821e-01 7.4156e-02 1.4528e-06 5.4043e-03 ---- ---- 5.4043e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6314e+01 7.9481e-02 3.4020e+00 2.2142e-01 5.29236e-01 0.5000 ---- 3\n",
" 2 2.8034e+01 7.5063e-02 1.7116e+00 1.0842e+00 2.94610e-01 1.0000 1.0842e+00 3\n",
" 3 5.2208e+00 7.4570e-02 1.2505e-02 5.0212e-01 2.41900e-01 0.1250 5.0212e-01 3\n",
" 4 4.7958e+00 7.4568e-02 1.0939e-02 4.6118e-01 1.03792e-01 0.5000 4.6118e-01 3\n",
" 5 3.0230e+00 7.4568e-02 5.4498e-03 2.8940e-01 1.30108e-01 0.2500 2.8940e-01 3\n",
" 6 2.6197e+00 7.4567e-02 4.0922e-03 2.5042e-01 3.08167e-02 1.0000 2.5042e-01 3\n",
" 7 1.9630e-01 7.4569e-02 1.8191e-05 1.2155e-02 1.09730e-01 0.0020 1.2155e-02 1\n",
" 8 1.9630e-01 7.4569e-02 1.8191e-05 1.2155e-02 1.09689e-01 0.0156 1.2155e-02 1\n",
" 9 1.9617e-01 7.4568e-02 1.7871e-05 1.2142e-02 1.00814e-01 0.0039 1.2142e-02 1\n",
" 10 1.9612e-01 7.4568e-02 1.7799e-05 1.2138e-02 9.83461e-02 0.0078 1.2138e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.9576e-01 7.4568e-02 1.7652e-05 1.2102e-02 9.04314e-02 0.0078 1.2102e-02 1\n",
" 12 1.9561e-01 7.4568e-02 1.7508e-05 1.2087e-02 6.14879e-02 0.0312 1.2087e-02 1\n",
" 13 1.9556e-01 7.4568e-02 1.6907e-05 1.2082e-02 5.47642e-02 0.0312 1.2082e-02 1\n",
" 14 1.9538e-01 7.4568e-02 1.6334e-05 1.2065e-02 4.87817e-02 0.0312 1.2065e-02 1\n",
" 15 1.9451e-01 7.4568e-02 1.5786e-05 1.1978e-02 4.34554e-02 0.0312 1.1978e-02 1\n",
" 16 1.9346e-01 7.4568e-02 1.5262e-05 1.1874e-02 3.87135e-02 0.0625 1.1874e-02 1\n",
" 17 1.9263e-01 7.4568e-02 1.4203e-05 1.1792e-02 3.02786e-02 0.0625 1.1792e-02 1\n",
" 18 1.9037e-01 7.4568e-02 1.3242e-05 1.1567e-02 2.37083e-02 0.1250 1.1567e-02 1\n",
" 19 1.8667e-01 7.4568e-02 1.1384e-05 1.1199e-02 1.35619e-02 0.5000 1.1199e-02 1\n",
" 20 1.7770e-01 7.4568e-02 4.2339e-06 1.0309e-02 1.04494e-02 0.5000 1.0309e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 1.6206e-01 7.4568e-02 2.3283e-06 8.7469e-03 ---- ---- 8.7469e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6543e+01 7.9903e-02 3.4315e+00 2.1482e-01 5.26852e-01 1.0000 ---- 3\n",
" 2 3.6369e+01 7.4993e-02 5.3351e-02 3.5761e+00 1.89874e-01 1.0000 3.5761e+00 3\n",
" 3 1.4284e+01 7.5011e-02 9.7709e-04 1.4200e+00 4.39961e-01 0.1250 1.4200e+00 3\n",
" 4 1.3991e+01 7.5003e-02 8.7223e-04 1.3908e+00 1.89256e-01 0.5000 1.3908e+00 3\n",
" 5 1.1591e+01 7.5004e-02 4.8850e-04 1.1511e+00 2.37242e-01 0.2500 1.1511e+00 3\n",
" 6 1.0473e+01 7.5002e-02 3.8003e-04 1.0394e+00 5.63449e-02 1.0000 1.0394e+00 3\n",
" 7 5.8520e-01 7.5012e-02 9.3602e-06 5.1009e-02 2.20928e-01 0.0156 5.1009e-02 3\n",
" 8 5.7996e-01 7.5012e-02 9.2381e-06 5.0486e-02 2.03258e-01 0.0312 5.0486e-02 3\n",
" 9 5.7838e-01 7.5011e-02 9.0326e-06 5.0328e-02 1.70681e-01 0.0312 5.0328e-02 3\n",
" 10 5.7012e-01 7.5011e-02 8.8118e-06 4.9502e-02 1.43312e-01 0.0625 4.9502e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 5.6538e-01 7.5011e-02 8.4409e-06 4.9028e-02 9.73651e-02 0.1250 4.9028e-02 3\n",
" 12 5.4552e-01 7.5010e-02 7.7718e-06 4.7043e-02 3.52350e-02 1.0000 4.7043e-02 3\n",
" 13 3.8593e-01 7.5011e-02 5.3115e-06 3.1087e-02 1.40236e-01 0.0312 3.1087e-02 3\n",
" 14 3.8200e-01 7.5010e-02 5.2156e-06 3.0694e-02 1.17757e-01 0.0625 3.0694e-02 3\n",
" 15 3.8125e-01 7.5010e-02 5.0935e-06 3.0619e-02 8.00049e-02 0.1250 3.0619e-02 3\n",
" 16 3.7470e-01 7.5010e-02 4.8535e-06 2.9964e-02 2.89335e-02 1.0000 2.9964e-02 3\n",
" 17 3.1415e-01 7.5010e-02 3.8545e-06 2.3910e-02 1.16278e-01 0.0020 2.3910e-02 1\n",
" 18 3.1415e-01 7.5010e-02 3.8545e-06 2.3910e-02 1.16269e-01 0.0312 2.3910e-02 1\n",
" 19 3.1293e-01 7.5010e-02 3.8893e-06 2.3788e-02 9.77601e-02 0.0156 2.3788e-02 1\n",
" 20 3.1265e-01 7.5010e-02 3.8586e-06 2.3760e-02 9.00109e-02 0.0312 2.3760e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.1072e-01 7.5010e-02 3.8446e-06 2.3567e-02 7.57275e-02 0.0312 2.3567e-02 1\n",
" 22 3.0938e-01 7.5010e-02 3.8073e-06 2.3433e-02 6.37909e-02 0.0625 2.3433e-02 1\n",
" 23 3.0681e-01 7.5010e-02 3.8349e-06 2.3176e-02 4.37707e-02 0.1250 2.3176e-02 1\n",
" 24 3.0267e-01 7.5010e-02 3.9851e-06 2.2762e-02 1.69774e-02 1.0000 2.2762e-02 1\n",
" 25 2.5809e-01 7.5010e-02 1.5231e-05 1.8292e-02 6.42709e-02 0.0625 1.8292e-02 1\n",
" 26 2.5653e-01 7.5010e-02 1.4340e-05 1.8138e-02 4.39596e-02 0.0625 1.8138e-02 1\n",
" 27 2.5348e-01 7.5010e-02 1.3470e-05 1.7833e-02 3.01095e-02 0.2500 1.7833e-02 1\n",
" 28 2.4961e-01 7.5010e-02 1.0285e-05 1.7449e-02 8.20864e-03 1.0000 1.7449e-02 1\n",
" 29 1.8464e-01 7.5010e-02 4.0668e-07 1.0962e-02 3.32787e-02 0.1250 1.0962e-02 1\n",
" 30 1.8256e-01 7.5010e-02 4.3354e-07 1.0755e-02 1.16874e-02 0.5000 1.0755e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.8216e-01 7.5010e-02 3.8447e-07 1.0715e-02 1.83730e-02 0.2500 1.0715e-02 1\n",
" 32 1.7609e-01 7.5010e-02 3.8799e-07 1.0108e-02 5.53679e-03 1.0000 1.0108e-02 1\n",
" END 1.4475e-01 7.5010e-02 1.2314e-07 6.9743e-03 ---- ---- 6.9743e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6199e+01 8.0353e-02 3.4586e+00 1.5329e-01 5.35612e-01 1.0000 ---- 3\n",
" 2 3.1859e+01 7.5458e-02 5.5118e-02 3.1232e+00 2.07371e-01 1.0000 3.1232e+00 3\n",
" 3 2.8370e+01 7.5470e-02 9.4259e-04 2.8285e+00 4.80222e-01 0.1250 2.8285e+00 3\n",
" 4 2.6695e+01 7.5458e-02 8.4070e-04 2.6611e+00 2.12618e-01 0.5000 2.6611e+00 3\n",
" 5 2.1900e+01 7.5459e-02 4.7593e-04 2.1820e+00 2.59125e-01 0.2500 2.1820e+00 3\n",
" 6 1.8419e+01 7.5457e-02 3.6655e-04 1.8340e+00 7.92770e-02 1.0000 1.8340e+00 3\n",
" 7 3.0152e+00 7.5475e-02 1.8401e-05 2.9396e-01 2.93771e-01 0.0312 2.9396e-01 3\n",
" 8 2.9793e+00 7.5473e-02 1.7922e-05 2.9036e-01 2.43578e-01 0.0312 2.9036e-01 3\n",
" 9 2.9106e+00 7.5473e-02 1.7423e-05 2.8350e-01 2.01874e-01 0.0625 2.8350e-01 3\n",
" 10 2.8337e+00 7.5472e-02 1.6488e-05 2.7581e-01 1.32499e-01 0.1250 2.7581e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.6288e+00 7.5471e-02 1.4646e-05 2.5531e-01 4.25873e-02 1.0000 2.5531e-01 3\n",
" 12 5.5735e-01 7.5474e-02 5.9695e-06 4.8181e-02 1.74157e-01 0.0156 4.8181e-02 3\n",
" 13 5.5504e-01 7.5474e-02 5.8851e-06 4.7951e-02 1.59481e-01 0.0156 4.7951e-02 3\n",
" 14 5.5127e-01 7.5474e-02 5.8011e-06 4.7574e-02 1.45994e-01 0.0156 4.7574e-02 3\n",
" 15 5.4631e-01 7.5473e-02 5.7174e-06 4.7078e-02 1.33619e-01 0.0312 4.7078e-02 3\n",
" 16 5.4449e-01 7.5473e-02 5.5630e-06 4.6896e-02 1.10868e-01 0.0312 4.6896e-02 3\n",
" 17 5.3611e-01 7.5473e-02 5.4073e-06 4.6059e-02 9.19792e-02 0.0625 4.6059e-02 3\n",
" 18 5.2910e-01 7.5473e-02 5.1234e-06 4.5358e-02 6.06631e-02 0.1250 4.5358e-02 3\n",
" 19 5.0285e-01 7.5473e-02 4.5934e-06 4.2733e-02 1.99185e-02 1.0000 4.2733e-02 3\n",
" 20 1.8355e-01 7.5473e-02 1.3915e-06 1.0806e-02 8.32622e-02 0.0020 1.0806e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.8355e-01 7.5473e-02 1.3915e-06 1.0806e-02 8.32366e-02 0.0156 1.0806e-02 1\n",
" 22 1.8311e-01 7.5473e-02 1.3746e-06 1.0762e-02 7.62573e-02 0.0078 1.0762e-02 1\n",
" 23 1.8307e-01 7.5473e-02 1.3650e-06 1.0758e-02 7.27647e-02 0.0156 1.0758e-02 1\n",
" 24 1.8276e-01 7.5473e-02 1.3477e-06 1.0728e-02 6.67154e-02 0.0156 1.0728e-02 1\n",
" 25 1.8270e-01 7.5473e-02 1.3304e-06 1.0722e-02 6.11939e-02 0.0156 1.0722e-02 1\n",
" 26 1.8225e-01 7.5473e-02 1.3132e-06 1.0677e-02 5.61524e-02 0.0156 1.0677e-02 1\n",
" 27 1.8184e-01 7.5473e-02 1.2960e-06 1.0635e-02 5.15576e-02 0.0312 1.0635e-02 1\n",
" 28 1.8169e-01 7.5473e-02 1.2673e-06 1.0620e-02 4.31803e-02 0.0312 1.0620e-02 1\n",
" 29 1.8131e-01 7.5473e-02 1.2381e-06 1.0583e-02 3.62809e-02 0.0312 1.0583e-02 1\n",
" 30 1.8033e-01 7.5473e-02 1.2076e-06 1.0485e-02 3.06040e-02 0.0625 1.0485e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.7926e-01 7.5473e-02 1.1623e-06 1.0378e-02 2.11912e-02 0.1250 1.0378e-02 1\n",
" 32 1.7609e-01 7.5473e-02 1.1327e-06 1.0061e-02 9.13250e-03 1.0000 1.0061e-02 1\n",
" END 1.3915e-01 7.5473e-02 6.6281e-06 6.3611e-03 ---- ---- 6.3611e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6398e+01 8.0825e-02 3.4871e+00 1.4458e-01 5.47219e-01 1.0000 ---- 3\n",
" 2 3.4180e+01 7.5950e-02 5.7023e-02 3.3534e+00 2.81495e-01 0.5000 3.3534e+00 3\n",
" 3 1.6418e+01 7.5950e-02 2.8560e-02 1.6057e+00 3.42832e-01 0.2500 1.6057e+00 3\n",
" 4 1.4766e+01 7.5945e-02 2.1418e-02 1.4476e+00 1.12641e-01 1.0000 1.4476e+00 3\n",
" 5 3.7576e+00 7.5957e-02 3.0876e-04 3.6785e-01 4.46606e-01 0.0312 3.6785e-01 3\n",
" 6 3.7320e+00 7.5955e-02 3.0019e-04 3.6530e-01 3.73462e-01 0.0312 3.6530e-01 3\n",
" 7 3.6589e+00 7.5954e-02 2.9155e-04 3.5801e-01 3.11526e-01 0.0625 3.5801e-01 3\n",
" 8 3.5993e+00 7.5952e-02 2.7539e-04 3.5206e-01 2.06933e-01 0.1250 3.5206e-01 3\n",
" 9 3.4192e+00 7.5951e-02 2.4464e-04 3.3408e-01 6.75564e-02 1.0000 3.3408e-01 3\n",
" 10 1.6130e+00 7.5954e-02 2.8473e-05 1.5368e-01 2.88571e-01 0.0312 1.5368e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.5928e+00 7.5953e-02 2.8025e-05 1.5165e-01 2.40492e-01 0.0312 1.5165e-01 3\n",
" 12 1.5565e+00 7.5953e-02 2.7471e-05 1.4803e-01 2.00304e-01 0.0625 1.4803e-01 3\n",
" 13 1.5173e+00 7.5952e-02 2.6679e-05 1.4411e-01 1.33100e-01 0.1250 1.4411e-01 3\n",
" 14 1.4202e+00 7.5951e-02 2.5116e-05 1.3440e-01 4.40040e-02 1.0000 1.3440e-01 3\n",
" 15 5.7391e-01 7.5953e-02 1.5773e-05 4.9780e-02 1.87742e-01 0.0312 4.9780e-02 3\n",
" 16 5.7301e-01 7.5952e-02 1.5528e-05 4.9690e-02 1.56497e-01 0.0312 4.9690e-02 3\n",
" 17 5.6540e-01 7.5952e-02 1.5219e-05 4.8930e-02 1.30426e-01 0.0625 4.8930e-02 3\n",
" 18 5.6353e-01 7.5952e-02 1.4769e-05 4.8743e-02 8.69993e-02 0.1250 4.8743e-02 3\n",
" 19 5.5240e-01 7.5951e-02 1.3868e-05 4.7631e-02 2.93852e-02 1.0000 4.7631e-02 3\n",
" 20 4.0536e-01 7.5952e-02 8.7747e-06 3.2932e-02 1.22831e-01 0.0625 3.2932e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.0354e-01 7.5951e-02 8.7101e-06 3.2751e-02 8.23706e-02 0.1250 3.2751e-02 2\n",
" 22 3.8904e-01 7.5951e-02 8.5579e-06 3.1300e-02 2.84911e-02 1.0000 3.1300e-02 2\n",
" 23 2.4775e-01 7.5951e-02 8.9918e-06 1.7171e-02 1.18505e-01 0.0312 1.7171e-02 2\n",
" 24 2.4492e-01 7.5951e-02 8.8418e-06 1.6888e-02 9.92915e-02 0.0312 1.6888e-02 2\n",
" 25 2.3975e-01 7.5951e-02 8.6596e-06 1.6371e-02 8.31913e-02 0.0625 1.6371e-02 2\n",
" 26 2.3474e-01 7.5951e-02 8.3886e-06 1.5870e-02 5.62165e-02 0.1250 1.5870e-02 2\n",
" 27 2.2275e-01 7.5951e-02 7.8636e-06 1.4672e-02 1.99576e-02 1.0000 1.4672e-02 2\n",
" END 1.6485e-01 7.5951e-02 5.2507e-06 8.8849e-03 ---- ---- 8.8849e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7012e+01 8.1308e-02 3.5216e+00 1.7148e-01 5.68887e-01 0.5000 ---- 3\n",
" 2 2.7708e+01 7.6927e-02 1.7726e+00 9.9044e-01 3.30824e-01 1.0000 9.9044e-01 3\n",
" 3 1.1725e+01 7.6471e-02 1.4327e-02 1.1505e+00 4.25457e-01 0.0625 1.1505e+00 3\n",
" 4 1.1329e+01 7.6467e-02 1.3424e-02 1.1119e+00 2.96153e-01 0.1250 1.1119e+00 3\n",
" 5 1.0463e+01 7.6464e-02 1.1728e-02 1.0269e+00 1.13728e-01 1.0000 1.0269e+00 3\n",
" 6 6.1288e+00 7.6472e-02 5.1620e-04 6.0472e-01 4.23960e-01 0.0312 6.0472e-01 3\n",
" 7 6.0097e+00 7.6470e-02 5.0168e-04 5.9282e-01 3.57475e-01 0.0625 5.9282e-01 3\n",
" 8 5.9421e+00 7.6467e-02 4.7494e-04 5.8609e-01 2.44782e-01 0.1250 5.8609e-01 3\n",
" 9 5.7792e+00 7.6465e-02 4.2441e-04 5.6985e-01 8.95100e-02 0.5000 5.6985e-01 3\n",
" 10 3.5775e+00 7.6467e-02 2.3175e-04 3.4987e-01 1.35906e-01 0.1250 3.4987e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.2351e+00 7.6467e-02 2.0563e-04 3.1565e-01 4.86803e-02 1.0000 3.1565e-01 3\n",
" 12 1.0483e+00 7.6470e-02 3.3518e-05 9.7150e-02 1.98575e-01 0.0312 9.7150e-02 3\n",
" 13 1.0455e+00 7.6470e-02 3.2890e-05 9.6873e-02 1.66597e-01 0.0312 9.6873e-02 3\n",
" 14 1.0305e+00 7.6469e-02 3.2160e-05 9.5375e-02 1.39689e-01 0.0625 9.5375e-02 3\n",
" 15 1.0282e+00 7.6469e-02 3.0997e-05 9.5143e-02 9.47274e-02 0.1250 9.5143e-02 3\n",
" 16 1.0124e+00 7.6468e-02 2.8715e-05 9.3560e-02 3.38439e-02 0.5000 9.3560e-02 3\n",
" 17 6.5201e-01 7.6469e-02 1.7910e-05 5.7536e-02 5.25426e-02 0.1250 5.7536e-02 3\n",
" 18 6.0074e-01 7.6469e-02 1.6154e-05 5.2411e-02 1.86689e-02 1.0000 5.2411e-02 3\n",
" 19 2.8125e-01 7.6469e-02 4.7917e-06 2.0473e-02 7.84575e-02 0.0020 2.0473e-02 1\n",
" 20 2.8125e-01 7.6469e-02 4.7917e-06 2.0473e-02 7.83946e-02 0.0156 2.0473e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.7972e-01 7.6469e-02 4.7001e-06 2.0320e-02 7.23862e-02 0.0078 2.0320e-02 1\n",
" 22 2.7949e-01 7.6469e-02 4.6593e-06 2.0297e-02 6.92972e-02 0.0156 2.0297e-02 1\n",
" 23 2.7842e-01 7.6469e-02 4.5708e-06 2.0190e-02 6.40279e-02 0.0156 2.0190e-02 1\n",
" 24 2.7788e-01 7.6469e-02 4.4842e-06 2.0137e-02 5.91731e-02 0.0312 2.0137e-02 1\n",
" 25 2.7758e-01 7.6469e-02 4.2860e-06 2.0107e-02 5.02483e-02 0.0312 2.0107e-02 1\n",
" 26 2.7665e-01 7.6469e-02 4.1001e-06 2.0014e-02 4.27465e-02 0.0625 2.0014e-02 1\n",
" 27 2.7651e-01 7.6469e-02 3.6603e-06 2.0000e-02 3.01775e-02 0.1250 2.0000e-02 1\n",
" 28 2.7602e-01 7.6469e-02 2.6662e-06 1.9953e-02 1.30796e-02 0.5000 1.9953e-02 1\n",
" 29 2.4517e-01 7.6469e-02 2.4041e-06 1.6868e-02 1.51250e-02 0.2500 1.6868e-02 1\n",
" 30 2.3272e-01 7.6469e-02 9.7761e-07 1.5624e-02 5.20839e-03 1.0000 1.5624e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 1.6215e-01 7.6469e-02 1.7145e-06 8.5665e-03 ---- ---- 8.5665e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7483e+01 8.1819e-02 3.5272e+00 2.1290e-01 5.36045e-01 0.5000 ---- 3\n",
" 2 2.8735e+01 7.6828e-02 1.7947e+00 1.0712e+00 2.92072e-01 1.0000 1.0712e+00 3\n",
" 3 5.6427e+00 7.5753e-02 3.6065e-02 5.2063e-01 2.49431e-01 0.1250 5.2063e-01 3\n",
" 4 5.3034e+00 7.5750e-02 3.1552e-02 4.9121e-01 1.03900e-01 0.5000 4.9121e-01 3\n",
" 5 3.2976e+00 7.5749e-02 1.5751e-02 3.0644e-01 1.28331e-01 0.2500 3.0644e-01 3\n",
" 6 2.9124e+00 7.5748e-02 1.1818e-02 2.7184e-01 3.73823e-02 1.0000 2.7184e-01 3\n",
" 7 5.9887e-01 7.5749e-02 2.6770e-05 5.2285e-02 1.43176e-01 0.0312 5.2285e-02 3\n",
" 8 5.9780e-01 7.5749e-02 2.5908e-05 5.2179e-02 1.18722e-01 0.0312 5.2179e-02 3\n",
" 9 5.8951e-01 7.5749e-02 2.5078e-05 5.1351e-02 9.84413e-02 0.0625 5.1351e-02 3\n",
" 10 5.8468e-01 7.5749e-02 2.3450e-05 5.0870e-02 6.48490e-02 0.1250 5.0870e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 5.2618e-01 7.5749e-02 2.0382e-05 4.5023e-02 2.09077e-02 1.0000 4.5023e-02 2\n",
" END 1.7263e-01 7.5749e-02 2.3298e-06 9.6857e-03 ---- ---- 9.6857e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7214e+01 8.1083e-02 3.4997e+00 2.1357e-01 5.71893e-01 0.5000 ---- 3\n",
" 2 3.0266e+01 7.6212e-02 1.7768e+00 1.2422e+00 4.06208e-01 0.5000 1.2422e+00 3\n",
" 3 2.1737e+01 7.5242e-02 8.9498e-01 1.2711e+00 3.93920e-01 0.2500 1.2711e+00 3\n",
" 4 2.0667e+01 7.5147e-02 6.7153e-01 1.3877e+00 1.93486e-01 1.0000 1.3877e+00 3\n",
" 5 8.4193e+00 7.5256e-02 4.8565e-03 8.2954e-01 5.00159e-01 0.0312 8.2954e-01 3\n",
" 6 8.3027e+00 7.5253e-02 4.7050e-03 8.1804e-01 4.15055e-01 0.0312 8.1804e-01 3\n",
" 7 8.0988e+00 7.5251e-02 4.5581e-03 7.9780e-01 3.43189e-01 0.0625 7.9780e-01 3\n",
" 8 7.8614e+00 7.5248e-02 4.2734e-03 7.7435e-01 2.22683e-01 0.1250 7.7435e-01 3\n",
" 9 7.2584e+00 7.5247e-02 3.7387e-03 7.1458e-01 6.57577e-02 1.0000 7.1458e-01 3\n",
" 10 1.3479e+00 7.5255e-02 3.1291e-05 1.2724e-01 2.91611e-01 0.0156 1.2724e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.3418e+00 7.5254e-02 3.0787e-05 1.2662e-01 2.66805e-01 0.0156 1.2662e-01 3\n",
" 12 1.3313e+00 7.5254e-02 3.0292e-05 1.2557e-01 2.43925e-01 0.0156 1.2557e-01 3\n",
" 13 1.3174e+00 7.5253e-02 2.9805e-05 1.2419e-01 2.22872e-01 0.0312 1.2419e-01 3\n",
" 14 1.3120e+00 7.5253e-02 2.8826e-05 1.2365e-01 1.84050e-01 0.0312 1.2365e-01 3\n",
" 15 1.2882e+00 7.5252e-02 2.7888e-05 1.2127e-01 1.51781e-01 0.0625 1.2127e-01 3\n",
" 16 1.2668e+00 7.5252e-02 2.6033e-05 1.1913e-01 9.82396e-02 0.1250 1.1913e-01 3\n",
" 17 1.1886e+00 7.5252e-02 2.2553e-05 1.1131e-01 2.93276e-02 1.0000 1.1131e-01 3\n",
" 18 3.8747e-01 7.5253e-02 2.5146e-06 3.1219e-02 1.31075e-01 0.0312 3.1219e-02 2\n",
" 19 3.8195e-01 7.5252e-02 2.4699e-06 3.0667e-02 1.07883e-01 0.0625 3.0667e-02 2\n",
" 20 3.8065e-01 7.5252e-02 2.4053e-06 3.0538e-02 6.98185e-02 0.1250 3.0538e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.6404e-01 7.5252e-02 2.2497e-06 2.8877e-02 2.11977e-02 1.0000 2.8877e-02 2\n",
" END 1.3356e-01 7.5252e-02 8.9476e-07 5.8300e-03 ---- ---- 6.6125e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6966e+01 8.0575e-02 3.4780e+00 2.1059e-01 5.77466e-01 0.5000 ---- 3\n",
" 2 3.0174e+01 7.5785e-02 1.7628e+00 1.2470e+00 4.00334e-01 0.5000 1.2470e+00 3\n",
" 3 2.1944e+01 7.4857e-02 8.8705e-01 1.2998e+00 4.08787e-01 0.1250 1.2998e+00 3\n",
" 4 2.0191e+01 7.4806e-02 7.7623e-01 1.2354e+00 1.92136e-01 1.0000 1.2354e+00 3\n",
" 5 5.5225e+00 7.4912e-02 5.5263e-03 5.3923e-01 4.47841e-01 0.0312 5.3923e-01 3\n",
" 6 5.4752e+00 7.4910e-02 5.3534e-03 5.3468e-01 3.72867e-01 0.0312 5.3468e-01 3\n",
" 7 5.3594e+00 7.4909e-02 5.1860e-03 5.2327e-01 3.09682e-01 0.0625 5.2327e-01 3\n",
" 8 5.2468e+00 7.4907e-02 4.8613e-03 5.1232e-01 2.03684e-01 0.1250 5.1232e-01 3\n",
" 9 4.9063e+00 7.4906e-02 4.2520e-03 4.7889e-01 6.44723e-02 1.0000 4.7889e-01 3\n",
" 10 1.1650e+00 7.4910e-02 4.8942e-05 1.0896e-01 2.73486e-01 0.0156 1.0896e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.1551e+00 7.4910e-02 4.8201e-05 1.0797e-01 2.50284e-01 0.0156 1.0797e-01 3\n",
" 12 1.1427e+00 7.4910e-02 4.7467e-05 1.0673e-01 2.28993e-01 0.0312 1.0673e-01 3\n",
" 13 1.1360e+00 7.4909e-02 4.6047e-05 1.0606e-01 1.89876e-01 0.0312 1.0606e-01 3\n",
" 14 1.1150e+00 7.4909e-02 4.4649e-05 1.0396e-01 1.57368e-01 0.0625 1.0396e-01 3\n",
" 15 1.0971e+00 7.4908e-02 4.1969e-05 1.0218e-01 1.03358e-01 0.1250 1.0218e-01 3\n",
" 16 1.0479e+00 7.4908e-02 3.6904e-05 9.7262e-02 3.28230e-02 1.0000 9.7262e-02 3\n",
" 17 4.9386e-01 7.4909e-02 1.5710e-06 4.1893e-02 1.40711e-01 0.0312 4.1893e-02 3\n",
" 18 4.8663e-01 7.4909e-02 1.5459e-06 4.1171e-02 1.16773e-01 0.0625 4.1171e-02 3\n",
" 19 4.8134e-01 7.4909e-02 1.5187e-06 4.0642e-02 7.70726e-02 0.1250 4.0642e-02 3\n",
" 20 4.6688e-01 7.4908e-02 1.4599e-06 3.9196e-02 2.50019e-02 1.0000 3.9196e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.0874e-01 7.4909e-02 1.2242e-06 3.3382e-02 1.06612e-01 0.0625 3.3382e-02 2\n",
" 22 4.0400e-01 7.4908e-02 1.2198e-06 3.2908e-02 7.20291e-02 0.0020 3.2908e-02 1\n",
" 23 4.0400e-01 7.4908e-02 1.2198e-06 3.2908e-02 7.20309e-02 0.0625 3.2908e-02 1\n",
" 24 3.9780e-01 7.4908e-02 9.5029e-07 3.2288e-02 4.85104e-02 0.0625 3.2288e-02 1\n",
" 25 3.9770e-01 7.4908e-02 7.7558e-07 3.2278e-02 3.31158e-02 0.2500 3.2278e-02 1\n",
" 26 3.8220e-01 7.4908e-02 1.1236e-06 3.0728e-02 1.23938e-02 1.0000 3.0728e-02 1\n",
" 27 3.2811e-01 7.4908e-02 6.1800e-06 2.5314e-02 4.68452e-02 0.0625 2.5314e-02 1\n",
" 28 3.2514e-01 7.4908e-02 5.7984e-06 2.5017e-02 3.04241e-02 0.1250 2.5017e-02 1\n",
" 29 3.2507e-01 7.4908e-02 5.0811e-06 2.5012e-02 9.22701e-03 1.0000 2.5012e-02 1\n",
" 30 2.3757e-01 7.4909e-02 1.4530e-07 1.6266e-02 4.10126e-02 0.0625 1.6266e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.3555e-01 7.4908e-02 1.4289e-07 1.6064e-02 2.67172e-02 0.0625 1.6064e-02 1\n",
" 32 2.3385e-01 7.4908e-02 1.3708e-07 1.5894e-02 1.74321e-02 0.2500 1.5894e-02 1\n",
" 33 2.2465e-01 7.4908e-02 1.2727e-07 1.4974e-02 6.79853e-03 1.0000 1.4974e-02 1\n",
" 34 1.9209e-01 7.4909e-02 8.6876e-08 1.1718e-02 3.19172e-02 0.0625 1.1718e-02 1\n",
" 35 1.9120e-01 7.4909e-02 8.8574e-08 1.1629e-02 2.04892e-02 0.0625 1.1629e-02 1\n",
" 36 1.8954e-01 7.4909e-02 8.5999e-08 1.1463e-02 1.31645e-02 0.2500 1.1463e-02 1\n",
" 37 1.8291e-01 7.4909e-02 8.4305e-08 1.0800e-02 5.66788e-03 1.0000 1.0800e-02 1\n",
" END 1.6029e-01 7.4909e-02 5.5912e-08 8.5383e-03 ---- ---- 8.5383e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6891e+01 8.0222e-02 3.4608e+00 2.2027e-01 5.26959e-01 0.5000 ---- 3\n",
" 2 2.7525e+01 7.5489e-02 1.7515e+00 9.9344e-01 3.09742e-01 1.0000 9.9344e-01 3\n",
" 3 8.4962e+00 7.4672e-02 2.4965e-02 8.1719e-01 3.88048e-01 0.0625 8.1719e-01 3\n",
" 4 8.1422e+00 7.4670e-02 2.3404e-02 7.8335e-01 2.61665e-01 0.1250 7.8335e-01 3\n",
" 5 7.4329e+00 7.4668e-02 2.0475e-02 7.1535e-01 9.31557e-02 1.0000 7.1535e-01 3\n",
" 6 3.2933e+00 7.4673e-02 1.4310e-04 3.2172e-01 3.42401e-01 0.0312 3.2172e-01 3\n",
" 7 3.2386e+00 7.4672e-02 1.3846e-04 3.1626e-01 2.87132e-01 0.0625 3.1626e-01 3\n",
" 8 3.2120e+00 7.4670e-02 1.2932e-04 3.1361e-01 1.93399e-01 0.1250 3.1361e-01 3\n",
" 9 3.1028e+00 7.4669e-02 1.1226e-04 3.0270e-01 6.71143e-02 1.0000 3.0270e-01 3\n",
" 10 2.7496e+00 7.4671e-02 1.1875e-05 2.6748e-01 2.69750e-01 0.0312 2.6748e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.6860e+00 7.4671e-02 1.1639e-05 2.6112e-01 2.25869e-01 0.0625 2.6112e-01 3\n",
" 12 2.6213e+00 7.4670e-02 1.1296e-05 2.5465e-01 1.52630e-01 0.1250 2.5465e-01 3\n",
" 13 2.4949e+00 7.4669e-02 1.0620e-05 2.4202e-01 5.43328e-02 0.5000 2.4202e-01 3\n",
" 14 1.5546e+00 7.4670e-02 7.0752e-06 1.4798e-01 8.16591e-02 0.2500 1.4798e-01 3\n",
" 15 1.4274e+00 7.4669e-02 6.2243e-06 1.3526e-01 2.44807e-02 1.0000 1.3526e-01 3\n",
" 16 4.6440e-01 7.4671e-02 1.8633e-06 3.8971e-02 1.11140e-01 0.0312 3.8971e-02 3\n",
" 17 4.6014e-01 7.4671e-02 1.8438e-06 3.8545e-02 9.16648e-02 0.0312 3.8545e-02 3\n",
" 18 4.5174e-01 7.4670e-02 1.8133e-06 3.7705e-02 7.56020e-02 0.0625 3.7705e-02 3\n",
" 19 4.4126e-01 7.4670e-02 1.7755e-06 3.6657e-02 4.90926e-02 0.1250 3.6657e-02 3\n",
" 20 4.1301e-01 7.4670e-02 1.6880e-06 3.3832e-02 1.49088e-02 1.0000 3.3832e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 8.5192e-02 7.4671e-02 1.0591e-06 1.0510e-03 ---- ---- 1.3467e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6670e+01 7.9978e-02 3.4508e+00 2.0817e-01 5.41619e-01 0.5000 ---- 3\n",
" 2 2.8915e+01 7.5283e-02 1.7445e+00 1.1395e+00 3.73341e-01 1.0000 1.1395e+00 3\n",
" 3 2.3163e+01 7.4506e-02 2.3507e-02 2.2853e+00 6.78114e-01 0.0625 2.2853e+00 3\n",
" 4 2.2430e+01 7.4498e-02 2.2040e-02 2.2135e+00 4.61684e-01 0.1250 2.2135e+00 3\n",
" 5 2.0903e+01 7.4493e-02 1.9285e-02 2.0636e+00 1.60184e-01 1.0000 2.0636e+00 3\n",
" 6 1.1655e+01 7.4512e-02 1.9284e-04 1.1578e+00 6.17101e-01 0.0312 1.1578e+00 3\n",
" 7 1.1518e+01 7.4508e-02 1.8612e-04 1.1441e+00 5.28190e-01 0.0312 1.1441e+00 3\n",
" 8 1.1249e+01 7.4505e-02 1.7971e-04 1.1172e+00 4.48297e-01 0.0625 1.1172e+00 3\n",
" 9 1.0951e+01 7.4501e-02 1.6657e-04 1.0875e+00 3.05932e-01 0.1250 1.0875e+00 3\n",
" 10 1.0157e+01 7.4499e-02 1.4152e-04 1.0081e+00 1.06204e-01 1.0000 1.0081e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.7157e+00 7.4509e-02 4.7152e-05 3.6407e-01 4.31174e-01 0.0312 3.6407e-01 3\n",
" 12 3.6742e+00 7.4507e-02 4.6478e-05 3.5992e-01 3.59722e-01 0.0312 3.5992e-01 3\n",
" 13 3.5983e+00 7.4506e-02 4.5562e-05 3.5234e-01 2.99789e-01 0.0625 3.5234e-01 3\n",
" 14 3.5461e+00 7.4504e-02 4.4152e-05 3.4712e-01 1.99481e-01 0.1250 3.4712e-01 3\n",
" 15 3.4030e+00 7.4503e-02 4.0989e-05 3.3281e-01 6.62521e-02 1.0000 3.3281e-01 3\n",
" 16 3.0124e+00 7.4506e-02 1.3051e-05 2.9377e-01 2.72288e-01 0.0312 2.9377e-01 3\n",
" 17 2.9412e+00 7.4505e-02 1.2858e-05 2.8665e-01 2.28244e-01 0.0625 2.8665e-01 3\n",
" 18 2.8631e+00 7.4504e-02 1.2672e-05 2.7885e-01 1.53844e-01 0.1250 2.7885e-01 3\n",
" 19 2.6867e+00 7.4504e-02 1.2250e-05 2.6121e-01 5.36393e-02 1.0000 2.6121e-01 3\n",
" 20 2.0272e+00 7.4506e-02 9.5490e-06 1.9526e-01 2.15690e-01 0.0625 1.9526e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.0134e+00 7.4505e-02 9.4643e-06 1.9388e-01 1.45911e-01 0.1250 1.9388e-01 3\n",
" 22 1.9885e+00 7.4504e-02 9.2331e-06 1.9139e-01 5.22503e-02 0.5000 1.9139e-01 3\n",
" 23 1.4626e+00 7.4505e-02 6.7884e-06 1.3880e-01 7.82374e-02 0.2500 1.3880e-01 3\n",
" 24 1.3649e+00 7.4504e-02 6.1683e-06 1.2903e-01 2.35352e-02 1.0000 1.2903e-01 3\n",
" 25 4.4114e-01 7.4506e-02 2.8900e-06 3.6661e-02 1.05795e-01 0.0312 3.6661e-02 3\n",
" 26 4.3768e-01 7.4506e-02 2.8523e-06 3.6315e-02 8.73507e-02 0.0312 3.6315e-02 3\n",
" 27 4.3037e-01 7.4505e-02 2.7997e-06 3.5584e-02 7.21240e-02 0.0625 3.5584e-02 3\n",
" 28 4.2202e-01 7.4505e-02 2.7263e-06 3.4749e-02 4.69661e-02 0.1250 3.4749e-02 3\n",
" 29 3.9906e-01 7.4505e-02 2.5638e-06 3.2453e-02 1.43497e-02 1.0000 3.2453e-02 3\n",
" 30 1.7552e-01 7.4506e-02 1.3017e-06 1.0100e-02 6.54174e-02 0.0020 1.0100e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.7552e-01 7.4506e-02 1.3017e-06 1.0100e-02 6.53775e-02 0.0312 1.0100e-02 1\n",
" END 1.7447e-01 7.4506e-02 1.2217e-06 9.9952e-03 ---- ---- 9.9952e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.6781e+01 7.9801e-02 3.4542e+00 2.1590e-01 5.51589e-01 0.5000 ---- 3\n",
" 2 2.9156e+01 7.5403e-02 1.7376e+00 1.1704e+00 3.72640e-01 1.0000 1.1704e+00 3\n",
" 3 1.8813e+01 7.4938e-02 1.2729e-02 1.8611e+00 6.40955e-01 0.0625 1.8611e+00 3\n",
" 4 1.8167e+01 7.4931e-02 1.1936e-02 1.7973e+00 4.33861e-01 0.1250 1.7973e+00 3\n",
" 5 1.6842e+01 7.4927e-02 1.0446e-02 1.6662e+00 1.50244e-01 1.0000 1.6662e+00 3\n",
" 6 9.3497e+00 7.4942e-02 1.9753e-04 9.2728e-01 5.86948e-01 0.0312 9.2728e-01 3\n",
" 7 9.2277e+00 7.4938e-02 1.9011e-04 9.1509e-01 4.97955e-01 0.0312 9.1509e-01 3\n",
" 8 9.0020e+00 7.4936e-02 1.8321e-04 8.9252e-01 4.20128e-01 0.0625 8.9252e-01 3\n",
" 9 8.7353e+00 7.4933e-02 1.6886e-04 8.6586e-01 2.84873e-01 0.1250 8.6586e-01 3\n",
" 10 8.0594e+00 7.4931e-02 1.4207e-04 7.9830e-01 9.83470e-02 1.0000 7.9830e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.2471e+00 7.4939e-02 5.2217e-05 3.1717e-01 4.00174e-01 0.0312 3.1717e-01 3\n",
" 12 3.2200e+00 7.4938e-02 5.1451e-05 3.1445e-01 3.34543e-01 0.0312 3.1445e-01 3\n",
" 13 3.1605e+00 7.4937e-02 5.0430e-05 3.0850e-01 2.79469e-01 0.0625 3.0850e-01 3\n",
" 14 3.1250e+00 7.4935e-02 4.8875e-05 3.0496e-01 1.87215e-01 0.1250 3.0496e-01 3\n",
" 15 3.0176e+00 7.4934e-02 4.5476e-05 2.9422e-01 6.39632e-02 1.0000 2.9422e-01 3\n",
" 16 2.6584e+00 7.4937e-02 1.6439e-05 2.5833e-01 2.60587e-01 0.0625 2.5833e-01 3\n",
" 17 2.6567e+00 7.4936e-02 1.6584e-05 2.5816e-01 1.76275e-01 0.1250 2.5816e-01 3\n",
" 18 2.6151e+00 7.4935e-02 1.6650e-05 2.5400e-01 6.22703e-02 0.5000 2.5400e-01 3\n",
" 19 1.7070e+00 7.4935e-02 1.2066e-05 1.6320e-01 9.41871e-02 0.2500 1.6320e-01 3\n",
" 20 1.6089e+00 7.4935e-02 1.1563e-05 1.5338e-01 2.90640e-02 1.0000 1.5338e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 5.2272e-01 7.4937e-02 5.7012e-06 4.4772e-02 1.31261e-01 0.0312 4.4772e-02 3\n",
" 22 5.1906e-01 7.4936e-02 5.6384e-06 4.4407e-02 1.08335e-01 0.0312 4.4407e-02 3\n",
" 23 5.1034e-01 7.4936e-02 5.5401e-06 4.3534e-02 8.94110e-02 0.0625 4.3534e-02 3\n",
" 24 5.0143e-01 7.4936e-02 5.4051e-06 4.2644e-02 5.82009e-02 0.1250 4.2644e-02 3\n",
" 25 4.7649e-01 7.4936e-02 5.0856e-06 4.0151e-02 1.77122e-02 1.0000 4.0151e-02 3\n",
" 26 2.0213e-01 7.4937e-02 2.1639e-06 1.2717e-02 8.04702e-02 0.0020 1.2717e-02 1\n",
" 27 2.0213e-01 7.4937e-02 2.1639e-06 1.2717e-02 8.04598e-02 0.0156 1.2717e-02 1\n",
" 28 2.0110e-01 7.4936e-02 2.1596e-06 1.2615e-02 7.34906e-02 0.0078 1.2615e-02 1\n",
" 29 2.0097e-01 7.4936e-02 2.1492e-06 1.2601e-02 7.00138e-02 0.0156 1.2601e-02 1\n",
" 30 2.0026e-01 7.4936e-02 2.1397e-06 1.2530e-02 6.39927e-02 0.0156 1.2530e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.9990e-01 7.4936e-02 2.1275e-06 1.2494e-02 5.85074e-02 0.0312 1.2494e-02 1\n",
" 32 1.9952e-01 7.4936e-02 2.1362e-06 1.2457e-02 4.84989e-02 0.0312 1.2457e-02 1\n",
" 33 1.9900e-01 7.4936e-02 2.1273e-06 1.2404e-02 4.02598e-02 0.0312 1.2404e-02 1\n",
" 34 1.9780e-01 7.4936e-02 2.1056e-06 1.2284e-02 3.34773e-02 0.0625 1.2284e-02 1\n",
" 35 1.9682e-01 7.4936e-02 2.1138e-06 1.2187e-02 2.23607e-02 0.1250 1.2187e-02 1\n",
" 36 1.9349e-01 7.4936e-02 2.1746e-06 1.1853e-02 8.39424e-03 1.0000 1.1853e-02 1\n",
" END 1.5690e-01 7.4936e-02 5.4229e-06 8.1906e-03 ---- ---- 8.1906e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7062e+01 8.0247e-02 3.4832e+00 2.1503e-01 5.49227e-01 0.5000 ---- 3\n",
" 2 2.8953e+01 7.5856e-02 1.7523e+00 1.1354e+00 3.48114e-01 1.0000 1.1354e+00 3\n",
" 3 1.0869e+01 7.5401e-02 1.2883e-02 1.0665e+00 5.11443e-01 0.0625 1.0665e+00 3\n",
" 4 1.0428e+01 7.5398e-02 1.2079e-02 1.0232e+00 3.44174e-01 0.1250 1.0232e+00 3\n",
" 5 9.5379e+00 7.5395e-02 1.0568e-02 9.3569e-01 1.20554e-01 1.0000 9.3569e-01 3\n",
" 6 5.1546e+00 7.5403e-02 1.7529e-04 5.0774e-01 4.65407e-01 0.0312 5.0774e-01 3\n",
" 7 5.0561e+00 7.5401e-02 1.6885e-04 4.9790e-01 3.90308e-01 0.0625 4.9790e-01 3\n",
" 8 4.9939e+00 7.5398e-02 1.5554e-04 4.9169e-01 2.62846e-01 0.1250 4.9169e-01 3\n",
" 9 4.7876e+00 7.5397e-02 1.3104e-04 4.7109e-01 9.06039e-02 1.0000 4.7109e-01 3\n",
" 10 4.2272e+00 7.5401e-02 4.0556e-05 4.1514e-01 3.67737e-01 0.0312 4.1514e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 4.1272e+00 7.5399e-02 3.9943e-05 4.0514e-01 3.08343e-01 0.0625 4.0514e-01 3\n",
" 12 4.0277e+00 7.5398e-02 3.9246e-05 3.9519e-01 2.08473e-01 0.1250 3.9519e-01 3\n",
" 13 3.8293e+00 7.5397e-02 3.7487e-05 3.7535e-01 7.36122e-02 1.0000 3.7535e-01 3\n",
" 14 3.6642e+00 7.5400e-02 2.1459e-05 3.5886e-01 2.94362e-01 0.0625 3.5886e-01 3\n",
" 15 3.6284e+00 7.5398e-02 2.1540e-05 3.5528e-01 2.00901e-01 0.1250 3.5528e-01 3\n",
" 16 3.5418e+00 7.5397e-02 2.1508e-05 3.4662e-01 7.33636e-02 0.5000 3.4662e-01 3\n",
" 17 2.4373e+00 7.5398e-02 1.5707e-05 2.3618e-01 1.07650e-01 0.2500 2.3618e-01 3\n",
" 18 2.2285e+00 7.5398e-02 1.4898e-05 2.1530e-01 3.04380e-02 1.0000 2.1530e-01 3\n",
" 19 4.4162e-01 7.5400e-02 6.3237e-06 3.6615e-02 1.36490e-01 0.0020 3.6615e-02 1\n",
" 20 4.4162e-01 7.5400e-02 6.3237e-06 3.6615e-02 1.36368e-01 0.0156 3.6615e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.3876e-01 7.5400e-02 6.2231e-06 3.6329e-02 1.24814e-01 0.0078 3.6329e-02 1\n",
" 22 4.3832e-01 7.5400e-02 6.1735e-06 3.6286e-02 1.18967e-01 0.0156 3.6286e-02 1\n",
" 23 4.3626e-01 7.5400e-02 6.0727e-06 3.6080e-02 1.08968e-01 0.0156 3.6080e-02 1\n",
" 24 4.3518e-01 7.5400e-02 5.9722e-06 3.5972e-02 9.98244e-02 0.0312 3.5972e-02 1\n",
" 25 4.3404e-01 7.5399e-02 5.7599e-06 3.5858e-02 8.31806e-02 0.0312 3.5858e-02 1\n",
" 26 4.3179e-01 7.5399e-02 5.5510e-06 3.5634e-02 6.94263e-02 0.0625 3.5634e-02 1\n",
" 27 4.2937e-01 7.5399e-02 5.0885e-06 3.5392e-02 4.67230e-02 0.1250 3.5392e-02 1\n",
" 28 4.2351e-01 7.5399e-02 4.0687e-06 3.4807e-02 1.71342e-02 1.0000 3.4807e-02 1\n",
" 29 3.2209e-01 7.5399e-02 1.0473e-05 2.4658e-02 6.10045e-02 0.0625 2.4658e-02 1\n",
" 30 3.1894e-01 7.5399e-02 9.7165e-06 2.4345e-02 4.01957e-02 0.0625 2.4345e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 3.1582e-01 7.5399e-02 9.0645e-06 2.4033e-02 2.65820e-02 0.2500 2.4033e-02 1\n",
" 32 3.0119e-01 7.5399e-02 6.4821e-06 2.2572e-02 9.58700e-03 1.0000 2.2572e-02 1\n",
" 33 2.4162e-01 7.5399e-02 6.3837e-07 1.6622e-02 4.37710e-02 0.0625 1.6622e-02 1\n",
" 34 2.3932e-01 7.5399e-02 6.5316e-07 1.6392e-02 2.83362e-02 0.0625 1.6392e-02 1\n",
" 35 2.3707e-01 7.5399e-02 6.3594e-07 1.6166e-02 1.83843e-02 0.2500 1.6166e-02 1\n",
" 36 2.2628e-01 7.5399e-02 6.4294e-07 1.5087e-02 7.53384e-03 1.0000 1.5087e-02 1\n",
" 37 1.8714e-01 7.5399e-02 3.7609e-07 1.1174e-02 3.51137e-02 0.0625 1.1174e-02 1\n",
" 38 1.8586e-01 7.5399e-02 3.8511e-07 1.1046e-02 2.25797e-02 0.0625 1.1046e-02 1\n",
" 39 1.8438e-01 7.5399e-02 3.7470e-07 1.0897e-02 1.45423e-02 0.2500 1.0897e-02 1\n",
" 40 1.7743e-01 7.5399e-02 3.7352e-07 1.0203e-02 6.17828e-03 1.0000 1.0203e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 1.5407e-01 7.5399e-02 2.5964e-07 7.8670e-03 ---- ---- 7.8670e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7334e+01 8.0724e-02 3.5158e+00 2.0953e-01 5.51766e-01 0.5000 ---- 3\n",
" 2 2.8856e+01 7.6344e-02 1.7689e+00 1.1091e+00 3.00145e-01 1.0000 1.1091e+00 3\n",
" 3 6.3155e+00 7.5901e-02 1.3019e-02 6.1094e-01 3.05435e-01 0.1250 6.1094e-01 3\n",
" 4 6.1558e+00 7.5898e-02 1.1389e-02 5.9660e-01 1.11522e-01 0.5000 5.9660e-01 3\n",
" 5 4.0165e+00 7.5898e-02 5.6817e-03 3.8838e-01 1.61868e-01 0.2500 3.8838e-01 3\n",
" 6 4.0059e+00 7.5897e-02 4.2643e-03 3.8874e-01 5.04763e-02 1.0000 3.8874e-01 3\n",
" 7 1.6026e+00 7.5900e-02 3.9509e-05 1.5263e-01 2.23322e-01 0.0312 1.5263e-01 3\n",
" 8 1.5825e+00 7.5900e-02 3.8376e-05 1.5062e-01 1.83708e-01 0.0312 1.5062e-01 3\n",
" 9 1.5468e+00 7.5899e-02 3.7240e-05 1.4705e-01 1.51128e-01 0.0625 1.4705e-01 3\n",
" 10 1.5059e+00 7.5899e-02 3.5067e-05 1.4296e-01 9.76732e-02 0.1250 1.4296e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.3954e+00 7.5899e-02 3.0884e-05 1.3192e-01 2.88391e-02 1.0000 1.3192e-01 3\n",
" 12 3.2492e-01 7.5900e-02 2.1320e-06 2.4900e-02 1.31764e-01 0.0312 2.4900e-02 2\n",
" 13 3.1725e-01 7.5900e-02 2.0945e-06 2.4133e-02 1.08843e-01 0.0625 2.4133e-02 2\n",
" 14 3.0920e-01 7.5900e-02 2.0470e-06 2.3328e-02 7.09229e-02 0.1250 2.3328e-02 2\n",
" 15 2.8977e-01 7.5899e-02 1.9419e-06 2.1386e-02 2.16866e-02 1.0000 2.1386e-02 2\n",
" 16 2.5540e-01 7.5900e-02 1.2172e-06 1.7949e-02 9.69660e-02 0.0020 1.7949e-02 1\n",
" 17 2.5540e-01 7.5900e-02 1.2172e-06 1.7949e-02 9.68947e-02 0.0156 1.7949e-02 1\n",
" 18 2.5446e-01 7.5900e-02 1.1985e-06 1.7855e-02 8.87626e-02 0.0078 1.7855e-02 1\n",
" 19 2.5434e-01 7.5899e-02 1.1892e-06 1.7843e-02 8.46580e-02 0.0156 1.7843e-02 1\n",
" 20 2.5367e-01 7.5899e-02 1.1712e-06 1.7775e-02 7.76204e-02 0.0156 1.7775e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.5340e-01 7.5899e-02 1.1535e-06 1.7749e-02 7.11909e-02 0.0156 1.7749e-02 1\n",
" 22 2.5254e-01 7.5899e-02 1.1358e-06 1.7663e-02 6.53197e-02 0.0156 1.7663e-02 1\n",
" 23 2.5175e-01 7.5899e-02 1.1184e-06 1.7584e-02 5.99475e-02 0.0312 1.7584e-02 1\n",
" 24 2.5105e-01 7.5899e-02 1.0845e-06 1.7514e-02 5.01344e-02 0.0312 1.7514e-02 1\n",
" 25 2.4963e-01 7.5899e-02 1.0516e-06 1.7372e-02 4.20028e-02 0.0625 1.7372e-02 1\n",
" 26 2.4804e-01 7.5899e-02 9.9445e-07 1.7213e-02 2.86315e-02 0.1250 1.7213e-02 1\n",
" 27 2.4375e-01 7.5899e-02 9.7322e-07 1.6784e-02 1.13336e-02 1.0000 1.6784e-02 1\n",
" 28 2.0032e-01 7.5899e-02 6.2687e-06 1.2436e-02 3.70329e-02 0.0625 1.2436e-02 1\n",
" 29 1.9879e-01 7.5899e-02 5.8831e-06 1.2283e-02 2.46438e-02 0.0625 1.2283e-02 1\n",
" 30 1.9684e-01 7.5899e-02 5.5184e-06 1.2088e-02 1.64938e-02 0.2500 1.2088e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.9051e-01 7.5899e-02 4.1604e-06 1.1457e-02 5.45566e-03 1.0000 1.1457e-02 1\n",
" END 1.4958e-01 7.5899e-02 1.0590e-07 7.3679e-03 ---- ---- 7.3679e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7679e+01 8.1237e-02 3.5485e+00 2.1135e-01 5.55735e-01 0.5000 ---- 3\n",
" 2 2.9374e+01 7.6869e-02 1.7855e+00 1.1442e+00 3.19970e-01 1.0000 1.1442e+00 3\n",
" 3 7.8743e+00 7.6440e-02 1.3424e-02 7.6636e-01 3.67785e-01 0.0625 7.6636e-01 3\n",
" 4 7.5837e+00 7.6438e-02 1.2585e-02 7.3814e-01 2.50038e-01 0.1250 7.3814e-01 3\n",
" 5 6.9869e+00 7.6436e-02 1.1010e-02 6.8004e-01 9.21718e-02 1.0000 6.8004e-01 3\n",
" 6 5.5433e+00 7.6440e-02 1.4681e-04 5.4654e-01 3.32731e-01 0.0625 5.4654e-01 3\n",
" 7 5.4329e+00 7.6438e-02 1.3613e-04 5.3551e-01 2.28061e-01 0.1250 5.3551e-01 3\n",
" 8 5.2063e+00 7.6436e-02 1.1621e-04 5.1287e-01 8.41039e-02 0.5000 5.1287e-01 3\n",
" 9 3.4133e+00 7.6437e-02 5.2307e-05 3.3364e-01 1.21866e-01 0.2500 3.3364e-01 3\n",
" 10 3.1187e+00 7.6436e-02 3.5916e-05 3.0419e-01 3.34066e-02 1.0000 3.0419e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 6.4805e-01 7.6439e-02 5.7192e-06 5.7156e-02 1.51865e-01 0.0312 5.7156e-02 3\n",
" 12 6.4533e-01 7.6439e-02 5.6517e-06 5.6883e-02 1.25004e-01 0.0312 5.6883e-02 3\n",
" 13 6.3496e-01 7.6439e-02 5.5514e-06 5.5847e-02 1.02904e-01 0.0625 5.5847e-02 3\n",
" 14 6.2616e-01 7.6439e-02 5.4144e-06 5.4966e-02 6.65598e-02 0.1250 5.4966e-02 3\n",
" 15 5.9511e-01 7.6438e-02 5.1073e-06 5.1862e-02 1.98314e-02 1.0000 5.1862e-02 3\n",
" 16 2.5348e-01 7.6439e-02 2.8871e-06 1.7701e-02 8.79056e-02 0.0312 1.7701e-02 2\n",
" 17 2.4909e-01 7.6439e-02 2.8318e-06 1.7263e-02 7.26831e-02 0.0625 1.7263e-02 2\n",
" 18 2.4500e-01 7.6439e-02 2.7539e-06 1.6854e-02 4.75561e-02 0.1250 1.6854e-02 2\n",
" 19 2.3131e-01 7.6439e-02 2.5938e-06 1.5484e-02 1.49728e-02 1.0000 1.5484e-02 2\n",
" END 1.5759e-01 7.6439e-02 1.5118e-06 8.1138e-03 ---- ---- 8.1138e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8074e+01 8.1789e-02 3.5830e+00 2.1626e-01 5.77790e-01 0.5000 ---- 3\n",
" 2 3.1554e+01 7.7435e-02 1.8031e+00 1.3446e+00 3.91406e-01 1.0000 1.3446e+00 3\n",
" 3 2.6007e+01 7.7022e-02 1.4091e-02 2.5789e+00 6.77255e-01 0.0625 2.5789e+00 3\n",
" 4 2.5128e+01 7.7013e-02 1.3213e-02 2.4918e+00 4.64018e-01 0.1250 2.4918e+00 3\n",
" 5 2.3455e+01 7.7007e-02 1.1565e-02 2.3262e+00 1.65037e-01 1.0000 2.3262e+00 3\n",
" 6 1.9375e+01 7.7028e-02 1.7804e-04 1.9297e+00 5.97368e-01 0.0312 1.9297e+00 3\n",
" 7 1.8903e+01 7.7022e-02 1.7127e-04 1.8825e+00 5.20095e-01 0.0625 1.8825e+00 3\n",
" 8 1.8425e+01 7.7015e-02 1.5626e-04 1.8346e+00 3.70798e-01 0.1250 1.8346e+00 3\n",
" 9 1.7424e+01 7.7010e-02 1.2552e-04 1.7345e+00 1.40665e-01 1.0000 1.7345e+00 3\n",
" 10 1.4530e+01 7.7025e-02 1.5933e-04 1.4451e+00 5.36294e-01 0.0312 1.4451e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.4173e+01 7.7021e-02 1.5658e-04 1.4095e+00 4.57356e-01 0.0625 1.4095e+00 3\n",
" 12 1.3833e+01 7.7016e-02 1.5303e-04 1.3754e+00 3.18170e-01 0.1250 1.3754e+00 3\n",
" 13 1.3207e+01 7.7012e-02 1.4500e-04 1.3129e+00 1.18839e-01 0.5000 1.3129e+00 3\n",
" 14 8.0979e+00 7.7017e-02 9.1894e-05 8.0199e-01 1.75041e-01 0.2500 8.0199e-01 3\n",
" 15 7.9685e+00 7.7015e-02 8.1370e-05 7.8907e-01 5.21728e-02 1.0000 7.8907e-01 3\n",
" 16 3.2231e+00 7.7024e-02 2.5926e-05 3.1458e-01 2.25262e-01 0.0312 3.1458e-01 3\n",
" 17 3.1969e+00 7.7022e-02 2.5359e-05 3.1196e-01 1.89336e-01 0.0312 3.1196e-01 3\n",
" 18 3.1296e+00 7.7022e-02 2.4763e-05 3.0524e-01 1.58237e-01 0.0625 3.0524e-01 3\n",
" 19 3.0606e+00 7.7021e-02 2.3822e-05 2.9833e-01 1.04692e-01 0.1250 2.9833e-01 3\n",
" 20 2.8493e+00 7.7020e-02 2.2075e-05 2.7720e-01 3.26566e-02 1.0000 2.7720e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 5.4598e-01 7.7023e-02 1.2079e-05 4.6884e-02 1.42354e-01 0.0156 4.6884e-02 3\n",
" 22 5.4323e-01 7.7023e-02 1.1951e-05 4.6609e-02 1.30041e-01 0.0156 4.6609e-02 3\n",
" 23 5.3920e-01 7.7023e-02 1.1815e-05 4.6206e-02 1.18767e-01 0.0156 4.6206e-02 3\n",
" 24 5.3414e-01 7.7022e-02 1.1671e-05 4.5700e-02 1.08451e-01 0.0312 4.5700e-02 3\n",
" 25 5.3206e-01 7.7022e-02 1.1442e-05 4.5493e-02 8.95855e-02 0.0312 4.5493e-02 3\n",
" 26 5.2399e-01 7.7022e-02 1.1175e-05 4.4686e-02 7.39794e-02 0.0625 4.4686e-02 3\n",
" 27 5.1838e-01 7.7022e-02 1.0718e-05 4.4125e-02 4.82076e-02 0.1250 4.4125e-02 3\n",
" 28 4.9704e-01 7.7022e-02 9.7689e-06 4.1992e-02 1.47413e-02 1.0000 4.1992e-02 3\n",
" 29 3.1241e-01 7.7022e-02 2.0481e-06 2.3536e-02 6.51339e-02 0.0312 2.3536e-02 3\n",
" 30 3.0992e-01 7.7022e-02 2.0252e-06 2.3287e-02 5.39641e-02 0.0312 2.3287e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 3.0475e-01 7.7022e-02 1.9906e-06 2.2771e-02 4.47088e-02 0.0625 2.2771e-02 3\n",
" 32 2.9867e-01 7.7022e-02 1.9456e-06 2.2163e-02 2.93670e-02 0.1250 2.2163e-02 3\n",
" 33 2.8217e-01 7.7022e-02 1.8415e-06 2.0513e-02 9.27777e-03 1.0000 2.0513e-02 3\n",
" END 1.2447e-01 7.7022e-02 9.0089e-07 4.7440e-03 ---- ---- 4.7440e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8028e+01 8.2368e-02 3.5902e+00 2.0432e-01 5.58473e-01 0.5000 ---- 3\n",
" 2 3.0146e+01 7.7383e-02 1.8260e+00 1.1810e+00 3.65957e-01 1.0000 1.1810e+00 3\n",
" 3 1.9467e+01 7.6321e-02 3.7121e-02 1.9020e+00 5.78219e-01 0.0625 1.9020e+00 3\n",
" 4 1.8783e+01 7.6315e-02 3.4805e-02 1.8359e+00 3.96224e-01 0.1250 1.8359e+00 3\n",
" 5 1.7498e+01 7.6310e-02 3.0458e-02 1.7117e+00 1.43329e-01 1.0000 1.7117e+00 3\n",
" 6 1.3463e+01 7.6324e-02 1.6595e-04 1.3385e+00 5.12166e-01 0.0312 1.3385e+00 3\n",
" 7 1.3189e+01 7.6320e-02 1.5983e-04 1.3111e+00 4.43717e-01 0.0625 1.3111e+00 3\n",
" 8 1.2988e+01 7.6314e-02 1.4653e-04 1.2910e+00 3.12678e-01 0.1250 1.2910e+00 3\n",
" 9 1.2416e+01 7.6311e-02 1.1972e-04 1.2338e+00 1.13610e-01 1.0000 1.2338e+00 3\n",
" 10 8.2950e+00 7.6322e-02 1.1249e-04 8.2176e-01 4.53925e-01 0.0312 8.2176e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 8.1468e+00 7.6319e-02 1.1073e-04 8.0694e-01 3.83205e-01 0.0625 8.0694e-01 3\n",
" 12 8.0934e+00 7.6316e-02 1.0855e-04 8.0160e-01 2.61129e-01 0.1250 8.0160e-01 3\n",
" 13 7.9203e+00 7.6313e-02 1.0282e-04 7.8430e-01 9.12095e-02 1.0000 7.8430e-01 3\n",
" 14 7.7423e+00 7.6320e-02 4.3824e-05 7.6656e-01 3.57702e-01 0.0312 7.6656e-01 3\n",
" 15 7.5684e+00 7.6318e-02 4.3101e-05 7.4916e-01 3.05536e-01 0.0625 7.4916e-01 3\n",
" 16 7.3943e+00 7.6316e-02 4.2410e-05 7.3176e-01 2.11983e-01 0.1250 7.3176e-01 3\n",
" 17 6.9382e+00 7.6314e-02 4.1267e-05 6.8615e-01 7.76813e-02 1.0000 6.8615e-01 3\n",
" 18 4.1871e+00 7.6320e-02 3.5437e-05 4.1104e-01 3.04688e-01 0.0312 4.1104e-01 3\n",
" 19 4.1131e+00 7.6319e-02 3.4919e-05 4.0365e-01 2.57045e-01 0.0625 4.0365e-01 3\n",
" 20 4.0825e+00 7.6317e-02 3.4341e-05 4.0058e-01 1.75862e-01 0.1250 4.0058e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.9856e+00 7.6316e-02 3.2776e-05 3.9090e-01 6.42347e-02 0.5000 3.9090e-01 3\n",
" 22 2.4853e+00 7.6317e-02 2.0721e-05 2.4088e-01 9.51928e-02 0.2500 2.4088e-01 3\n",
" 23 2.3853e+00 7.6317e-02 1.8479e-05 2.3088e-01 2.74763e-02 1.0000 2.3088e-01 3\n",
" 24 1.1313e+00 7.6319e-02 7.7889e-06 1.0549e-01 1.21997e-01 0.0312 1.0549e-01 3\n",
" 25 1.1173e+00 7.6319e-02 7.6737e-06 1.0409e-01 1.01374e-01 0.0625 1.0409e-01 3\n",
" 26 1.1140e+00 7.6318e-02 7.5635e-06 1.0376e-01 6.65551e-02 0.1250 1.0376e-01 3\n",
" 27 1.0659e+00 7.6318e-02 7.2947e-06 9.8946e-02 2.05805e-02 1.0000 9.8946e-02 3\n",
" 28 2.6188e-01 7.6319e-02 4.4363e-06 1.8552e-02 9.18459e-02 0.0020 1.8552e-02 1\n",
" 29 2.6188e-01 7.6319e-02 4.4363e-06 1.8552e-02 9.18007e-02 0.0156 1.8552e-02 1\n",
" 30 2.6045e-01 7.6319e-02 4.3629e-06 1.8409e-02 8.41689e-02 0.0078 1.8409e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.6020e-01 7.6319e-02 4.3275e-06 1.8384e-02 8.03149e-02 0.0156 1.8384e-02 1\n",
" 32 2.5915e-01 7.6319e-02 4.2543e-06 1.8279e-02 7.37054e-02 0.0156 1.8279e-02 1\n",
" 33 2.5856e-01 7.6319e-02 4.1815e-06 1.8220e-02 6.76634e-02 0.0312 1.8220e-02 1\n",
" 34 2.5792e-01 7.6319e-02 4.0244e-06 1.8156e-02 5.66523e-02 0.0312 1.8156e-02 1\n",
" 35 2.5673e-01 7.6319e-02 3.8715e-06 1.8037e-02 4.75483e-02 0.0625 1.8037e-02 1\n",
" 36 2.5544e-01 7.6319e-02 3.5265e-06 1.7909e-02 3.24910e-02 0.1250 1.7909e-02 1\n",
" 37 2.5190e-01 7.6319e-02 2.7655e-06 1.7556e-02 1.28194e-02 1.0000 1.7556e-02 1\n",
" 38 2.4321e-01 7.6319e-02 7.9723e-06 1.6681e-02 4.01092e-02 0.0312 1.6681e-02 1\n",
" 39 2.4244e-01 7.6319e-02 7.7077e-06 1.6604e-02 3.33148e-02 0.0312 1.6604e-02 1\n",
" 40 2.4091e-01 7.6319e-02 7.4560e-06 1.6452e-02 2.76601e-02 0.0625 1.6452e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 2.3893e-01 7.6319e-02 6.9601e-06 1.6254e-02 1.83190e-02 0.1250 1.6254e-02 1\n",
" 42 2.3351e-01 7.6319e-02 6.0371e-06 1.5713e-02 6.11579e-03 1.0000 1.5713e-02 1\n",
" END 1.6842e-01 7.6319e-02 4.1677e-07 9.2097e-03 ---- ---- 9.2097e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7763e+01 8.1648e-02 3.5582e+00 2.0992e-01 5.42171e-01 0.5000 ---- 3\n",
" 2 2.9290e+01 7.6782e-02 1.8058e+00 1.1156e+00 3.43431e-01 1.0000 1.1156e+00 3\n",
" 3 1.1296e+01 7.5840e-02 3.1824e-02 1.0902e+00 4.87278e-01 0.0625 1.0902e+00 3\n",
" 4 1.0933e+01 7.5836e-02 2.9837e-02 1.0558e+00 3.26991e-01 0.1250 1.0558e+00 3\n",
" 5 1.0148e+01 7.5833e-02 2.6107e-02 9.8107e-01 1.13279e-01 1.0000 9.8107e-01 3\n",
" 6 5.5899e+00 7.5841e-02 1.4308e-04 5.5126e-01 4.27388e-01 0.0312 5.5126e-01 3\n",
" 7 5.4884e+00 7.5839e-02 1.3782e-04 5.4111e-01 3.58370e-01 0.0625 5.4111e-01 3\n",
" 8 5.4222e+00 7.5836e-02 1.2689e-04 5.3451e-01 2.40236e-01 0.1250 5.3451e-01 3\n",
" 9 5.1952e+00 7.5834e-02 1.0669e-04 5.1183e-01 8.07191e-02 1.0000 5.1183e-01 3\n",
" 10 4.0055e+00 7.5839e-02 3.6344e-05 3.9293e-01 3.29789e-01 0.0312 3.9293e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.9192e+00 7.5838e-02 3.5776e-05 3.8430e-01 2.76056e-01 0.0625 3.8430e-01 3\n",
" 12 3.8457e+00 7.5836e-02 3.5080e-05 3.7695e-01 1.85901e-01 0.1250 3.7695e-01 3\n",
" 13 3.6782e+00 7.5835e-02 3.3297e-05 3.6021e-01 6.48445e-02 1.0000 3.6021e-01 3\n",
" 14 3.5565e+00 7.5838e-02 1.5685e-05 3.4805e-01 2.59838e-01 0.0625 3.4805e-01 3\n",
" 15 3.5221e+00 7.5836e-02 1.5826e-05 3.4461e-01 1.77260e-01 0.1250 3.4461e-01 3\n",
" 16 3.4401e+00 7.5835e-02 1.5859e-05 3.3641e-01 6.45214e-02 0.5000 3.3641e-01 3\n",
" 17 2.3772e+00 7.5836e-02 1.1266e-05 2.3012e-01 9.43984e-02 0.2500 2.3012e-01 3\n",
" 18 2.1820e+00 7.5836e-02 1.0702e-05 2.1060e-01 2.66604e-02 1.0000 2.1060e-01 3\n",
" 19 4.7153e-01 7.5838e-02 5.5663e-06 3.9564e-02 1.20718e-01 0.0312 3.9564e-02 2\n",
" 20 4.5983e-01 7.5837e-02 5.5162e-06 3.8394e-02 9.94667e-02 0.0625 3.8394e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.4588e-01 7.5837e-02 5.5065e-06 3.6998e-02 6.44946e-02 0.1250 3.6998e-02 2\n",
" 22 4.0819e-01 7.5837e-02 5.3683e-06 3.3230e-02 1.93650e-02 1.0000 3.3230e-02 2\n",
" END 1.4220e-01 7.5838e-02 3.2832e-06 6.6331e-03 ---- ---- 6.6331e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7463e+01 8.1155e-02 3.5365e+00 2.0168e-01 5.63730e-01 0.5000 ---- 3\n",
" 2 3.0372e+01 7.6372e-02 1.7919e+00 1.2377e+00 3.51308e-01 1.0000 1.2377e+00 3\n",
" 3 2.2130e+01 7.5508e-02 2.7519e-02 2.1779e+00 5.57574e-01 0.0625 2.1779e+00 3\n",
" 4 2.1840e+01 7.5500e-02 2.5794e-02 2.1506e+00 3.94636e-01 0.1250 2.1506e+00 3\n",
" 5 2.0744e+01 7.5494e-02 2.2556e-02 2.0443e+00 1.41578e-01 1.0000 2.0443e+00 3\n",
" 6 8.2954e+00 7.5515e-02 4.2815e-04 8.2156e-01 5.53404e-01 0.0312 8.2156e-01 3\n",
" 7 8.2297e+00 7.5511e-02 4.1737e-04 8.1500e-01 4.63164e-01 0.0312 8.1500e-01 3\n",
" 8 8.0661e+00 7.5508e-02 4.0601e-04 7.9866e-01 3.86594e-01 0.0625 7.9866e-01 3\n",
" 9 7.9735e+00 7.5505e-02 3.8497e-04 7.8942e-01 2.55931e-01 0.1250 7.8942e-01 3\n",
" 10 7.6778e+00 7.5503e-02 3.4324e-04 7.5988e-01 8.04439e-02 1.0000 7.5988e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 4.1702e+00 7.5511e-02 2.2849e-05 4.0944e-01 3.42142e-01 0.0312 4.0944e-01 3\n",
" 12 4.1384e+00 7.5509e-02 2.2636e-05 4.0627e-01 2.86102e-01 0.0312 4.0627e-01 3\n",
" 13 4.0506e+00 7.5508e-02 2.2301e-05 3.9748e-01 2.38345e-01 0.0625 3.9748e-01 3\n",
" 14 3.9585e+00 7.5507e-02 2.1988e-05 3.8828e-01 1.57301e-01 0.1250 3.8828e-01 3\n",
" 15 3.6769e+00 7.5506e-02 2.1227e-05 3.6012e-01 4.95626e-02 1.0000 3.6012e-01 3\n",
" 16 1.0801e+00 7.5510e-02 1.3820e-05 1.0045e-01 2.09498e-01 0.0625 1.0045e-01 3\n",
" 17 1.0765e+00 7.5509e-02 1.3905e-05 1.0008e-01 1.38056e-01 0.0625 1.0008e-01 3\n",
" 18 1.0240e+00 7.5508e-02 1.3417e-05 9.4834e-02 9.09896e-02 0.1250 9.4834e-02 3\n",
" 19 9.2793e-01 7.5508e-02 1.2361e-05 8.5230e-02 2.92568e-02 1.0000 8.5230e-02 3\n",
" 20 6.1991e-01 7.5508e-02 3.4959e-06 5.4437e-02 1.24331e-01 0.0312 5.4437e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 6.0287e-01 7.5508e-02 3.4465e-06 5.2733e-02 1.03376e-01 0.0625 5.2733e-02 2\n",
" 22 5.8339e-01 7.5508e-02 3.3991e-06 5.0785e-02 6.85656e-02 0.1250 5.0785e-02 2\n",
" 23 5.4468e-01 7.5508e-02 3.2756e-06 4.6914e-02 2.26678e-02 1.0000 4.6914e-02 2\n",
" 24 4.0940e-01 7.5508e-02 2.2027e-06 3.3387e-02 9.38437e-02 0.0312 3.3387e-02 3\n",
" 25 4.0248e-01 7.5508e-02 2.1686e-06 3.2695e-02 7.84443e-02 0.0625 3.2695e-02 3\n",
" 26 3.9700e-01 7.5508e-02 2.1295e-06 3.2147e-02 5.26950e-02 0.1250 3.2147e-02 3\n",
" 27 3.8664e-01 7.5508e-02 2.0377e-06 3.1111e-02 1.82957e-02 0.5000 3.1111e-02 3\n",
" 28 2.7457e-01 7.5508e-02 1.3615e-06 1.9905e-02 3.04906e-02 0.0020 1.9905e-02 1\n",
" 29 2.7457e-01 7.5508e-02 1.3615e-06 1.9905e-02 3.04532e-02 0.1250 1.9905e-02 1\n",
" 30 2.6472e-01 7.5508e-02 1.6395e-06 1.8920e-02 1.26126e-02 0.5000 1.8920e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.6326e-01 7.5508e-02 3.7938e-06 1.8771e-02 1.63772e-02 0.2500 1.8771e-02 1\n",
" 32 2.4699e-01 7.5508e-02 2.2950e-06 1.7146e-02 5.58451e-03 1.0000 1.7146e-02 1\n",
" END 1.7333e-01 7.5508e-02 2.0744e-06 9.7801e-03 ---- ---- 9.7801e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7400e+01 8.0816e-02 3.5185e+00 2.1348e-01 5.28763e-01 0.5000 ---- 3\n",
" 2 2.9415e+01 7.6088e-02 1.7802e+00 1.1537e+00 3.33175e-01 1.0000 1.1537e+00 3\n",
" 3 8.8610e+00 7.5285e-02 2.4906e-02 8.5366e-01 4.70109e-01 0.0625 8.5366e-01 3\n",
" 4 8.5980e+00 7.5282e-02 2.3350e-02 8.2893e-01 3.07117e-01 0.1250 8.2893e-01 3\n",
" 5 7.9481e+00 7.5280e-02 2.0431e-02 7.6685e-01 9.74269e-02 1.0000 7.6685e-01 3\n",
" 6 4.0484e+00 7.5286e-02 1.1924e-04 3.9720e-01 3.80981e-01 0.0312 3.9720e-01 3\n",
" 7 3.9810e+00 7.5285e-02 1.1490e-04 3.9046e-01 3.16159e-01 0.0625 3.9046e-01 3\n",
" 8 3.9454e+00 7.5283e-02 1.0600e-04 3.8690e-01 2.07322e-01 0.1250 3.8690e-01 3\n",
" 9 3.7779e+00 7.5282e-02 8.9724e-05 3.7017e-01 6.50098e-02 1.0000 3.7017e-01 3\n",
" 10 2.4203e+00 7.5285e-02 1.9632e-05 2.3449e-01 2.72312e-01 0.0312 2.3449e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.3791e+00 7.5284e-02 1.9379e-05 2.3037e-01 2.26121e-01 0.0625 2.3037e-01 3\n",
" 12 2.3604e+00 7.5283e-02 1.9139e-05 2.2849e-01 1.49489e-01 0.1250 2.2849e-01 3\n",
" 13 2.2932e+00 7.5282e-02 1.8351e-05 2.2177e-01 4.93426e-02 1.0000 2.2177e-01 3\n",
" 14 2.2737e+00 7.5284e-02 8.4059e-06 2.1983e-01 2.02998e-01 0.0625 2.1983e-01 3\n",
" 15 2.2637e+00 7.5283e-02 8.4558e-06 2.1884e-01 1.36850e-01 0.1250 2.1884e-01 3\n",
" 16 2.2263e+00 7.5282e-02 8.4174e-06 2.1509e-01 4.79772e-02 0.5000 2.1509e-01 3\n",
" 17 1.5342e+00 7.5282e-02 5.9775e-06 1.4588e-01 7.24441e-02 0.2500 1.4588e-01 3\n",
" 18 1.4396e+00 7.5282e-02 5.7228e-06 1.3643e-01 2.21522e-02 1.0000 1.3643e-01 3\n",
" 19 3.2585e-01 7.5283e-02 3.9721e-06 2.5053e-02 1.05058e-01 0.0020 2.5053e-02 1\n",
" 20 3.2585e-01 7.5283e-02 3.9721e-06 2.5053e-02 1.04950e-01 0.0156 2.5053e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.2419e-01 7.5283e-02 3.9032e-06 2.4887e-02 9.58068e-02 0.0078 2.4887e-02 1\n",
" 22 3.2391e-01 7.5283e-02 3.8708e-06 2.4859e-02 9.12080e-02 0.0156 2.4859e-02 1\n",
" 23 3.2268e-01 7.5283e-02 3.8027e-06 2.4735e-02 8.33340e-02 0.0156 2.4735e-02 1\n",
" 24 3.2201e-01 7.5283e-02 3.7354e-06 2.4669e-02 7.61563e-02 0.0312 2.4669e-02 1\n",
" 25 3.2158e-01 7.5283e-02 3.5868e-06 2.4626e-02 6.31288e-02 0.0312 2.4626e-02 1\n",
" 26 3.2021e-01 7.5283e-02 3.4441e-06 2.4490e-02 5.24524e-02 0.0625 2.4490e-02 1\n",
" 27 3.1899e-01 7.5283e-02 3.1171e-06 2.4368e-02 3.50023e-02 0.1250 2.4368e-02 1\n",
" 28 3.1453e-01 7.5283e-02 2.3941e-06 2.3923e-02 1.28701e-02 1.0000 2.3923e-02 1\n",
" 29 2.3986e-01 7.5283e-02 8.0473e-06 1.6450e-02 4.20262e-02 0.0625 1.6450e-02 1\n",
" 30 2.3750e-01 7.5283e-02 7.4902e-06 1.6214e-02 2.74015e-02 0.0625 1.6214e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.3541e-01 7.5283e-02 6.9986e-06 1.6006e-02 1.79773e-02 0.2500 1.6006e-02 1\n",
" 32 2.2517e-01 7.5283e-02 5.0846e-06 1.4984e-02 7.02368e-03 1.0000 1.4984e-02 1\n",
" 33 1.8588e-01 7.5283e-02 3.3528e-07 1.1060e-02 3.31734e-02 0.0625 1.1060e-02 1\n",
" 34 1.8473e-01 7.5283e-02 3.4914e-07 1.0944e-02 2.09848e-02 0.0625 1.0944e-02 1\n",
" 35 1.8327e-01 7.5283e-02 3.4182e-07 1.0799e-02 1.33219e-02 0.2500 1.0799e-02 1\n",
" 36 1.7634e-01 7.5283e-02 3.5451e-07 1.0106e-02 6.18393e-03 1.0000 1.0106e-02 1\n",
" END 1.5614e-01 7.5283e-02 2.6319e-07 8.0859e-03 ---- ---- 8.0859e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7226e+01 8.0586e-02 3.5069e+00 2.0763e-01 5.32142e-01 0.5000 ---- 3\n",
" 2 2.8792e+01 7.5896e-02 1.7726e+00 1.0990e+00 2.98812e-01 1.0000 1.0990e+00 3\n",
" 3 6.7526e+00 7.5132e-02 2.2281e-02 6.4546e-01 3.25973e-01 0.0625 6.4546e-01 3\n",
" 4 6.4900e+00 7.5129e-02 2.0887e-02 6.2060e-01 2.16635e-01 0.1250 6.2060e-01 3\n",
" 5 5.8876e+00 7.5128e-02 1.8273e-02 5.6297e-01 7.54846e-02 1.0000 5.6297e-01 3\n",
" 6 3.3841e+00 7.5132e-02 1.2152e-04 3.3078e-01 2.68099e-01 0.0625 3.3078e-01 3\n",
" 7 3.3269e+00 7.5130e-02 1.1407e-04 3.2507e-01 1.79135e-01 0.1250 3.2507e-01 3\n",
" 8 3.2046e+00 7.5129e-02 1.0002e-04 3.1284e-01 6.06075e-02 0.5000 3.1284e-01 3\n",
" 9 2.0406e+00 7.5130e-02 5.0918e-05 1.9650e-01 9.42415e-02 0.2500 1.9650e-01 3\n",
" 10 1.9896e+00 7.5129e-02 3.8048e-05 1.9141e-01 3.16623e-02 1.0000 1.9141e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 7.2830e-01 7.5131e-02 2.2094e-06 6.5315e-02 1.51234e-01 0.0312 6.5315e-02 3\n",
" 12 7.2355e-01 7.5130e-02 2.1820e-06 6.4840e-02 1.23406e-01 0.0312 6.4840e-02 3\n",
" 13 7.1014e-01 7.5130e-02 2.1414e-06 6.3499e-02 1.00717e-01 0.0625 6.3499e-02 3\n",
" 14 6.9536e-01 7.5130e-02 2.0811e-06 6.2020e-02 6.37565e-02 0.1250 6.2020e-02 3\n",
" 15 6.4887e-01 7.5130e-02 1.9368e-06 5.7372e-02 1.77210e-02 1.0000 5.7372e-02 3\n",
" 16 2.7810e-01 7.5130e-02 5.9639e-07 2.0297e-02 8.07791e-02 0.0625 2.0297e-02 3\n",
" 17 2.7807e-01 7.5130e-02 5.9869e-07 2.0294e-02 5.21243e-02 0.1250 2.0294e-02 3\n",
" 18 2.7365e-01 7.5130e-02 5.9908e-07 1.9852e-02 1.53737e-02 1.0000 1.9852e-02 3\n",
" 19 2.6419e-01 7.5130e-02 6.6496e-07 1.8905e-02 6.83250e-02 0.0625 1.8905e-02 2\n",
" 20 2.6226e-01 7.5130e-02 6.6591e-07 1.8712e-02 4.45733e-02 0.1250 1.8712e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.5151e-01 7.5130e-02 6.6120e-07 1.7638e-02 1.38292e-02 1.0000 1.7638e-02 2\n",
" 22 1.8696e-01 7.5130e-02 6.1577e-07 1.1182e-02 6.00789e-02 0.0312 1.1182e-02 2\n",
" 23 1.8425e-01 7.5130e-02 6.0780e-07 1.0911e-02 4.99039e-02 0.0625 1.0911e-02 2\n",
" 24 1.8261e-01 7.5130e-02 6.0181e-07 1.0747e-02 3.30276e-02 0.1250 1.0747e-02 2\n",
" 25 1.7728e-01 7.5130e-02 5.8587e-07 1.0215e-02 1.47265e-02 0.0020 1.0215e-02 1\n",
" 26 1.7728e-01 7.5130e-02 5.8587e-07 1.0215e-02 1.47364e-02 0.2500 1.0215e-02 1\n",
" END 1.6503e-01 7.5130e-02 1.0814e-06 8.9887e-03 ---- ---- 8.9887e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7302e+01 8.0423e-02 3.5118e+00 2.1035e-01 5.43165e-01 0.5000 ---- 3\n",
" 2 2.9213e+01 7.6042e-02 1.7667e+00 1.1470e+00 3.18827e-01 1.0000 1.1470e+00 3\n",
" 3 6.7422e+00 7.5607e-02 1.2673e-02 6.5399e-01 3.63103e-01 0.0625 6.5399e-01 3\n",
" 4 6.4820e+00 7.5605e-02 1.1881e-02 6.2876e-01 2.40619e-01 0.1250 6.2876e-01 3\n",
" 5 5.9252e+00 7.5603e-02 1.0395e-02 5.7457e-01 8.16365e-02 1.0000 5.7457e-01 3\n",
" 6 4.2837e+00 7.5606e-02 1.2266e-04 4.2069e-01 3.01105e-01 0.0625 4.2069e-01 3\n",
" 7 4.2250e+00 7.5604e-02 1.1351e-04 4.1483e-01 2.00994e-01 0.1250 4.1483e-01 3\n",
" 8 4.0882e+00 7.5603e-02 9.6844e-05 4.0117e-01 6.73009e-02 0.5000 4.0117e-01 3\n",
" 9 2.5433e+00 7.5604e-02 4.6215e-05 2.4672e-01 1.04456e-01 0.2500 2.4672e-01 3\n",
" 10 2.4125e+00 7.5603e-02 3.1548e-05 2.3366e-01 3.50321e-02 1.0000 2.3366e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 5.2544e-01 7.5606e-02 8.6126e-06 4.4975e-02 1.66172e-01 0.0312 4.4975e-02 3\n",
" 12 5.2290e-01 7.5605e-02 8.5372e-06 4.4721e-02 1.35807e-01 0.0312 4.4721e-02 3\n",
" 13 5.1457e-01 7.5605e-02 8.3999e-06 4.3888e-02 1.11017e-01 0.0625 4.3888e-02 3\n",
" 14 5.0662e-01 7.5605e-02 8.2217e-06 4.3094e-02 7.05535e-02 0.1250 4.3094e-02 3\n",
" 15 4.8526e-01 7.5605e-02 7.7596e-06 4.0958e-02 1.94214e-02 1.0000 4.0958e-02 3\n",
" 16 2.6790e-01 7.5605e-02 2.9032e-06 1.9226e-02 9.01061e-02 0.0312 1.9226e-02 3\n",
" 17 2.6579e-01 7.5605e-02 2.8686e-06 1.9016e-02 7.41217e-02 0.0312 1.9016e-02 3\n",
" 18 2.6186e-01 7.5605e-02 2.8174e-06 1.8622e-02 6.09972e-02 0.0625 1.8622e-02 3\n",
" 19 2.5786e-01 7.5605e-02 2.7463e-06 1.8223e-02 3.94438e-02 0.1250 1.8223e-02 3\n",
" 20 2.5105e-01 7.5605e-02 2.5817e-06 1.7542e-02 1.17901e-02 1.0000 1.7542e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 1.4970e-01 7.5605e-02 1.1425e-06 7.4079e-03 ---- ---- 7.4079e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7597e+01 8.0917e-02 3.5464e+00 2.0514e-01 5.61161e-01 0.5000 ---- 3\n",
" 2 3.0231e+01 7.6549e-02 1.7842e+00 1.2313e+00 3.83789e-01 1.0000 1.2313e+00 3\n",
" 3 2.1262e+01 7.6127e-02 1.3141e-02 2.1054e+00 6.89822e-01 0.0625 2.1054e+00 3\n",
" 4 2.0729e+01 7.6119e-02 1.2323e-02 2.0530e+00 4.54585e-01 0.1250 2.0530e+00 3\n",
" 5 1.9523e+01 7.6114e-02 1.0787e-02 1.9339e+00 1.40844e-01 1.0000 1.9339e+00 3\n",
" 6 1.2812e+01 7.6132e-02 1.6953e-04 1.2734e+00 5.54553e-01 0.0312 1.2734e+00 3\n",
" 7 1.2553e+01 7.6128e-02 1.6296e-04 1.2475e+00 4.72699e-01 0.0625 1.2475e+00 3\n",
" 8 1.2379e+01 7.6123e-02 1.4864e-04 1.2301e+00 3.22838e-01 0.1250 1.2301e+00 3\n",
" 9 1.1827e+01 7.6119e-02 1.2074e-04 1.1749e+00 1.06950e-01 1.0000 1.1749e+00 3\n",
" 10 8.1246e+00 7.6130e-02 9.5469e-05 8.0475e-01 4.37576e-01 0.0312 8.0475e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 7.9839e+00 7.6127e-02 9.4052e-05 7.9068e-01 3.67742e-01 0.0625 7.9068e-01 3\n",
" 12 7.9423e+00 7.6123e-02 9.2480e-05 7.8652e-01 2.47437e-01 0.1250 7.8652e-01 3\n",
" 13 7.7854e+00 7.6121e-02 8.8197e-05 7.7084e-01 8.25854e-02 1.0000 7.7084e-01 3\n",
" 14 7.4994e+00 7.6127e-02 4.1116e-05 7.4229e-01 3.27924e-01 0.0312 7.4229e-01 3\n",
" 15 7.3301e+00 7.6125e-02 4.0404e-05 7.2536e-01 2.79645e-01 0.0625 7.2536e-01 3\n",
" 16 7.1696e+00 7.6123e-02 3.9689e-05 7.0931e-01 1.93344e-01 0.1250 7.0931e-01 3\n",
" 17 6.7812e+00 7.6121e-02 3.8603e-05 6.7047e-01 6.96655e-02 1.0000 6.7047e-01 3\n",
" 18 4.7308e+00 7.6127e-02 3.3882e-05 4.6543e-01 2.74438e-01 0.0312 4.6543e-01 3\n",
" 19 4.6305e+00 7.6126e-02 3.3370e-05 4.5540e-01 2.31828e-01 0.0625 4.5540e-01 3\n",
" 20 4.5518e+00 7.6124e-02 3.2812e-05 4.4754e-01 1.58815e-01 0.1250 4.4754e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.3766e+00 7.6123e-02 3.1409e-05 4.3002e-01 5.76812e-02 1.0000 4.3002e-01 3\n",
" 22 4.1180e+00 7.6126e-02 1.7862e-05 4.0417e-01 2.24061e-01 0.0625 4.0417e-01 3\n",
" 23 4.1124e+00 7.6125e-02 1.7880e-05 4.0361e-01 1.55877e-01 0.1250 4.0361e-01 3\n",
" 24 4.0520e+00 7.6123e-02 1.7874e-05 3.9757e-01 5.91680e-02 0.5000 3.9757e-01 3\n",
" 25 2.6841e+00 7.6124e-02 1.3242e-05 2.6078e-01 8.45499e-02 0.2500 2.6078e-01 3\n",
" 26 2.5362e+00 7.6124e-02 1.2528e-05 2.4600e-01 2.23822e-02 1.0000 2.4600e-01 3\n",
" 27 6.8811e-01 7.6126e-02 4.8737e-06 6.1193e-02 1.00189e-01 0.0312 6.1193e-02 3\n",
" 28 6.8011e-01 7.6126e-02 4.8372e-06 6.0394e-02 8.29251e-02 0.0625 6.0394e-02 3\n",
" 29 6.7996e-01 7.6126e-02 4.8412e-06 6.0379e-02 5.42586e-02 0.1250 6.0379e-02 3\n",
" 30 6.6003e-01 7.6126e-02 4.7222e-06 5.8386e-02 1.68859e-02 1.0000 5.8386e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.4553e-01 7.6126e-02 2.8892e-06 1.6937e-02 7.39375e-02 0.0156 1.6937e-02 3\n",
" 32 2.4495e-01 7.6126e-02 2.8553e-06 1.6880e-02 6.77244e-02 0.0156 1.6880e-02 3\n",
" 33 2.4385e-01 7.6126e-02 2.8203e-06 1.6769e-02 6.20274e-02 0.0156 1.6769e-02 3\n",
" 34 2.4233e-01 7.6126e-02 2.7843e-06 1.6618e-02 5.68078e-02 0.0312 1.6618e-02 3\n",
" 35 2.4201e-01 7.6126e-02 2.7248e-06 1.6586e-02 4.72306e-02 0.0312 1.6586e-02 3\n",
" 36 2.3945e-01 7.6126e-02 2.6589e-06 1.6330e-02 3.92739e-02 0.0625 1.6330e-02 3\n",
" 37 2.3754e-01 7.6126e-02 2.5469e-06 1.6139e-02 2.60778e-02 0.2500 1.6139e-02 2\n",
" 38 2.3274e-01 7.6126e-02 2.3129e-06 1.5659e-02 9.08676e-03 0.5000 1.5659e-02 3\n",
" 39 1.9033e-01 7.6126e-02 1.2964e-06 1.1419e-02 1.68454e-02 0.1250 1.1419e-02 2\n",
" 40 1.7754e-01 7.6126e-02 1.1813e-06 1.0140e-02 4.77790e-03 1.0000 1.0140e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 7.9259e-02 7.6126e-02 3.3498e-07 3.1296e-04 ---- ---- 1.1308e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7862e+01 8.1458e-02 3.5824e+00 1.9558e-01 5.60161e-01 0.5000 ---- 3\n",
" 2 3.0288e+01 7.7104e-02 1.8024e+00 1.2187e+00 3.53887e-01 1.0000 1.2187e+00 3\n",
" 3 1.5376e+01 7.6699e-02 1.3340e-02 1.5166e+00 5.38023e-01 0.0625 1.5166e+00 3\n",
" 4 1.4918e+01 7.6694e-02 1.2508e-02 1.4716e+00 3.58278e-01 0.1250 1.4716e+00 3\n",
" 5 1.3977e+01 7.6690e-02 1.0946e-02 1.3791e+00 1.16568e-01 1.0000 1.3791e+00 3\n",
" 6 1.1135e+01 7.6702e-02 1.3294e-04 1.1057e+00 4.32362e-01 0.0312 1.1057e+00 3\n",
" 7 1.0895e+01 7.6699e-02 1.2801e-04 1.0817e+00 3.71667e-01 0.0625 1.0817e+00 3\n",
" 8 1.0701e+01 7.6695e-02 1.1744e-04 1.0623e+00 2.57747e-01 0.1250 1.0623e+00 3\n",
" 9 1.0144e+01 7.6692e-02 9.6673e-05 1.0066e+00 8.87544e-02 1.0000 1.0066e+00 3\n",
" 10 5.2512e+00 7.6702e-02 6.5344e-05 5.1739e-01 3.58634e-01 0.0312 5.1739e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 5.2013e+00 7.6699e-02 6.4354e-05 5.1239e-01 3.01121e-01 0.0312 5.1239e-01 3\n",
" 12 5.0952e+00 7.6698e-02 6.3049e-05 5.0179e-01 2.51991e-01 0.0625 5.0179e-01 3\n",
" 13 5.0141e+00 7.6696e-02 6.0992e-05 4.9368e-01 1.68233e-01 0.1250 4.9368e-01 3\n",
" 14 4.7813e+00 7.6695e-02 5.6381e-05 4.7040e-01 5.54039e-02 1.0000 4.7040e-01 3\n",
" 15 2.8597e+00 7.6699e-02 1.5237e-05 2.7828e-01 2.29065e-01 0.0312 2.7828e-01 3\n",
" 16 2.8185e+00 7.6698e-02 1.5042e-05 2.7417e-01 1.92679e-01 0.0625 2.7417e-01 3\n",
" 17 2.8104e+00 7.6697e-02 1.4913e-05 2.7336e-01 1.30125e-01 0.1250 2.7336e-01 3\n",
" 18 2.7247e+00 7.6696e-02 1.4535e-05 2.6478e-01 4.50507e-02 1.0000 2.6478e-01 3\n",
" 19 2.2415e+00 7.6699e-02 8.9713e-06 2.1647e-01 1.79458e-01 0.0625 2.1647e-01 3\n",
" 20 2.2157e+00 7.6697e-02 9.0451e-06 2.1389e-01 1.22256e-01 0.1250 2.1389e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.2020e+00 7.6697e-02 9.0337e-06 2.1252e-01 4.43645e-02 0.5000 2.1252e-01 3\n",
" 22 1.5137e+00 7.6697e-02 6.3876e-06 1.4370e-01 6.53629e-02 0.2500 1.4370e-01 3\n",
" 23 1.4407e+00 7.6697e-02 5.9740e-06 1.3640e-01 1.85300e-02 1.0000 1.3640e-01 3\n",
" 24 4.6586e-01 7.6698e-02 3.3407e-06 3.8913e-02 8.33198e-02 0.0312 3.8913e-02 3\n",
" 25 4.6087e-01 7.6698e-02 3.2940e-06 3.8413e-02 6.87215e-02 0.0625 3.8413e-02 3\n",
" 26 4.5907e-01 7.6698e-02 3.2473e-06 3.8234e-02 4.45433e-02 0.1250 3.8234e-02 3\n",
" 27 4.3851e-01 7.6698e-02 3.1110e-06 3.6178e-02 1.33521e-02 1.0000 3.6178e-02 3\n",
" END 1.5031e-01 7.6698e-02 1.3602e-06 7.3603e-03 ---- ---- 7.3603e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8112e+01 8.2039e-02 3.5955e+00 2.0745e-01 5.56630e-01 0.5000 ---- 3\n",
" 2 3.1170e+01 7.7166e-02 1.8243e+00 1.2849e+00 3.93324e-01 0.5000 1.2849e+00 3\n",
" 3 2.1691e+01 7.6202e-02 9.1881e-01 1.2427e+00 3.72357e-01 0.2500 1.2427e+00 3\n",
" 4 2.1545e+01 7.6108e-02 6.8942e-01 1.4574e+00 2.20590e-01 1.0000 1.4574e+00 3\n",
" 5 1.4338e+01 7.6224e-02 5.1341e-03 1.4211e+00 6.54473e-01 0.0312 1.4211e+00 3\n",
" 6 1.4070e+01 7.6220e-02 4.9770e-03 1.3944e+00 5.34313e-01 0.0625 1.3944e+00 3\n",
" 7 1.3919e+01 7.6214e-02 4.6744e-03 1.3796e+00 3.37471e-01 0.1250 1.3796e+00 3\n",
" 8 1.3367e+01 7.6210e-02 4.1011e-03 1.3250e+00 8.42798e-02 1.0000 1.3250e+00 3\n",
" 9 6.8634e+00 7.6226e-02 1.5641e-05 6.7870e-01 3.83637e-01 0.0312 6.7870e-01 3\n",
" 10 6.8296e+00 7.6223e-02 1.5340e-05 6.7532e-01 3.24759e-01 0.0312 6.7532e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 6.6961e+00 7.6221e-02 1.5082e-05 6.6197e-01 2.71660e-01 0.0625 6.6197e-01 3\n",
" 12 6.5708e+00 7.6218e-02 1.5099e-05 6.4945e-01 1.76516e-01 0.1250 6.4945e-01 3\n",
" 13 6.0819e+00 7.6217e-02 1.6307e-05 6.0056e-01 4.70109e-02 1.0000 6.0056e-01 3\n",
" 14 3.3886e-01 7.6224e-02 3.1730e-05 2.6232e-02 2.18773e-01 0.0156 2.6232e-02 3\n",
" 15 3.3696e-01 7.6224e-02 3.1373e-05 2.6042e-02 1.98215e-01 0.0156 2.6042e-02 3\n",
" 16 3.3432e-01 7.6223e-02 3.0995e-05 2.5779e-02 1.79516e-01 0.0312 2.5779e-02 3\n",
" 17 3.3286e-01 7.6223e-02 3.0385e-05 2.5633e-02 1.45497e-01 0.0312 2.5633e-02 3\n",
" 18 3.2801e-01 7.6222e-02 2.9662e-05 2.5149e-02 1.17786e-01 0.0625 2.5149e-02 3\n",
" 19 3.2234e-01 7.6222e-02 2.8382e-05 2.4584e-02 7.28946e-02 0.1250 2.4584e-02 3\n",
" 20 3.0458e-01 7.6221e-02 2.5655e-05 2.2811e-02 1.79816e-02 1.0000 2.2811e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.4243e-01 7.6221e-02 2.5364e-06 1.6618e-02 8.55360e-02 0.0312 1.6618e-02 2\n",
" 22 2.3878e-01 7.6221e-02 2.5205e-06 1.6253e-02 6.96513e-02 0.0625 1.6253e-02 2\n",
" 23 2.3563e-01 7.6221e-02 2.5336e-06 1.5939e-02 4.37397e-02 0.1250 1.5939e-02 2\n",
" 24 2.2221e-01 7.6221e-02 2.4914e-06 1.4596e-02 1.56254e-02 0.0020 1.4596e-02 1\n",
" 25 2.2221e-01 7.6221e-02 2.4914e-06 1.4596e-02 1.56232e-02 0.5000 1.4596e-02 1\n",
" 26 1.8820e-01 7.6221e-02 4.7295e-06 1.1193e-02 2.63005e-02 0.0625 1.1193e-02 1\n",
" 27 1.8760e-01 7.6221e-02 4.4036e-06 1.1133e-02 1.73608e-02 0.2500 1.1133e-02 1\n",
" 28 1.8227e-01 7.6221e-02 2.9469e-06 1.0602e-02 7.48251e-03 1.0000 1.0602e-02 1\n",
" END 1.7449e-01 7.6221e-02 3.1067e-06 9.8234e-03 ---- ---- 9.8234e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7826e+01 8.1549e-02 3.5695e+00 2.0492e-01 5.49864e-01 0.5000 ---- 3\n",
" 2 2.9665e+01 7.6757e-02 1.8081e+00 1.1507e+00 3.84583e-01 1.0000 1.1507e+00 3\n",
" 3 2.1759e+01 7.5894e-02 2.8238e-02 2.1401e+00 7.50547e-01 0.0625 2.1401e+00 3\n",
" 4 2.1459e+01 7.5885e-02 2.6484e-02 2.1118e+00 4.84171e-01 0.1250 2.1118e+00 3\n",
" 5 2.0514e+01 7.5879e-02 2.3184e-02 2.0206e+00 1.37653e-01 1.0000 2.0206e+00 3\n",
" 6 1.1087e+01 7.5901e-02 1.0960e-04 1.1010e+00 5.70008e-01 0.0312 1.1010e+00 3\n",
" 7 1.0998e+01 7.5897e-02 1.0496e-04 1.0921e+00 4.83333e-01 0.0312 1.0921e+00 3\n",
" 8 1.0759e+01 7.5893e-02 1.0065e-04 1.0682e+00 4.05105e-01 0.0625 1.0682e+00 3\n",
" 9 1.0494e+01 7.5890e-02 9.1179e-05 1.0417e+00 2.65817e-01 0.1250 1.0417e+00 3\n",
" 10 9.6328e+00 7.5888e-02 7.4294e-05 9.5562e-01 7.71279e-02 1.0000 9.5562e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.6632e+00 7.5899e-02 5.0485e-05 1.5868e-01 3.32882e-01 0.0312 1.5868e-01 3\n",
" 12 1.6415e+00 7.5897e-02 4.9727e-05 1.5651e-01 2.72936e-01 0.0312 1.5651e-01 3\n",
" 13 1.6036e+00 7.5896e-02 4.8704e-05 1.5273e-01 2.23713e-01 0.0625 1.5273e-01 3\n",
" 14 1.5623e+00 7.5895e-02 4.7038e-05 1.4859e-01 1.43084e-01 0.1250 1.4859e-01 3\n",
" 15 1.4740e+00 7.5894e-02 4.3258e-05 1.3977e-01 4.12013e-02 1.0000 1.3977e-01 3\n",
" 16 1.1488e+00 7.5895e-02 9.0071e-06 1.0729e-01 1.78639e-01 0.0312 1.0729e-01 3\n",
" 17 1.1227e+00 7.5895e-02 8.8959e-06 1.0467e-01 1.47691e-01 0.0625 1.0467e-01 3\n",
" 18 1.0990e+00 7.5894e-02 8.8012e-06 1.0230e-01 9.65015e-02 0.1250 1.0230e-01 3\n",
" 19 1.0496e+00 7.5894e-02 8.4584e-06 9.7361e-02 3.03982e-02 1.0000 9.7361e-02 3\n",
" 20 9.6092e-01 7.5895e-02 3.7865e-06 8.8499e-02 1.27154e-01 0.0625 8.8499e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 9.5990e-01 7.5894e-02 3.8124e-06 8.8397e-02 8.47641e-02 0.1250 8.8397e-02 3\n",
" 22 9.5501e-01 7.5894e-02 3.7846e-06 8.7907e-02 2.88671e-02 0.5000 8.7907e-02 3\n",
" 23 6.9000e-01 7.5894e-02 2.5747e-06 6.1408e-02 4.40614e-02 0.2500 6.1408e-02 3\n",
" 24 6.5828e-01 7.5894e-02 2.3078e-06 5.8236e-02 1.38822e-02 1.0000 5.8236e-02 3\n",
" 25 2.0579e-01 7.5894e-02 1.5704e-06 1.2988e-02 6.65299e-02 0.0020 1.2988e-02 1\n",
" 26 2.0579e-01 7.5894e-02 1.5704e-06 1.2988e-02 6.65399e-02 0.0312 1.2988e-02 1\n",
" 27 2.0571e-01 7.5894e-02 1.6204e-06 1.2980e-02 5.45871e-02 0.0156 1.2980e-02 1\n",
" 28 2.0560e-01 7.5894e-02 1.6136e-06 1.2969e-02 4.97399e-02 0.0312 1.2969e-02 1\n",
" 29 2.0469e-01 7.5894e-02 1.6276e-06 1.2878e-02 4.09409e-02 0.0312 1.2878e-02 1\n",
" 30 2.0397e-01 7.5894e-02 1.6251e-06 1.2805e-02 3.38059e-02 0.0625 1.2805e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.0241e-01 7.5894e-02 1.6700e-06 1.2650e-02 2.23418e-02 0.1250 1.2650e-02 1\n",
" 32 1.9909e-01 7.5894e-02 1.7787e-06 1.2317e-02 8.31764e-03 1.0000 1.2317e-02 1\n",
" END 1.6846e-01 7.5894e-02 4.2704e-06 9.2527e-03 ---- ---- 9.2527e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7639e+01 8.1212e-02 3.5507e+00 2.0505e-01 5.36088e-01 0.5000 ---- 3\n",
" 2 2.9722e+01 7.6477e-02 1.7962e+00 1.1683e+00 3.51525e-01 1.0000 1.1683e+00 3\n",
" 3 1.2861e+01 7.5673e-02 2.4979e-02 1.2536e+00 5.72262e-01 0.0625 1.2536e+00 3\n",
" 4 1.2683e+01 7.5668e-02 2.3422e-02 1.2373e+00 3.66700e-01 0.1250 1.2373e+00 3\n",
" 5 1.2039e+01 7.5664e-02 2.0497e-02 1.1758e+00 1.05377e-01 1.0000 1.1758e+00 3\n",
" 6 5.4681e+00 7.5676e-02 1.0661e-04 5.3913e-01 4.30407e-01 0.0312 5.3913e-01 3\n",
" 7 5.4019e+00 7.5673e-02 1.0245e-04 5.3252e-01 3.57182e-01 0.0312 5.3252e-01 3\n",
" 8 5.2718e+00 7.5672e-02 9.8645e-05 5.1951e-01 2.95381e-01 0.0625 5.1951e-01 3\n",
" 9 5.1163e+00 7.5670e-02 9.0770e-05 5.0397e-01 1.91450e-01 0.1250 5.0397e-01 3\n",
" 10 4.7144e+00 7.5669e-02 7.6447e-05 4.6380e-01 5.66614e-02 1.0000 4.6380e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.7840e+00 7.5674e-02 1.8986e-05 1.7081e-01 2.39737e-01 0.0312 1.7081e-01 3\n",
" 12 1.7464e+00 7.5673e-02 1.8687e-05 1.6706e-01 1.98346e-01 0.0625 1.6706e-01 3\n",
" 13 1.7139e+00 7.5672e-02 1.8307e-05 1.6380e-01 1.29854e-01 0.1250 1.6380e-01 3\n",
" 14 1.6451e+00 7.5671e-02 1.7309e-05 1.5693e-01 4.10768e-02 0.5000 1.5693e-01 3\n",
" 15 1.0527e+00 7.5671e-02 1.0354e-05 9.7688e-02 6.57178e-02 0.1250 9.7688e-02 3\n",
" 16 9.5594e-01 7.5671e-02 9.3243e-06 8.8018e-02 2.11084e-02 1.0000 8.8018e-02 3\n",
" 17 4.1373e-01 7.5672e-02 1.4831e-06 3.3805e-02 8.81865e-02 0.0312 3.3805e-02 2\n",
" 18 4.0486e-01 7.5672e-02 1.4698e-06 3.2918e-02 7.35601e-02 0.0625 3.2918e-02 2\n",
" 19 3.9792e-01 7.5671e-02 1.4663e-06 3.2223e-02 4.92399e-02 0.1250 3.2223e-02 2\n",
" 20 3.8047e-01 7.5671e-02 1.4246e-06 3.0479e-02 1.70108e-02 0.5000 3.0479e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.2152e-01 7.5671e-02 8.6117e-07 1.4584e-02 2.89300e-02 0.0020 1.4584e-02 1\n",
" 22 2.2152e-01 7.5671e-02 8.6117e-07 1.4584e-02 2.89096e-02 0.0625 1.4584e-02 1\n",
" 23 2.1711e-01 7.5671e-02 8.8552e-07 1.4143e-02 2.04418e-02 0.0625 1.4143e-02 1\n",
" 24 2.1617e-01 7.5671e-02 8.8314e-07 1.4049e-02 1.48964e-02 0.2500 1.4049e-02 1\n",
" 25 2.0806e-01 7.5671e-02 1.2450e-06 1.3238e-02 7.25482e-03 1.0000 1.3238e-02 1\n",
" END 1.5396e-01 7.5671e-02 3.0727e-06 7.8260e-03 ---- ---- 7.8260e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7718e+01 8.0980e-02 3.5542e+00 2.0953e-01 5.53070e-01 0.5000 ---- 3\n",
" 2 2.9989e+01 7.6613e-02 1.7881e+00 1.2030e+00 3.14401e-01 1.0000 1.2030e+00 3\n",
" 3 6.2593e+00 7.6203e-02 1.2981e-02 6.0533e-01 3.27082e-01 0.0625 6.0533e-01 3\n",
" 4 6.1962e+00 7.6200e-02 1.2170e-02 5.9983e-01 2.13989e-01 0.1250 5.9983e-01 3\n",
" 5 5.9423e+00 7.6198e-02 1.0648e-02 5.7597e-01 7.01223e-02 1.0000 5.7597e-01 3\n",
" 6 4.7120e+00 7.6202e-02 9.4291e-05 4.6349e-01 2.45475e-01 0.0625 4.6349e-01 3\n",
" 7 4.6508e+00 7.6200e-02 8.7661e-05 4.5738e-01 1.64580e-01 0.1250 4.5738e-01 3\n",
" 8 4.4954e+00 7.6198e-02 7.5375e-05 4.4184e-01 5.55193e-02 1.0000 4.4184e-01 3\n",
" 9 4.3811e+00 7.6201e-02 1.2900e-05 4.3048e-01 2.18329e-01 0.0312 4.3048e-01 3\n",
" 10 4.2778e+00 7.6200e-02 1.2655e-05 4.2015e-01 1.84907e-01 0.0625 4.2015e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 4.1762e+00 7.6199e-02 1.2328e-05 4.0999e-01 1.26756e-01 0.1250 4.0999e-01 3\n",
" 12 3.9567e+00 7.6198e-02 1.1674e-05 3.8804e-01 4.60718e-02 1.0000 3.8804e-01 3\n",
" 13 3.1370e+00 7.6201e-02 7.1404e-06 3.0607e-01 1.80179e-01 0.0312 3.0607e-01 3\n",
" 14 3.0691e+00 7.6200e-02 7.0098e-06 2.9928e-01 1.52934e-01 0.0625 2.9928e-01 3\n",
" 15 3.0119e+00 7.6199e-02 6.8422e-06 2.9357e-01 1.05739e-01 0.1250 2.9357e-01 3\n",
" 16 2.8858e+00 7.6198e-02 6.5208e-06 2.8095e-01 3.97106e-02 1.0000 2.8095e-01 3\n",
" 17 2.7046e+00 7.6200e-02 4.3786e-06 2.6284e-01 1.52512e-01 0.0312 2.6284e-01 3\n",
" 18 2.6406e+00 7.6200e-02 4.2988e-06 2.5644e-01 1.29838e-01 0.0625 2.5644e-01 3\n",
" 19 2.5749e+00 7.6199e-02 4.1952e-06 2.4986e-01 9.04485e-02 0.1250 2.4986e-01 3\n",
" 20 2.4345e+00 7.6198e-02 3.9911e-06 2.3582e-01 3.50586e-02 1.0000 2.3582e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.9603e+00 7.6200e-02 2.7776e-06 1.8841e-01 1.34636e-01 0.0312 1.8841e-01 3\n",
" 22 1.9151e+00 7.6200e-02 2.7284e-06 1.8388e-01 1.14283e-01 0.0625 1.8388e-01 3\n",
" 23 1.8712e+00 7.6199e-02 2.6629e-06 1.7950e-01 7.94240e-02 0.1250 1.7950e-01 3\n",
" 24 1.7843e+00 7.6199e-02 2.5162e-06 1.7081e-01 3.07609e-02 1.0000 1.7081e-01 3\n",
" 25 1.7613e+00 7.6200e-02 1.6761e-06 1.6851e-01 1.17144e-01 0.0625 1.6851e-01 3\n",
" 26 1.7525e+00 7.6199e-02 1.6497e-06 1.6763e-01 8.19643e-02 0.1250 1.6763e-01 3\n",
" 27 1.7213e+00 7.6199e-02 1.6153e-06 1.6451e-01 3.24255e-02 0.5000 1.6451e-01 3\n",
" 28 1.1487e+00 7.6199e-02 1.3593e-06 1.0725e-01 4.53225e-02 0.2500 1.0725e-01 3\n",
" 29 1.0860e+00 7.6199e-02 1.2337e-06 1.0098e-01 1.05802e-02 1.0000 1.0098e-01 3\n",
" END 1.6302e-01 7.6200e-02 7.5255e-07 8.6809e-03 ---- ---- 8.6809e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8048e+01 8.1533e-02 3.5946e+00 2.0205e-01 5.60654e-01 0.5000 ---- 3\n",
" 2 3.1125e+01 7.7183e-02 1.8085e+00 1.2963e+00 3.34754e-01 1.0000 1.2963e+00 3\n",
" 3 1.4414e+01 7.6789e-02 1.3287e-02 1.4204e+00 4.48979e-01 0.0625 1.4204e+00 3\n",
" 4 1.4222e+01 7.6783e-02 1.2456e-02 1.4021e+00 3.01413e-01 0.1250 1.4021e+00 3\n",
" 5 1.3602e+01 7.6779e-02 1.0898e-02 1.3417e+00 9.56733e-02 1.0000 1.3417e+00 3\n",
" 6 8.1135e+00 7.6791e-02 1.7838e-04 8.0349e-01 3.55417e-01 0.0312 8.0349e-01 3\n",
" 7 7.9863e+00 7.6789e-02 1.7284e-04 7.9078e-01 3.00217e-01 0.0625 7.9078e-01 3\n",
" 8 7.9768e+00 7.6785e-02 1.6196e-04 7.8984e-01 2.01467e-01 0.1250 7.8984e-01 3\n",
" 9 7.7887e+00 7.6783e-02 1.4085e-04 7.7105e-01 6.37120e-02 1.0000 7.7105e-01 3\n",
" 10 4.6933e+00 7.6790e-02 2.2150e-05 4.6162e-01 2.61665e-01 0.0312 4.6162e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 4.6399e+00 7.6788e-02 2.1786e-05 4.5629e-01 2.20111e-01 0.0312 4.5629e-01 3\n",
" 12 4.5357e+00 7.6787e-02 2.1335e-05 4.4587e-01 1.84158e-01 0.0625 4.4587e-01 3\n",
" 13 4.4322e+00 7.6785e-02 2.0635e-05 4.3552e-01 1.22466e-01 0.1250 4.3552e-01 3\n",
" 14 4.1626e+00 7.6785e-02 1.9138e-05 4.0856e-01 3.98635e-02 1.0000 4.0856e-01 3\n",
" 15 1.9919e+00 7.6788e-02 6.0767e-06 1.9151e-01 1.60303e-01 0.0312 1.9151e-01 3\n",
" 16 1.9682e+00 7.6788e-02 5.9908e-06 1.8913e-01 1.34948e-01 0.0312 1.8913e-01 3\n",
" 17 1.9239e+00 7.6787e-02 5.8859e-06 1.8470e-01 1.13257e-01 0.0625 1.8470e-01 3\n",
" 18 1.8774e+00 7.6787e-02 5.7702e-06 1.8005e-01 7.63853e-02 0.1250 1.8005e-01 3\n",
" 19 1.7639e+00 7.6786e-02 5.5678e-06 1.6871e-01 2.70167e-02 1.0000 1.6871e-01 3\n",
" 20 9.2035e-01 7.6788e-02 5.5347e-06 8.4351e-02 1.07088e-01 0.0312 8.4351e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 9.0523e-01 7.6787e-02 5.4389e-06 8.2838e-02 8.98412e-02 0.0625 8.2838e-02 3\n",
" 22 9.0302e-01 7.6787e-02 5.3175e-06 8.2618e-02 6.08262e-02 0.1250 8.2618e-02 3\n",
" 23 9.0131e-01 7.6787e-02 5.0571e-06 8.2448e-02 2.20489e-02 1.0000 8.2448e-02 3\n",
" 24 8.4000e-01 7.6787e-02 3.4776e-06 7.6318e-02 8.50681e-02 0.0312 7.6318e-02 3\n",
" 25 8.2328e-01 7.6787e-02 3.4165e-06 7.4646e-02 7.18541e-02 0.0625 7.4646e-02 3\n",
" 26 8.0785e-01 7.6787e-02 3.3415e-06 7.3103e-02 4.95008e-02 0.1250 7.3103e-02 3\n",
" 27 7.7013e-01 7.6787e-02 3.1920e-06 6.9331e-02 1.89872e-02 1.0000 6.9331e-02 3\n",
" 28 5.7198e-01 7.6787e-02 2.4198e-06 4.9517e-02 7.26496e-02 0.0312 4.9517e-02 3\n",
" 29 5.6092e-01 7.6787e-02 2.3784e-06 4.8411e-02 6.14537e-02 0.0625 4.8411e-02 3\n",
" 30 5.5033e-01 7.6787e-02 2.3277e-06 4.7352e-02 4.25127e-02 0.1250 4.7352e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 5.3183e-01 7.6787e-02 2.2232e-06 4.5503e-02 1.65264e-02 1.0000 4.5503e-02 3\n",
" 32 4.7412e-01 7.6787e-02 1.7388e-06 3.9732e-02 6.22883e-02 0.0625 3.9732e-02 2\n",
" 33 4.6204e-01 7.6787e-02 1.7264e-06 3.8524e-02 4.33206e-02 0.1250 3.8524e-02 3\n",
" 34 4.6198e-01 7.6787e-02 1.6935e-06 3.8518e-02 1.71518e-02 0.5000 3.8518e-02 3\n",
" 35 3.4159e-01 7.6787e-02 1.2787e-06 2.6479e-02 2.37345e-02 0.2500 2.6479e-02 2\n",
" 36 2.9530e-01 7.6787e-02 1.1500e-06 2.1850e-02 5.59114e-03 1.0000 2.1850e-02 2\n",
" END 9.0921e-02 7.6787e-02 2.1054e-07 1.4131e-03 ---- ---- 2.5649e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8173e+01 8.2133e-02 3.6090e+00 2.0011e-01 5.38005e-01 0.5000 ---- 3\n",
" 2 3.0473e+01 7.7286e-02 1.8299e+00 1.2097e+00 3.37953e-01 1.0000 1.2097e+00 3\n",
" 3 1.4393e+01 7.6372e-02 3.0118e-02 1.4016e+00 4.75636e-01 0.0625 1.4016e+00 3\n",
" 4 1.4034e+01 7.6366e-02 2.8238e-02 1.3675e+00 3.13812e-01 0.1250 1.3675e+00 3\n",
" 5 1.3249e+01 7.6362e-02 2.4709e-02 1.2926e+00 9.90221e-02 1.0000 1.2926e+00 3\n",
" 6 1.0182e+01 7.6374e-02 1.0875e-04 1.0105e+00 3.48833e-01 0.0312 1.0105e+00 3\n",
" 7 9.9597e+00 7.6371e-02 1.0493e-04 9.8823e-01 3.01199e-01 0.0625 9.8823e-01 3\n",
" 8 9.7900e+00 7.6367e-02 9.6909e-05 9.7127e-01 2.11558e-01 0.1250 9.7127e-01 3\n",
" 9 9.3381e+00 7.6364e-02 8.0930e-05 9.2609e-01 7.41118e-02 1.0000 9.2609e-01 3\n",
" 10 5.2146e+00 7.6373e-02 5.2957e-05 5.1377e-01 2.97783e-01 0.0312 5.1377e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 5.1679e+00 7.6371e-02 5.2192e-05 5.0910e-01 2.51262e-01 0.0312 5.0910e-01 3\n",
" 12 5.0625e+00 7.6370e-02 5.1167e-05 4.9856e-01 2.10824e-01 0.0625 4.9856e-01 3\n",
" 13 4.9810e+00 7.6368e-02 4.9593e-05 4.9041e-01 1.40990e-01 0.1250 4.9041e-01 3\n",
" 14 4.7383e+00 7.6367e-02 4.5975e-05 4.6615e-01 4.65532e-02 1.0000 4.6615e-01 3\n",
" 15 2.2273e+00 7.6371e-02 1.0772e-05 2.1508e-01 1.91721e-01 0.0312 2.1508e-01 3\n",
" 16 2.2160e+00 7.6370e-02 1.0655e-05 2.1396e-01 1.61712e-01 0.0312 2.1396e-01 3\n",
" 17 2.1742e+00 7.6370e-02 1.0486e-05 2.0977e-01 1.35961e-01 0.0625 2.0977e-01 3\n",
" 18 2.1407e+00 7.6369e-02 1.0302e-05 2.0643e-01 9.17758e-02 0.1250 2.0643e-01 3\n",
" 19 2.0268e+00 7.6368e-02 9.8662e-06 1.9503e-01 3.17588e-02 1.0000 1.9503e-01 3\n",
" 20 1.1870e+00 7.6370e-02 4.8827e-06 1.1106e-01 1.27754e-01 0.0625 1.1106e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.1806e+00 7.6369e-02 4.9287e-06 1.1042e-01 8.63386e-02 0.1250 1.1042e-01 3\n",
" 22 1.1738e+00 7.6369e-02 4.8952e-06 1.0974e-01 3.12530e-02 0.5000 1.0974e-01 3\n",
" 23 8.4875e-01 7.6369e-02 3.2159e-06 7.7235e-02 4.57863e-02 0.2500 7.7235e-02 3\n",
" 24 8.2285e-01 7.6369e-02 2.9973e-06 7.4645e-02 1.35217e-02 1.0000 7.4645e-02 3\n",
" 25 3.1239e-01 7.6370e-02 1.7401e-06 2.3600e-02 6.09607e-02 0.0312 2.3600e-02 3\n",
" 26 3.1178e-01 7.6370e-02 1.7170e-06 2.3540e-02 5.02553e-02 0.0312 2.3540e-02 3\n",
" 27 3.0825e-01 7.6369e-02 1.6848e-06 2.3186e-02 4.14170e-02 0.0625 2.3186e-02 3\n",
" 28 3.0605e-01 7.6369e-02 1.6377e-06 2.2967e-02 2.68566e-02 0.1250 2.2967e-02 3\n",
" 29 2.9389e-01 7.6369e-02 1.5286e-06 2.1751e-02 8.09277e-03 1.0000 2.1751e-02 3\n",
" END 1.6128e-01 7.6370e-02 4.9724e-07 8.4907e-03 ---- ---- 8.4907e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7915e+01 8.1703e-02 3.5859e+00 1.9750e-01 5.37407e-01 0.5000 ---- 3\n",
" 2 3.0449e+01 7.6928e-02 1.8153e+00 1.2219e+00 3.58301e-01 1.0000 1.2219e+00 3\n",
" 3 2.0254e+01 7.6086e-02 2.6808e-02 1.9909e+00 5.92889e-01 0.0625 1.9909e+00 3\n",
" 4 1.9825e+01 7.6079e-02 2.5138e-02 1.9497e+00 3.88178e-01 0.1250 1.9497e+00 3\n",
" 5 1.8825e+01 7.6073e-02 2.2002e-02 1.8529e+00 1.15686e-01 1.0000 1.8529e+00 3\n",
" 6 1.2854e+01 7.6092e-02 1.0373e-04 1.2777e+00 4.27429e-01 0.0312 1.2777e+00 3\n",
" 7 1.2668e+01 7.6088e-02 9.9899e-05 1.2590e+00 3.70904e-01 0.0625 1.2590e+00 3\n",
" 8 1.2667e+01 7.6082e-02 9.1441e-05 1.2590e+00 2.59879e-01 0.1250 1.2590e+00 3\n",
" 9 1.2234e+01 7.6078e-02 7.4123e-05 1.2157e+00 8.48544e-02 1.0000 1.2157e+00 3\n",
" 10 4.4177e+00 7.6091e-02 7.8194e-05 4.3409e-01 3.55775e-01 0.0156 4.3409e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 4.3692e+00 7.6090e-02 7.7299e-05 4.2923e-01 3.26020e-01 0.0312 4.2923e-01 3\n",
" 12 4.3548e+00 7.6088e-02 7.5954e-05 4.2779e-01 2.70782e-01 0.0312 4.2779e-01 3\n",
" 13 4.2789e+00 7.6086e-02 7.4278e-05 4.2021e-01 2.24118e-01 0.0625 4.2021e-01 3\n",
" 14 4.2554e+00 7.6084e-02 7.1452e-05 4.1786e-01 1.45432e-01 0.1250 4.1786e-01 3\n",
" 15 4.0869e+00 7.6083e-02 6.5227e-05 4.0102e-01 4.35997e-02 1.0000 4.0102e-01 3\n",
" 16 1.4327e+00 7.6087e-02 9.3421e-06 1.3565e-01 1.86928e-01 0.0156 1.3565e-01 3\n",
" 17 1.4165e+00 7.6087e-02 9.2463e-06 1.3404e-01 1.71262e-01 0.0312 1.3404e-01 3\n",
" 18 1.4074e+00 7.6086e-02 9.1263e-06 1.3313e-01 1.42305e-01 0.0312 1.3313e-01 3\n",
" 19 1.3800e+00 7.6086e-02 8.9568e-06 1.3039e-01 1.18151e-01 0.0625 1.3039e-01 3\n",
" 20 1.3546e+00 7.6085e-02 8.7105e-06 1.2784e-01 7.79448e-02 0.1250 1.2784e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.2778e+00 7.6085e-02 8.1231e-06 1.2017e-01 2.55778e-02 1.0000 1.2017e-01 3\n",
" 22 8.8560e-01 7.6086e-02 2.2272e-06 8.0949e-02 1.03756e-01 0.0625 8.0949e-02 3\n",
" 23 8.7231e-01 7.6086e-02 2.2396e-06 7.9621e-02 6.96130e-02 0.1250 7.9621e-02 3\n",
" 24 8.4826e-01 7.6085e-02 2.2125e-06 7.7215e-02 2.48002e-02 0.5000 7.7215e-02 3\n",
" 25 5.9860e-01 7.6086e-02 1.4657e-06 5.2249e-02 3.60804e-02 0.2500 5.2249e-02 3\n",
" 26 5.6016e-01 7.6086e-02 1.3107e-06 4.8407e-02 1.08647e-02 1.0000 4.8407e-02 3\n",
" 27 1.8115e-01 7.6086e-02 9.8367e-07 1.0506e-02 5.28121e-02 0.0020 1.0506e-02 1\n",
" 28 1.8115e-01 7.6086e-02 9.8367e-07 1.0506e-02 5.28012e-02 0.0156 1.0506e-02 1\n",
" 29 1.8068e-01 7.6086e-02 9.8510e-07 1.0459e-02 4.82245e-02 0.0078 1.0459e-02 1\n",
" 30 1.8062e-01 7.6086e-02 9.8106e-07 1.0452e-02 4.59509e-02 0.0156 1.0452e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.8027e-01 7.6086e-02 9.7929e-07 1.0417e-02 4.20253e-02 0.0156 1.0417e-02 1\n",
" 32 1.8014e-01 7.6086e-02 9.7582e-07 1.0404e-02 3.84658e-02 0.0156 1.0404e-02 1\n",
" 33 1.7965e-01 7.6086e-02 9.7092e-07 1.0356e-02 3.52292e-02 0.0156 1.0356e-02 1\n",
" 34 1.7920e-01 7.6086e-02 9.6479e-07 1.0310e-02 3.22973e-02 0.0312 1.0310e-02 1\n",
" 35 1.7879e-01 7.6086e-02 9.6633e-07 1.0270e-02 2.69781e-02 0.0312 1.0270e-02 1\n",
" 36 1.7794e-01 7.6086e-02 9.6018e-07 1.0184e-02 2.26417e-02 0.0625 1.0184e-02 1\n",
" 37 1.7689e-01 7.6086e-02 9.7353e-07 1.0079e-02 1.55692e-02 0.1250 1.0079e-02 1\n",
" END 1.7391e-01 7.6086e-02 1.0142e-06 9.7811e-03 ---- ---- 9.7811e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7770e+01 8.1412e-02 3.5690e+00 1.9987e-01 5.28754e-01 0.5000 ---- 3\n",
" 2 2.9914e+01 7.6686e-02 1.8046e+00 1.1791e+00 3.21903e-01 1.0000 1.1791e+00 3\n",
" 3 7.1989e+00 7.5896e-02 2.3706e-02 6.8859e-01 4.21599e-01 0.0625 6.8859e-01 3\n",
" 4 7.1535e+00 7.5893e-02 2.2225e-02 6.8554e-01 2.64540e-01 0.1250 6.8554e-01 3\n",
" 5 6.8189e+00 7.5891e-02 1.9446e-02 6.5485e-01 7.28566e-02 1.0000 6.5485e-01 3\n",
" 6 3.9907e+00 7.5896e-02 8.7091e-05 3.9139e-01 2.75390e-01 0.0312 3.9139e-01 3\n",
" 7 3.8977e+00 7.5895e-02 8.4077e-05 3.8210e-01 2.27235e-01 0.0625 3.8210e-01 3\n",
" 8 3.7988e+00 7.5894e-02 7.7998e-05 3.7221e-01 1.46738e-01 0.1250 3.7221e-01 3\n",
" 9 3.5505e+00 7.5893e-02 6.6821e-05 3.4740e-01 4.35459e-02 1.0000 3.4740e-01 3\n",
" 10 1.7169e+00 7.5896e-02 9.5491e-06 1.6409e-01 1.83030e-01 0.0312 1.6409e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.6993e+00 7.5895e-02 9.3996e-06 1.6233e-01 1.52212e-01 0.0312 1.6233e-01 3\n",
" 12 1.6635e+00 7.5895e-02 9.2071e-06 1.5875e-01 1.26493e-01 0.0625 1.5875e-01 3\n",
" 13 1.6309e+00 7.5894e-02 8.9065e-06 1.5550e-01 8.35631e-02 0.1250 1.5550e-01 3\n",
" 14 1.5514e+00 7.5894e-02 8.2585e-06 1.4754e-01 2.75798e-02 1.0000 1.4754e-01 3\n",
" 15 1.0022e+00 7.5895e-02 3.5450e-06 9.2628e-02 1.11877e-01 0.0312 9.2628e-02 3\n",
" 16 9.8572e-01 7.5895e-02 3.4749e-06 9.0979e-02 9.35792e-02 0.0625 9.0979e-02 3\n",
" 17 9.7736e-01 7.5894e-02 3.3733e-06 9.0144e-02 6.28952e-02 0.1250 9.0144e-02 3\n",
" 18 9.4896e-01 7.5894e-02 3.1668e-06 8.7303e-02 2.21446e-02 1.0000 8.7303e-02 3\n",
" 19 8.8529e-01 7.5894e-02 1.9111e-06 8.0937e-02 8.81174e-02 0.0625 8.0937e-02 3\n",
" 20 8.8291e-01 7.5894e-02 1.8560e-06 8.0700e-02 5.98490e-02 0.1250 8.0700e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 8.7014e-01 7.5894e-02 1.7786e-06 7.9423e-02 2.19127e-02 0.5000 7.9423e-02 3\n",
" 22 5.9239e-01 7.5894e-02 1.4804e-06 5.1648e-02 3.18366e-02 0.2500 5.1648e-02 3\n",
" 23 5.6398e-01 7.5894e-02 1.2675e-06 4.8807e-02 8.99503e-03 1.0000 4.8807e-02 3\n",
" 24 2.1420e-01 7.5894e-02 5.7636e-07 1.3830e-02 4.04630e-02 0.0312 1.3830e-02 2\n",
" 25 2.0982e-01 7.5894e-02 5.6857e-07 1.3392e-02 3.34104e-02 0.0625 1.3392e-02 2\n",
" 26 2.0404e-01 7.5894e-02 5.6061e-07 1.2814e-02 2.17640e-02 0.1250 1.2814e-02 2\n",
" 27 1.8937e-01 7.5894e-02 5.3534e-07 1.1347e-02 6.68091e-03 1.0000 1.1347e-02 2\n",
" END 8.7587e-02 7.5894e-02 3.0158e-07 1.1690e-03 ---- ---- 1.1690e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7784e+01 8.1210e-02 3.5760e+00 1.9422e-01 5.56836e-01 0.5000 ---- 3\n",
" 2 3.0576e+01 7.6826e-02 1.7991e+00 1.2508e+00 3.53715e-01 1.0000 1.2508e+00 3\n",
" 3 1.7816e+01 7.6403e-02 1.3103e-02 1.7608e+00 5.26643e-01 0.0625 1.7608e+00 3\n",
" 4 1.7441e+01 7.6397e-02 1.2286e-02 1.7242e+00 3.45145e-01 0.1250 1.7242e+00 3\n",
" 5 1.6579e+01 7.6392e-02 1.0751e-02 1.6395e+00 1.02273e-01 1.0000 1.6395e+00 3\n",
" 6 1.2676e+01 7.6407e-02 1.1850e-04 1.2598e+00 3.72256e-01 0.0312 1.2598e+00 3\n",
" 7 1.2409e+01 7.6403e-02 1.1436e-04 1.2331e+00 3.22607e-01 0.0625 1.2331e+00 3\n",
" 8 1.2240e+01 7.6398e-02 1.0566e-04 1.2162e+00 2.29105e-01 0.1250 1.2162e+00 3\n",
" 9 1.1775e+01 7.6395e-02 8.8296e-05 1.1698e+00 8.08258e-02 1.0000 1.1698e+00 3\n",
" 10 7.3139e+00 7.6406e-02 5.9659e-05 7.2369e-01 3.21854e-01 0.0312 7.2369e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 7.2384e+00 7.6404e-02 5.8786e-05 7.1614e-01 2.73870e-01 0.0312 7.1614e-01 3\n",
" 12 7.0841e+00 7.6402e-02 5.7619e-05 7.0072e-01 2.30876e-01 0.0625 7.0072e-01 3\n",
" 13 6.9546e+00 7.6399e-02 5.5799e-05 6.8777e-01 1.54741e-01 0.1250 6.8777e-01 3\n",
" 14 6.5835e+00 7.6397e-02 5.1598e-05 6.5066e-01 5.00459e-02 1.0000 6.5066e-01 3\n",
" 15 2.5193e+00 7.6404e-02 1.2768e-05 2.4427e-01 2.08905e-01 0.0156 2.4427e-01 3\n",
" 16 2.4919e+00 7.6404e-02 1.2614e-05 2.4154e-01 1.92956e-01 0.0312 2.4154e-01 3\n",
" 17 2.4817e+00 7.6403e-02 1.2381e-05 2.4052e-01 1.62457e-01 0.0312 2.4052e-01 3\n",
" 18 2.4355e+00 7.6402e-02 1.2114e-05 2.3590e-01 1.36065e-01 0.0625 2.3590e-01 3\n",
" 19 2.3963e+00 7.6401e-02 1.1701e-05 2.3198e-01 9.07776e-02 0.1250 2.3198e-01 3\n",
" 20 2.2534e+00 7.6401e-02 1.0840e-05 2.1768e-01 3.00807e-02 1.0000 2.1768e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.1275e+00 7.6403e-02 2.7453e-06 1.0511e-01 1.22492e-01 0.0625 1.0511e-01 3\n",
" 22 1.1272e+00 7.6402e-02 2.7751e-06 1.0508e-01 8.19546e-02 0.1250 1.0508e-01 3\n",
" 23 1.1113e+00 7.6401e-02 2.7522e-06 1.0349e-01 2.86044e-02 0.5000 1.0349e-01 3\n",
" 24 7.4054e-01 7.6402e-02 1.9211e-06 6.6412e-02 4.27337e-02 0.2500 6.6412e-02 3\n",
" 25 7.1653e-01 7.6402e-02 1.7403e-06 6.4011e-02 1.38489e-02 1.0000 6.4011e-02 3\n",
" 26 2.7216e-01 7.6402e-02 1.6273e-06 1.9574e-02 6.38372e-02 0.0312 1.9574e-02 2\n",
" 27 2.7162e-01 7.6402e-02 1.6062e-06 1.9521e-02 5.23775e-02 0.0312 1.9521e-02 2\n",
" 28 2.6666e-01 7.6402e-02 1.5760e-06 1.9025e-02 4.29579e-02 0.0625 1.9025e-02 2\n",
" 29 2.6201e-01 7.6402e-02 1.5306e-06 1.8559e-02 2.74804e-02 0.1250 1.8559e-02 2\n",
" 30 2.4453e-01 7.6402e-02 1.4223e-06 1.6811e-02 7.85423e-03 1.0000 1.6811e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 1.0404e-01 7.6402e-02 3.9033e-07 2.7634e-03 ---- ---- 2.7634e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8137e+01 8.1742e-02 3.6134e+00 1.9220e-01 5.64746e-01 0.5000 ---- 3\n",
" 2 3.0818e+01 7.7378e-02 1.8179e+00 1.2561e+00 3.64866e-01 1.0000 1.2561e+00 3\n",
" 3 1.9596e+01 7.6978e-02 1.3430e-02 1.9385e+00 5.94257e-01 0.0625 1.9385e+00 3\n",
" 4 1.9259e+01 7.6970e-02 1.2594e-02 1.9056e+00 3.84246e-01 0.1250 1.9056e+00 3\n",
" 5 1.8343e+01 7.6964e-02 1.1022e-02 1.8156e+00 1.07609e-01 1.0000 1.8156e+00 3\n",
" 6 1.1093e+01 7.6983e-02 1.1256e-04 1.1015e+00 4.23855e-01 0.0312 1.1015e+00 3\n",
" 7 1.0964e+01 7.6979e-02 1.0836e-04 1.0886e+00 3.63784e-01 0.0312 1.0886e+00 3\n",
" 8 1.0704e+01 7.6976e-02 1.0438e-04 1.0626e+00 3.08055e-01 0.0625 1.0626e+00 3\n",
" 9 1.0400e+01 7.6972e-02 9.5900e-05 1.0322e+00 2.06077e-01 0.1250 1.0322e+00 3\n",
" 10 9.5395e+00 7.6971e-02 7.9840e-05 9.4617e-01 6.35604e-02 1.0000 9.4617e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.0899e+00 7.6981e-02 2.9353e-05 3.0126e-01 2.55603e-01 0.0312 3.0126e-01 3\n",
" 12 3.0311e+00 7.6980e-02 2.8814e-05 2.9538e-01 2.13330e-01 0.0625 2.9538e-01 3\n",
" 13 2.9954e+00 7.6978e-02 2.8041e-05 2.9181e-01 1.41007e-01 0.1250 2.9181e-01 3\n",
" 14 2.9012e+00 7.6977e-02 2.6280e-05 2.8239e-01 4.54146e-02 1.0000 2.8239e-01 3\n",
" 15 2.5880e+00 7.6979e-02 9.6487e-06 2.5109e-01 1.83263e-01 0.0625 2.5109e-01 3\n",
" 16 2.5871e+00 7.6977e-02 9.7038e-06 2.5101e-01 1.23208e-01 0.1250 2.5101e-01 3\n",
" 17 2.5855e+00 7.6976e-02 9.5296e-06 2.5084e-01 4.29841e-02 0.5000 2.5084e-01 3\n",
" 18 1.8298e+00 7.6977e-02 5.4922e-06 1.7527e-01 6.49985e-02 0.1250 1.7527e-01 3\n",
" 19 1.6622e+00 7.6976e-02 5.0225e-06 1.5851e-01 2.34488e-02 1.0000 1.5851e-01 3\n",
" 20 6.1533e-01 7.6978e-02 2.1166e-06 5.3834e-02 9.25311e-02 0.0312 5.3834e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 6.1188e-01 7.6978e-02 2.0800e-06 5.3488e-02 7.78201e-02 0.0312 5.3488e-02 3\n",
" 22 6.0225e-01 7.6977e-02 2.0347e-06 5.2525e-02 6.54334e-02 0.0625 5.2525e-02 3\n",
" 23 5.9785e-01 7.6977e-02 1.9596e-06 5.2085e-02 4.46214e-02 0.1250 5.2085e-02 3\n",
" 24 5.8543e-01 7.6977e-02 1.7961e-06 5.0844e-02 1.64710e-02 0.5000 5.0844e-02 3\n",
" 25 3.9594e-01 7.6977e-02 9.5067e-07 3.1895e-02 2.43116e-02 0.2500 3.1895e-02 3\n",
" 26 3.8846e-01 7.6977e-02 8.2528e-07 3.1148e-02 6.62271e-03 1.0000 3.1148e-02 3\n",
" END 1.3651e-01 7.6977e-02 1.6171e-07 5.9529e-03 ---- ---- 5.9529e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8296e+01 8.2333e-02 3.6249e+00 1.9644e-01 5.42785e-01 0.5000 ---- 3\n",
" 2 3.0462e+01 7.7491e-02 1.8374e+00 1.2011e+00 3.50711e-01 1.0000 1.2011e+00 3\n",
" 3 1.2628e+01 7.6588e-02 2.9492e-02 1.2256e+00 5.58474e-01 0.0625 1.2256e+00 3\n",
" 4 1.2488e+01 7.6582e-02 2.7652e-02 1.2135e+00 3.47547e-01 0.1250 1.2135e+00 3\n",
" 5 1.1824e+01 7.6578e-02 2.4197e-02 1.1506e+00 8.92006e-02 1.0000 1.1506e+00 3\n",
" 6 5.1811e+00 7.6590e-02 1.0003e-04 5.1035e-01 3.60524e-01 0.0312 5.1035e-01 3\n",
" 7 5.1225e+00 7.6588e-02 9.6310e-05 5.0450e-01 2.97943e-01 0.0312 5.0450e-01 3\n",
" 8 5.0019e+00 7.6586e-02 9.2875e-05 4.9244e-01 2.45316e-01 0.0625 4.9244e-01 3\n",
" 9 4.8630e+00 7.6584e-02 8.5888e-05 4.7855e-01 1.57381e-01 0.1250 4.7855e-01 3\n",
" 10 4.4937e+00 7.6584e-02 7.3198e-05 4.4164e-01 4.52071e-02 1.0000 4.4164e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.8512e+00 7.6588e-02 9.3537e-06 1.7745e-01 1.87221e-01 0.0312 1.7745e-01 3\n",
" 12 1.8207e+00 7.6587e-02 9.1820e-06 1.7440e-01 1.56013e-01 0.0625 1.7440e-01 3\n",
" 13 1.8073e+00 7.6586e-02 8.9466e-06 1.7306e-01 1.03257e-01 0.1250 1.7306e-01 3\n",
" 14 1.7568e+00 7.6585e-02 8.4363e-06 1.6801e-01 3.36131e-02 1.0000 1.6801e-01 3\n",
" 15 1.6048e+00 7.6587e-02 4.7092e-06 1.5281e-01 1.36509e-01 0.0312 1.5281e-01 3\n",
" 16 1.5683e+00 7.6586e-02 4.6230e-06 1.4917e-01 1.14057e-01 0.0625 1.4917e-01 3\n",
" 17 1.5326e+00 7.6586e-02 4.5019e-06 1.4560e-01 7.65090e-02 0.1250 1.4560e-01 3\n",
" 18 1.4574e+00 7.6585e-02 4.2341e-06 1.3807e-01 2.67732e-02 1.0000 1.3807e-01 3\n",
" 19 1.2704e+00 7.6586e-02 2.5932e-06 1.1938e-01 1.05904e-01 0.0625 1.1938e-01 3\n",
" 20 1.2682e+00 7.6586e-02 2.5233e-06 1.1916e-01 7.24904e-02 0.1250 1.1916e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.2536e+00 7.6585e-02 2.4407e-06 1.1770e-01 2.70276e-02 0.5000 1.1770e-01 3\n",
" 22 8.3373e-01 7.6585e-02 2.0811e-06 7.5713e-02 3.85147e-02 0.2500 7.5713e-02 3\n",
" 23 7.9172e-01 7.6585e-02 1.8062e-06 7.1512e-02 1.08558e-02 1.0000 7.1512e-02 3\n",
" 24 2.3871e-01 7.6586e-02 6.6671e-07 1.6212e-02 4.85365e-02 0.0312 1.6212e-02 2\n",
" 25 2.3472e-01 7.6586e-02 6.5818e-07 1.5813e-02 4.00384e-02 0.0625 1.5813e-02 2\n",
" 26 2.3140e-01 7.6586e-02 6.5006e-07 1.5480e-02 2.60261e-02 0.1250 1.5480e-02 2\n",
" 27 2.2031e-01 7.6586e-02 6.2197e-07 1.4371e-02 7.94609e-03 1.0000 1.4371e-02 2\n",
" END 1.2437e-01 7.6586e-02 3.2745e-07 4.7782e-03 ---- ---- 4.7782e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7971e+01 8.1930e-02 3.6023e+00 1.8660e-01 5.37843e-01 0.5000 ---- 3\n",
" 2 3.0174e+01 7.7157e-02 1.8232e+00 1.1865e+00 3.50718e-01 1.0000 1.1865e+00 3\n",
" 3 1.5237e+01 7.6324e-02 2.6137e-02 1.4900e+00 5.57829e-01 0.0625 1.4900e+00 3\n",
" 4 1.5078e+01 7.6318e-02 2.4507e-02 1.4757e+00 3.50718e-01 0.1250 1.4757e+00 3\n",
" 5 1.4357e+01 7.6313e-02 2.1446e-02 1.4067e+00 9.06112e-02 1.0000 1.4067e+00 3\n",
" 6 6.4532e+00 7.6328e-02 9.4235e-05 6.3759e-01 3.62716e-01 0.0312 6.3759e-01 3\n",
" 7 6.4005e+00 7.6325e-02 9.0779e-05 6.3232e-01 3.04184e-01 0.0312 6.3232e-01 3\n",
" 8 6.2634e+00 7.6323e-02 8.7564e-05 6.1862e-01 2.53247e-01 0.0625 6.1862e-01 3\n",
" 9 6.1224e+00 7.6321e-02 8.0992e-05 6.0453e-01 1.64860e-01 0.1250 6.0453e-01 3\n",
" 10 5.6783e+00 7.6320e-02 6.8998e-05 5.6013e-01 4.83488e-02 1.0000 5.6013e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.0856e+00 7.6326e-02 1.2459e-05 2.0092e-01 1.97808e-01 0.0312 2.0092e-01 3\n",
" 12 2.0511e+00 7.6325e-02 1.2217e-05 1.9746e-01 1.64433e-01 0.0625 1.9746e-01 3\n",
" 13 2.0339e+00 7.6324e-02 1.1893e-05 1.9574e-01 1.08131e-01 0.1250 1.9574e-01 3\n",
" 14 1.9581e+00 7.6323e-02 1.1186e-05 1.8817e-01 3.48388e-02 1.0000 1.8817e-01 3\n",
" 15 1.5165e+00 7.6324e-02 5.1398e-06 1.4401e-01 1.41260e-01 0.0312 1.4401e-01 3\n",
" 16 1.4847e+00 7.6324e-02 5.0480e-06 1.4083e-01 1.17937e-01 0.0625 1.4083e-01 3\n",
" 17 1.4581e+00 7.6323e-02 4.9217e-06 1.3817e-01 7.89672e-02 0.1250 1.3817e-01 3\n",
" 18 1.4008e+00 7.6323e-02 4.6366e-06 1.3245e-01 2.77192e-02 1.0000 1.3245e-01 3\n",
" 19 1.3954e+00 7.6324e-02 3.4149e-06 1.3191e-01 1.07623e-01 0.0625 1.3191e-01 3\n",
" 20 1.3841e+00 7.6323e-02 3.2799e-06 1.3078e-01 7.41493e-02 0.1250 1.3078e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.3522e+00 7.6323e-02 3.1009e-06 1.2759e-01 2.79721e-02 0.5000 1.2759e-01 3\n",
" 22 8.9761e-01 7.6323e-02 2.8284e-06 8.2126e-02 3.89638e-02 0.2500 8.2126e-02 3\n",
" 23 8.3009e-01 7.6323e-02 2.4309e-06 7.5374e-02 1.06839e-02 1.0000 7.5374e-02 3\n",
" 24 2.3831e-01 7.6323e-02 6.3426e-07 1.6198e-02 4.86693e-02 0.0312 1.6198e-02 3\n",
" 25 2.3701e-01 7.6323e-02 6.2476e-07 1.6068e-02 3.99892e-02 0.0312 1.6068e-02 3\n",
" 26 2.3387e-01 7.6323e-02 6.1188e-07 1.5754e-02 3.28729e-02 0.0625 1.5754e-02 3\n",
" 27 2.3075e-01 7.6323e-02 5.9063e-07 1.5442e-02 2.12011e-02 0.1250 1.5442e-02 3\n",
" 28 2.2145e-01 7.6323e-02 5.4157e-07 1.4512e-02 6.26811e-03 1.0000 1.4512e-02 3\n",
" END 1.1081e-01 7.6323e-02 1.9303e-07 3.4484e-03 ---- ---- 3.4484e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7791e+01 8.1660e-02 3.5862e+00 1.8475e-01 5.34377e-01 0.5000 ---- 3\n",
" 2 2.9865e+01 7.6935e-02 1.8129e+00 1.1659e+00 3.43082e-01 1.0000 1.1659e+00 3\n",
" 3 1.2737e+01 7.6150e-02 2.3429e-02 1.2426e+00 5.35316e-01 0.0625 1.2426e+00 3\n",
" 4 1.2672e+01 7.6144e-02 2.1967e-02 1.2376e+00 3.30761e-01 0.1250 1.2376e+00 3\n",
" 5 1.2072e+01 7.6141e-02 1.9223e-02 1.1803e+00 8.07034e-02 1.0000 1.1803e+00 3\n",
" 6 4.0942e+00 7.6153e-02 9.0566e-05 4.0172e-01 3.28488e-01 0.0312 4.0172e-01 3\n",
" 7 4.0800e+00 7.6151e-02 8.7249e-05 4.0030e-01 2.71198e-01 0.0312 4.0030e-01 3\n",
" 8 4.0039e+00 7.6150e-02 8.4181e-05 3.9269e-01 2.23028e-01 0.0625 3.9269e-01 3\n",
" 9 3.9409e+00 7.6148e-02 7.7987e-05 3.8639e-01 1.42404e-01 0.1250 3.8639e-01 3\n",
" 10 3.6989e+00 7.6147e-02 6.6761e-05 3.6220e-01 4.04957e-02 1.0000 3.6220e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.6734e+00 7.6151e-02 7.6542e-06 1.5972e-01 1.65965e-01 0.0312 1.5972e-01 3\n",
" 12 1.6471e+00 7.6150e-02 7.4866e-06 1.5708e-01 1.38012e-01 0.0625 1.5708e-01 3\n",
" 13 1.6360e+00 7.6149e-02 7.2241e-06 1.5598e-01 9.10096e-02 0.1250 1.5598e-01 3\n",
" 14 1.5787e+00 7.6149e-02 6.7270e-06 1.5025e-01 2.98823e-02 1.0000 1.5025e-01 3\n",
" 15 1.2660e+00 7.6150e-02 4.0908e-06 1.1898e-01 1.19345e-01 0.0312 1.1898e-01 3\n",
" 16 1.2379e+00 7.6149e-02 4.0083e-06 1.1617e-01 9.97544e-02 0.0625 1.1617e-01 3\n",
" 17 1.2111e+00 7.6149e-02 3.8891e-06 1.1349e-01 6.71947e-02 0.1250 1.1349e-01 3\n",
" 18 1.1568e+00 7.6149e-02 3.6584e-06 1.0806e-01 2.42020e-02 0.5000 1.0806e-01 3\n",
" 19 7.3823e-01 7.6149e-02 2.6173e-06 6.6206e-02 3.49835e-02 0.2500 6.6206e-02 3\n",
" 20 6.8408e-01 7.6149e-02 2.2354e-06 6.0791e-02 1.05003e-02 1.0000 6.0791e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.7278e-01 7.6149e-02 6.1797e-07 1.9663e-02 4.93445e-02 0.0312 1.9663e-02 3\n",
" 22 2.7026e-01 7.6149e-02 6.0954e-07 1.9411e-02 4.03697e-02 0.0625 1.9411e-02 3\n",
" 23 2.6947e-01 7.6149e-02 6.0038e-07 1.9332e-02 2.56649e-02 0.1250 1.9332e-02 3\n",
" 24 2.6044e-01 7.6149e-02 5.6984e-07 1.8429e-02 7.10954e-03 1.0000 1.8429e-02 3\n",
" END 1.0057e-01 7.6149e-02 2.2368e-07 2.4421e-03 ---- ---- 2.4421e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7946e+01 8.1479e-02 3.5996e+00 1.8687e-01 5.51729e-01 0.5000 ---- 3\n",
" 2 3.0849e+01 7.7073e-02 1.8109e+00 1.2663e+00 3.26483e-01 1.0000 1.2663e+00 3\n",
" 3 1.1433e+01 7.6640e-02 1.3263e-02 1.1223e+00 4.30643e-01 0.0625 1.1223e+00 3\n",
" 4 1.1352e+01 7.6635e-02 1.2434e-02 1.1151e+00 2.66260e-01 0.1250 1.1151e+00 3\n",
" 5 1.0886e+01 7.6632e-02 1.0879e-02 1.0701e+00 6.67021e-02 1.0000 1.0701e+00 3\n",
" 6 6.4451e+00 7.6642e-02 9.2374e-05 6.3675e-01 2.16453e-01 0.0312 6.3675e-01 3\n",
" 7 6.3301e+00 7.6640e-02 8.9390e-05 6.2526e-01 1.87443e-01 0.0625 6.2526e-01 3\n",
" 8 6.2874e+00 7.6638e-02 8.3380e-05 6.2099e-01 1.31864e-01 0.1250 6.2099e-01 3\n",
" 9 6.0768e+00 7.6636e-02 7.1484e-05 5.9994e-01 4.63637e-02 1.0000 5.9994e-01 3\n",
" 10 2.5987e+00 7.6641e-02 2.5880e-05 2.5218e-01 1.88308e-01 0.0156 2.5218e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.5688e+00 7.6641e-02 2.5582e-05 2.4919e-01 1.73132e-01 0.0312 2.4919e-01 3\n",
" 12 2.5558e+00 7.6640e-02 2.5140e-05 2.4789e-01 1.44572e-01 0.0312 2.4789e-01 3\n",
" 13 2.5106e+00 7.6639e-02 2.4598e-05 2.4338e-01 1.20262e-01 0.0625 2.4338e-01 3\n",
" 14 2.4864e+00 7.6638e-02 2.3718e-05 2.4095e-01 7.89996e-02 0.1250 2.4095e-01 3\n",
" 15 2.3865e+00 7.6637e-02 2.1815e-05 2.3097e-01 2.48098e-02 1.0000 2.3097e-01 3\n",
" 16 8.9361e-01 7.6639e-02 5.4925e-06 8.1692e-02 1.03145e-01 0.0156 8.1692e-02 3\n",
" 17 8.8487e-01 7.6639e-02 5.4258e-06 8.0818e-02 9.50755e-02 0.0312 8.0818e-02 3\n",
" 18 8.8227e-01 7.6639e-02 5.3253e-06 8.0558e-02 7.98874e-02 0.0312 8.0558e-02 3\n",
" 19 8.6711e-01 7.6639e-02 5.2127e-06 7.9041e-02 6.69012e-02 0.0625 7.9041e-02 3\n",
" 20 8.5374e-01 7.6638e-02 5.0491e-06 7.7705e-02 4.48752e-02 0.1250 7.7705e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 8.0145e-01 7.6638e-02 4.7437e-06 7.2477e-02 1.51667e-02 1.0000 7.2477e-02 3\n",
" 22 2.8412e-01 7.6639e-02 2.5470e-06 2.0746e-02 6.50401e-02 0.0020 2.0746e-02 1\n",
" 23 2.8412e-01 7.6639e-02 2.5470e-06 2.0746e-02 6.49910e-02 0.0156 2.0746e-02 1\n",
" 24 2.8297e-01 7.6639e-02 2.5059e-06 2.0631e-02 5.98055e-02 0.0078 2.0631e-02 1\n",
" 25 2.8279e-01 7.6639e-02 2.4858e-06 2.0613e-02 5.71700e-02 0.0156 2.0613e-02 1\n",
" 26 2.8197e-01 7.6639e-02 2.4450e-06 2.0530e-02 5.26694e-02 0.0156 2.0530e-02 1\n",
" 27 2.8160e-01 7.6639e-02 2.4044e-06 2.0494e-02 4.85440e-02 0.0156 2.0494e-02 1\n",
" 28 2.8059e-01 7.6639e-02 2.3643e-06 2.0393e-02 4.47806e-02 0.0156 2.0393e-02 1\n",
" 29 2.7965e-01 7.6638e-02 2.3248e-06 2.0299e-02 4.13387e-02 0.0312 2.0299e-02 1\n",
" 30 2.7880e-01 7.6638e-02 2.2416e-06 2.0214e-02 3.50665e-02 0.0312 2.0214e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.7716e-01 7.6638e-02 2.1614e-06 2.0050e-02 2.98957e-02 0.0625 2.0050e-02 1\n",
" 32 2.7542e-01 7.6638e-02 1.9923e-06 1.9876e-02 2.13170e-02 0.1250 1.9876e-02 1\n",
" 33 2.7107e-01 7.6638e-02 1.6665e-06 1.9442e-02 1.00300e-02 1.0000 1.9442e-02 1\n",
" 34 2.3967e-01 7.6638e-02 1.1011e-05 1.6292e-02 2.44673e-02 0.0625 1.6292e-02 1\n",
" 35 2.3768e-01 7.6638e-02 1.0307e-05 1.6093e-02 1.68907e-02 0.1250 1.6093e-02 1\n",
" 36 2.3653e-01 7.6638e-02 8.9905e-06 1.5980e-02 6.75481e-03 1.0000 1.5980e-02 1\n",
" 37 2.2850e-01 7.6638e-02 5.0179e-07 1.5185e-02 2.39754e-02 0.0625 1.5185e-02 1\n",
" 38 2.2776e-01 7.6638e-02 4.8857e-07 1.5111e-02 1.66373e-02 0.1250 1.5111e-02 1\n",
" 39 2.2622e-01 7.6638e-02 4.6082e-07 1.4957e-02 6.66531e-03 0.5000 1.4957e-02 1\n",
" 40 2.0690e-01 7.6638e-02 3.1297e-07 1.3026e-02 8.66447e-03 0.5000 1.3026e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 2.0599e-01 7.6638e-02 3.2751e-07 1.2935e-02 1.22991e-02 0.1250 1.2935e-02 1\n",
" 42 2.0209e-01 7.6638e-02 3.0903e-07 1.2545e-02 4.54393e-03 1.0000 1.2545e-02 1\n",
" END 1.6776e-01 7.6638e-02 2.0564e-07 9.1122e-03 ---- ---- 9.1122e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8215e+01 8.1991e-02 3.6076e+00 2.0564e-01 5.42162e-01 0.5000 ---- 3\n",
" 2 3.0709e+01 7.7225e-02 1.8254e+00 1.2378e+00 3.32602e-01 1.0000 1.2378e+00 3\n",
" 3 1.1351e+01 7.6404e-02 2.5224e-02 1.1022e+00 4.89108e-01 0.0625 1.1022e+00 3\n",
" 4 1.1281e+01 7.6400e-02 2.3650e-02 1.0968e+00 2.96976e-01 0.1250 1.0968e+00 3\n",
" 5 1.0720e+01 7.6397e-02 2.0697e-02 1.0437e+00 7.00819e-02 1.0000 1.0437e+00 3\n",
" 6 4.3839e+00 7.6408e-02 1.1281e-04 4.3063e-01 2.46595e-01 0.0312 4.3063e-01 3\n",
" 7 4.3370e+00 7.6407e-02 1.0882e-04 4.2595e-01 2.05694e-01 0.0312 4.2595e-01 3\n",
" 8 4.2405e+00 7.6405e-02 1.0501e-04 4.1630e-01 1.70759e-01 0.0625 4.1630e-01 3\n",
" 9 4.1414e+00 7.6404e-02 9.7144e-05 4.0640e-01 1.12560e-01 0.1250 4.0640e-01 3\n",
" 10 3.8517e+00 7.6403e-02 8.2029e-05 3.7744e-01 3.74728e-02 1.0000 3.7744e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.2319e+00 7.6407e-02 3.2146e-05 1.1552e-01 1.45710e-01 0.0312 1.1552e-01 3\n",
" 12 1.2272e+00 7.6406e-02 3.1573e-05 1.1505e-01 1.21713e-01 0.0312 1.1505e-01 3\n",
" 13 1.2069e+00 7.6405e-02 3.0897e-05 1.1302e-01 1.01427e-01 0.0625 1.1302e-01 3\n",
" 14 1.1961e+00 7.6405e-02 2.9854e-05 1.1194e-01 6.74537e-02 0.1250 1.1194e-01 3\n",
" 15 1.1518e+00 7.6405e-02 2.7764e-05 1.0751e-01 2.42190e-02 1.0000 1.0751e-01 3\n",
" 16 6.8700e-01 7.6405e-02 1.1923e-05 6.1047e-02 9.29284e-02 0.0312 6.1047e-02 3\n",
" 17 6.7697e-01 7.6405e-02 1.1708e-05 6.0045e-02 7.80840e-02 0.0625 6.0045e-02 3\n",
" 18 6.7302e-01 7.6405e-02 1.1476e-05 5.9650e-02 5.31541e-02 0.1250 5.9650e-02 3\n",
" 19 6.5693e-01 7.6405e-02 1.1058e-05 5.8041e-02 1.96131e-02 1.0000 5.8041e-02 3\n",
" 20 5.2965e-01 7.6405e-02 7.8458e-06 4.5317e-02 7.38961e-02 0.0312 4.5317e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 5.1929e-01 7.6405e-02 7.7216e-06 4.4281e-02 6.26770e-02 0.0625 4.4281e-02 3\n",
" 22 5.0898e-01 7.6405e-02 7.5800e-06 4.3250e-02 4.35259e-02 0.1250 4.3250e-02 3\n",
" 23 4.8623e-01 7.6404e-02 7.2545e-06 4.0975e-02 1.70617e-02 1.0000 4.0975e-02 3\n",
" 24 4.0570e-01 7.6405e-02 4.6049e-06 3.2925e-02 6.36424e-02 0.0625 3.2925e-02 3\n",
" 25 4.0424e-01 7.6404e-02 4.5985e-06 3.2779e-02 4.43169e-02 0.1250 3.2779e-02 3\n",
" 26 3.9918e-01 7.6404e-02 4.5525e-06 3.2273e-02 1.80600e-02 0.5000 3.2273e-02 3\n",
" 27 2.9880e-01 7.6404e-02 3.3178e-06 2.2237e-02 2.34083e-02 0.2500 2.2237e-02 2\n",
" 28 2.4847e-01 7.6404e-02 2.9569e-06 1.7204e-02 5.42925e-03 1.0000 1.7204e-02 2\n",
" END 8.3780e-02 7.6404e-02 6.8860e-07 7.3685e-04 ---- ---- 7.3685e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8159e+01 8.1749e-02 3.6198e+00 1.8802e-01 5.61494e-01 0.5000 ---- 3\n",
" 2 3.0421e+01 7.7345e-02 1.8210e+00 1.2133e+00 3.60033e-01 1.0000 1.2133e+00 3\n",
" 3 2.2828e+01 7.6916e-02 1.3991e-02 2.2611e+00 5.96328e-01 0.0625 2.2611e+00 3\n",
" 4 2.2440e+01 7.6907e-02 1.3121e-02 2.2232e+00 3.86311e-01 0.1250 2.2232e+00 3\n",
" 5 2.1456e+01 7.6900e-02 1.1487e-02 2.1265e+00 1.07467e-01 1.0000 2.1265e+00 3\n",
" 6 1.6311e+01 7.6922e-02 1.3301e-04 1.6232e+00 3.86698e-01 0.0312 1.6232e+00 3\n",
" 7 1.6000e+01 7.6917e-02 1.2800e-04 1.5922e+00 3.29826e-01 0.0625 1.5922e+00 3\n",
" 8 1.5889e+01 7.6909e-02 1.1684e-04 1.5810e+00 2.39673e-01 0.1250 1.5810e+00 3\n",
" 9 1.5435e+01 7.6904e-02 9.2630e-05 1.5357e+00 9.11040e-02 1.0000 1.5357e+00 3\n",
" 10 9.1811e+00 7.6921e-02 1.6064e-04 9.1026e-01 3.58025e-01 0.0312 9.1026e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 9.0877e+00 7.6917e-02 1.5775e-04 9.0092e-01 3.06718e-01 0.0312 9.0092e-01 3\n",
" 12 8.8972e+00 7.6915e-02 1.5443e-04 8.8187e-01 2.60136e-01 0.0625 8.8187e-01 3\n",
" 13 8.7502e+00 7.6911e-02 1.4947e-04 8.6718e-01 1.76424e-01 0.1250 8.6718e-01 3\n",
" 14 8.3204e+00 7.6909e-02 1.3953e-04 8.2421e-01 6.12412e-02 1.0000 8.2421e-01 3\n",
" 15 3.3895e+00 7.6918e-02 5.7518e-05 3.3120e-01 2.43405e-01 0.0156 3.3120e-01 3\n",
" 16 3.3492e+00 7.6917e-02 5.6821e-05 3.2717e-01 2.25013e-01 0.0312 3.2717e-01 3\n",
" 17 3.3228e+00 7.6916e-02 5.5771e-05 3.2453e-01 1.89836e-01 0.0312 3.2453e-01 3\n",
" 18 3.2490e+00 7.6915e-02 5.4592e-05 3.1715e-01 1.59440e-01 0.0625 3.1715e-01 3\n",
" 19 3.1584e+00 7.6914e-02 5.2869e-05 3.0810e-01 1.07402e-01 0.1250 3.0810e-01 3\n",
" 20 2.9277e+00 7.6913e-02 4.9535e-05 2.8503e-01 3.78577e-02 1.0000 2.8503e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.1353e+00 7.6916e-02 2.2032e-05 1.0581e-01 1.42646e-01 0.0625 1.0581e-01 3\n",
" 22 1.1252e+00 7.6915e-02 2.2015e-05 1.0481e-01 9.84074e-02 0.0625 1.0481e-01 3\n",
" 23 1.0819e+00 7.6915e-02 2.1253e-05 1.0048e-01 6.80772e-02 0.1250 1.0048e-01 3\n",
" 24 1.0054e+00 7.6915e-02 1.9700e-05 9.2825e-02 2.75257e-02 1.0000 9.2825e-02 3\n",
" 25 7.9493e-01 7.6915e-02 8.3995e-06 7.1793e-02 9.49099e-02 0.0625 7.1793e-02 3\n",
" 26 7.7616e-01 7.6915e-02 8.3002e-06 6.9916e-02 6.76036e-02 0.1250 6.9916e-02 3\n",
" 27 7.4408e-01 7.6915e-02 8.1012e-06 6.6708e-02 3.00073e-02 0.5000 6.6708e-02 3\n",
" 28 5.5604e-01 7.6915e-02 6.2641e-06 4.7907e-02 3.49027e-02 0.2500 4.7907e-02 3\n",
" 29 4.7758e-01 7.6915e-02 5.4727e-06 4.0061e-02 6.89328e-03 1.0000 4.0061e-02 2\n",
" END 1.3008e-01 7.6915e-02 5.9251e-07 5.3162e-03 ---- ---- 5.3162e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8159e+01 8.2281e-02 3.6251e+00 1.8257e-01 5.47758e-01 0.5000 ---- 3\n",
" 2 3.0661e+01 7.7484e-02 1.8353e+00 1.2230e+00 3.74435e-01 1.0000 1.2230e+00 3\n",
" 3 2.5322e+01 7.6634e-02 2.6955e-02 2.4976e+00 7.13541e-01 0.0625 2.4976e+00 3\n",
" 4 2.5036e+01 7.6623e-02 2.5280e-02 2.4706e+00 4.45060e-01 0.1250 2.4706e+00 3\n",
" 5 2.3976e+01 7.6616e-02 2.2132e-02 2.3678e+00 1.08048e-01 1.0000 2.3678e+00 3\n",
" 6 1.3857e+01 7.6643e-02 1.1630e-04 1.3779e+00 4.14201e-01 0.0312 1.3779e+00 3\n",
" 7 1.3714e+01 7.6637e-02 1.1215e-04 1.3636e+00 3.47617e-01 0.0312 1.3636e+00 3\n",
" 8 1.3417e+01 7.6633e-02 1.0814e-04 1.3339e+00 2.99369e-01 0.0625 1.3339e+00 3\n",
" 9 1.3142e+01 7.6628e-02 9.9529e-05 1.3064e+00 2.07489e-01 0.1250 1.3064e+00 3\n",
" 10 1.2242e+01 7.6626e-02 8.2227e-05 1.2165e+00 6.69837e-02 1.0000 1.2165e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.9316e+00 7.6641e-02 6.5087e-05 1.8543e-01 2.80581e-01 0.0156 1.8543e-01 3\n",
" 12 1.9259e+00 7.6640e-02 6.4368e-05 1.8487e-01 2.56073e-01 0.0156 1.8487e-01 3\n",
" 13 1.9134e+00 7.6639e-02 6.3605e-05 1.8361e-01 2.33364e-01 0.0156 1.8361e-01 3\n",
" 14 1.8952e+00 7.6638e-02 6.2808e-05 1.8179e-01 2.12582e-01 0.0312 1.8179e-01 3\n",
" 15 1.8945e+00 7.6637e-02 6.1478e-05 1.8172e-01 1.74118e-01 0.0312 1.8172e-01 3\n",
" 16 1.8632e+00 7.6636e-02 5.9956e-05 1.7859e-01 1.42337e-01 0.0625 1.7859e-01 3\n",
" 17 1.8389e+00 7.6635e-02 5.7206e-05 1.7617e-01 9.01977e-02 0.1250 1.7617e-01 3\n",
" 18 1.7200e+00 7.6634e-02 5.1413e-05 1.6429e-01 2.66803e-02 1.0000 1.6429e-01 3\n",
" 19 4.4230e-01 7.6636e-02 4.2769e-06 3.6562e-02 1.06023e-01 0.0312 3.6562e-02 3\n",
" 20 4.3383e-01 7.6636e-02 4.2094e-06 3.5715e-02 8.79132e-02 0.0625 3.5715e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.2640e-01 7.6635e-02 4.1304e-06 3.4973e-02 5.83806e-02 0.1250 3.4973e-02 3\n",
" 22 4.1283e-01 7.6635e-02 3.9478e-06 3.3615e-02 2.02156e-02 1.0000 3.3615e-02 2\n",
" 23 3.8460e-01 7.6635e-02 2.2790e-06 3.0794e-02 7.69294e-02 0.0625 3.0794e-02 2\n",
" 24 3.7301e-01 7.6635e-02 2.2558e-06 2.9635e-02 5.26969e-02 0.1250 2.9635e-02 2\n",
" 25 3.5320e-01 7.6635e-02 2.2131e-06 2.7654e-02 2.02434e-02 0.5000 2.7654e-02 2\n",
" 26 2.5588e-01 7.6635e-02 1.7935e-06 1.7923e-02 3.01310e-02 0.0020 1.7923e-02 1\n",
" 27 2.5588e-01 7.6635e-02 1.7935e-06 1.7923e-02 3.01679e-02 0.1250 1.7923e-02 1\n",
" 28 2.5285e-01 7.6635e-02 1.8378e-06 1.7620e-02 1.40110e-02 0.2500 1.7620e-02 1\n",
" 29 2.5011e-01 7.6635e-02 2.1501e-06 1.7345e-02 5.75522e-03 1.0000 1.7345e-02 1\n",
" END 1.2042e-01 7.6635e-02 3.4387e-06 4.3751e-03 ---- ---- 4.3751e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8086e+01 8.1992e-02 3.6062e+00 1.9420e-01 5.42848e-01 0.5000 ---- 3\n",
" 2 3.0079e+01 7.7247e-02 1.8235e+00 1.1767e+00 3.40195e-01 1.0000 1.1767e+00 3\n",
" 3 1.1361e+01 7.6452e-02 2.3741e-02 1.1048e+00 5.37116e-01 0.0312 1.1048e+00 3\n",
" 4 1.1100e+01 7.6449e-02 2.3000e-02 1.0794e+00 4.27647e-01 0.0625 1.0794e+00 3\n",
" 5 1.0785e+01 7.6446e-02 2.1563e-02 1.0493e+00 2.52840e-01 0.1250 1.0493e+00 3\n",
" 6 9.8357e+00 7.6444e-02 1.8868e-02 9.5706e-01 5.58364e-02 1.0000 9.5706e-01 3\n",
" 7 1.7216e+00 7.6455e-02 9.3614e-05 1.6442e-01 1.92702e-01 0.0156 1.6442e-01 3\n",
" 8 1.7026e+00 7.6455e-02 9.2117e-05 1.6253e-01 1.76618e-01 0.0312 1.6253e-01 3\n",
" 9 1.6935e+00 7.6454e-02 8.9112e-05 1.6162e-01 1.46267e-01 0.0312 1.6162e-01 3\n",
" 10 1.6597e+00 7.6453e-02 8.6226e-05 1.5824e-01 1.20643e-01 0.0625 1.5824e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.6230e+00 7.6452e-02 8.0538e-05 1.5458e-01 7.76643e-02 0.1250 1.5458e-01 3\n",
" 12 1.4973e+00 7.6452e-02 6.9932e-05 1.4201e-01 2.41112e-02 1.0000 1.4201e-01 3\n",
" 13 2.9680e-01 7.6453e-02 5.2450e-06 2.2030e-02 9.24483e-02 0.0312 2.2030e-02 3\n",
" 14 2.9284e-01 7.6453e-02 5.1288e-06 2.1634e-02 7.62157e-02 0.0625 2.1634e-02 3\n",
" 15 2.9019e-01 7.6453e-02 4.9388e-06 2.1368e-02 4.96211e-02 0.1250 2.1368e-02 3\n",
" 16 2.8339e-01 7.6452e-02 4.5588e-06 2.0689e-02 1.67891e-02 0.5000 2.0689e-02 3\n",
" 17 2.2898e-01 7.6452e-02 2.9329e-06 1.5250e-02 2.36781e-02 0.2500 1.5250e-02 2\n",
" 18 2.0934e-01 7.6452e-02 2.4383e-06 1.3286e-02 1.38848e-02 0.0020 1.3286e-02 1\n",
" 19 2.0934e-01 7.6452e-02 2.4383e-06 1.3286e-02 1.38306e-02 0.5000 1.3286e-02 1\n",
" 20 1.7661e-01 7.6452e-02 2.6839e-06 1.0013e-02 1.95452e-02 0.0625 1.0013e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.7653e-01 7.6452e-02 2.4939e-06 1.0006e-02 1.23500e-02 0.2500 1.0006e-02 1\n",
" END 1.7513e-01 7.6452e-02 1.7019e-06 9.8657e-03 ---- ---- 9.8657e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7957e+01 8.1804e-02 3.5939e+00 1.9362e-01 5.40923e-01 0.5000 ---- 3\n",
" 2 3.0057e+01 7.7095e-02 1.8155e+00 1.1825e+00 3.48570e-01 1.0000 1.1825e+00 3\n",
" 3 1.3493e+01 7.6336e-02 2.1720e-02 1.3200e+00 6.01159e-01 0.0312 1.3200e+00 3\n",
" 4 1.3219e+01 7.6332e-02 2.1042e-02 1.2932e+00 4.79213e-01 0.0625 1.2932e+00 3\n",
" 5 1.2947e+01 7.6328e-02 1.9730e-02 1.2674e+00 2.83282e-01 0.1250 1.2674e+00 3\n",
" 6 1.1959e+01 7.6326e-02 1.7264e-02 1.1710e+00 6.00250e-02 1.0000 1.1710e+00 3\n",
" 7 1.7762e+00 7.6339e-02 9.9263e-05 1.6989e-01 2.28770e-01 0.0312 1.6989e-01 3\n",
" 8 1.7595e+00 7.6338e-02 9.5971e-05 1.6822e-01 1.88088e-01 0.0312 1.6822e-01 3\n",
" 9 1.7201e+00 7.6337e-02 9.2833e-05 1.6428e-01 1.53852e-01 0.0625 1.6428e-01 3\n",
" 10 1.6725e+00 7.6336e-02 8.6640e-05 1.5953e-01 9.68019e-02 0.1250 1.5953e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.5427e+00 7.6335e-02 7.5243e-05 1.4656e-01 2.75822e-02 1.0000 1.4656e-01 3\n",
" 12 4.9052e-01 7.6337e-02 7.0979e-06 4.1411e-02 1.05860e-01 0.0625 4.1411e-02 3\n",
" 13 4.7679e-01 7.6336e-02 6.8317e-06 4.0038e-02 6.80973e-02 0.0625 4.0038e-02 3\n",
" 14 4.5160e-01 7.6336e-02 6.4902e-06 3.7520e-02 4.44068e-02 0.1250 3.7520e-02 3\n",
" 15 4.1951e-01 7.6336e-02 5.8609e-06 3.4312e-02 1.56302e-02 1.0000 3.4312e-02 3\n",
" 16 3.9561e-01 7.6336e-02 4.1691e-06 3.1923e-02 5.37072e-02 0.0625 3.1923e-02 3\n",
" 17 3.8996e-01 7.6336e-02 3.9941e-06 3.1358e-02 3.70202e-02 0.1250 3.1358e-02 3\n",
" 18 3.8091e-01 7.6335e-02 3.7185e-06 3.0453e-02 1.48760e-02 0.5000 3.0453e-02 3\n",
" 19 3.1039e-01 7.6335e-02 2.9951e-06 2.3403e-02 1.82771e-02 0.5000 2.3403e-02 2\n",
" 20 2.9905e-01 7.6335e-02 2.5285e-06 2.2269e-02 2.99468e-02 0.0020 2.2269e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.9905e-01 7.6335e-02 2.5285e-06 2.2269e-02 3.00037e-02 0.1250 2.2269e-02 1\n",
" 22 2.9466e-01 7.6335e-02 1.9360e-06 2.1831e-02 1.31316e-02 0.2500 2.1831e-02 1\n",
" 23 2.8954e-01 7.6335e-02 1.4983e-06 2.1319e-02 6.14748e-03 1.0000 2.1319e-02 1\n",
" END 1.3101e-01 7.6335e-02 3.0081e-06 5.4646e-03 ---- ---- 5.4646e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8197e+01 8.1681e-02 3.6140e+00 1.9750e-01 5.55357e-01 0.5000 ---- 3\n",
" 2 3.0145e+01 7.7239e-02 1.8181e+00 1.1886e+00 3.32438e-01 1.0000 1.1886e+00 3\n",
" 3 1.1665e+01 7.6786e-02 1.3613e-02 1.1452e+00 4.69106e-01 0.0312 1.1452e+00 3\n",
" 4 1.1406e+01 7.6783e-02 1.3187e-02 1.1197e+00 3.75594e-01 0.0625 1.1197e+00 3\n",
" 5 1.1111e+01 7.6779e-02 1.2363e-02 1.0911e+00 2.23698e-01 0.1250 1.0911e+00 3\n",
" 6 1.0190e+01 7.6778e-02 1.0817e-02 1.0005e+00 5.25973e-02 1.0000 1.0005e+00 3\n",
" 7 1.6537e+00 7.6788e-02 1.0446e-04 1.5759e-01 1.79232e-01 0.0156 1.5759e-01 3\n",
" 8 1.6358e+00 7.6787e-02 1.0281e-04 1.5580e-01 1.64710e-01 0.0312 1.5580e-01 3\n",
" 9 1.6276e+00 7.6786e-02 9.9509e-05 1.5498e-01 1.37131e-01 0.0312 1.5498e-01 3\n",
" 10 1.5952e+00 7.6786e-02 9.6336e-05 1.5174e-01 1.13563e-01 0.0625 1.5174e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.5591e+00 7.6785e-02 9.0137e-05 1.4814e-01 7.37832e-02 0.1250 1.4814e-01 3\n",
" 12 1.4345e+00 7.6785e-02 7.8633e-05 1.3569e-01 2.42664e-02 1.0000 1.3569e-01 3\n",
" 13 4.3632e-01 7.6786e-02 6.1059e-06 3.5947e-02 9.14376e-02 0.0312 3.5947e-02 3\n",
" 14 4.2716e-01 7.6785e-02 5.9343e-06 3.5031e-02 7.54534e-02 0.0625 3.5031e-02 3\n",
" 15 4.2193e-01 7.6785e-02 5.6294e-06 3.4509e-02 4.92488e-02 0.1250 3.4509e-02 3\n",
" 16 4.0807e-01 7.6785e-02 5.1015e-06 3.3123e-02 1.73079e-02 0.5000 3.3123e-02 3\n",
" 17 2.7990e-01 7.6785e-02 3.6593e-06 2.0308e-02 2.35478e-02 0.2500 2.0308e-02 3\n",
" 18 2.6513e-01 7.6785e-02 2.8412e-06 1.8832e-02 7.97753e-03 1.0000 1.8832e-02 2\n",
" 19 2.0613e-01 7.6785e-02 7.0769e-07 1.2934e-02 4.21547e-02 0.0312 1.2934e-02 2\n",
" 20 2.0326e-01 7.6785e-02 7.0177e-07 1.2647e-02 3.38726e-02 0.0625 1.2647e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.0059e-01 7.6785e-02 7.0201e-07 1.2380e-02 2.42238e-02 0.0020 1.2380e-02 1\n",
" 22 2.0059e-01 7.6785e-02 7.0201e-07 1.2380e-02 2.42157e-02 0.0625 1.2380e-02 1\n",
" 23 1.9783e-01 7.6785e-02 7.4149e-07 1.2104e-02 1.62692e-02 0.0625 1.2104e-02 1\n",
" 24 1.9694e-01 7.6785e-02 7.4242e-07 1.2015e-02 1.15298e-02 0.5000 1.2015e-02 1\n",
" 25 1.9610e-01 7.6785e-02 2.3865e-06 1.1929e-02 1.93413e-02 0.1250 1.1929e-02 2\n",
" 26 1.8572e-01 7.6785e-02 2.2370e-06 1.0891e-02 6.11772e-03 1.0000 1.0891e-02 2\n",
" END 7.8963e-02 7.6785e-02 2.1265e-06 2.1571e-04 ---- ---- 6.1961e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8415e+01 8.2159e-02 3.6539e+00 1.7939e-01 5.55818e-01 0.5000 ---- 3\n",
" 2 3.0305e+01 7.7741e-02 1.8382e+00 1.1845e+00 3.33347e-01 1.0000 1.1845e+00 3\n",
" 3 1.3195e+01 7.7314e-02 1.3820e-02 1.2980e+00 4.98259e-01 0.0312 1.2980e+00 3\n",
" 4 1.2929e+01 7.7310e-02 1.3389e-02 1.2718e+00 4.00921e-01 0.0625 1.2718e+00 3\n",
" 5 1.2645e+01 7.7306e-02 1.2552e-02 1.2442e+00 2.39655e-01 0.1250 1.2442e+00 3\n",
" 6 1.1585e+01 7.7305e-02 1.0981e-02 1.1398e+00 5.50910e-02 1.0000 1.1398e+00 3\n",
" 7 1.3861e+00 7.7317e-02 1.2345e-04 1.3075e-01 1.85394e-01 0.0156 1.3075e-01 3\n",
" 8 1.3721e+00 7.7317e-02 1.2150e-04 1.2936e-01 1.68959e-01 0.0312 1.2936e-01 3\n",
" 9 1.3677e+00 7.7316e-02 1.1760e-04 1.2892e-01 1.39195e-01 0.0312 1.2892e-01 3\n",
" 10 1.3419e+00 7.7315e-02 1.1386e-04 1.2634e-01 1.14407e-01 0.0625 1.2634e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.3140e+00 7.7314e-02 1.0655e-04 1.2357e-01 7.35071e-02 0.1250 1.2357e-01 3\n",
" 12 1.2177e+00 7.7314e-02 9.2990e-05 1.1394e-01 2.47940e-02 1.0000 1.1394e-01 3\n",
" 13 5.7532e-01 7.7315e-02 6.7058e-06 4.9793e-02 8.72662e-02 0.0312 4.9793e-02 3\n",
" 14 5.6285e-01 7.7315e-02 6.5224e-06 4.8548e-02 7.21957e-02 0.0625 4.8548e-02 3\n",
" 15 5.5413e-01 7.7314e-02 6.1983e-06 4.7675e-02 4.76911e-02 0.1250 4.7675e-02 3\n",
" 16 5.3310e-01 7.7314e-02 5.6275e-06 4.5573e-02 1.79638e-02 1.0000 4.5573e-02 3\n",
" 17 4.2076e-01 7.7314e-02 4.5221e-06 3.4340e-02 6.19688e-02 0.0625 3.4340e-02 3\n",
" 18 4.1181e-01 7.7314e-02 4.3124e-06 3.3446e-02 4.25046e-02 0.1250 3.3446e-02 3\n",
" 19 3.9679e-01 7.7314e-02 4.0099e-06 3.1944e-02 1.70368e-02 0.5000 3.1944e-02 3\n",
" 20 3.1797e-01 7.7314e-02 3.5047e-06 2.4062e-02 2.13536e-02 0.2500 2.4062e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.6021e-01 7.7314e-02 2.8841e-06 1.8286e-02 1.09781e-02 0.0020 1.8286e-02 1\n",
" 22 2.6021e-01 7.7314e-02 2.8841e-06 1.8286e-02 1.09502e-02 1.0000 1.8286e-02 1\n",
" 23 2.0071e-01 7.7314e-02 8.1215e-06 1.2332e-02 3.79575e-02 0.0625 1.2332e-02 2\n",
" 24 1.9839e-01 7.7314e-02 7.7342e-06 1.2100e-02 2.40490e-02 0.1250 1.2100e-02 2\n",
" 25 1.8707e-01 7.7314e-02 7.0576e-06 1.0968e-02 9.17291e-03 1.0000 1.0968e-02 2\n",
" END 8.5657e-02 7.7314e-02 6.4389e-06 8.2789e-04 ---- ---- 8.2789e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8459e+01 8.2708e-02 3.6521e+00 1.8562e-01 5.48083e-01 0.5000 ---- 3\n",
" 2 3.0599e+01 7.7873e-02 1.8499e+00 1.2022e+00 3.46807e-01 1.0000 1.2022e+00 3\n",
" 3 1.2417e+01 7.6997e-02 2.7605e-02 1.2064e+00 6.02000e-01 0.0312 1.2064e+00 3\n",
" 4 1.2195e+01 7.6993e-02 2.6743e-02 1.1851e+00 4.71521e-01 0.0625 1.1851e+00 3\n",
" 5 1.1982e+01 7.6989e-02 2.5073e-02 1.1654e+00 2.66474e-01 0.1250 1.1654e+00 3\n",
" 6 1.1011e+01 7.6988e-02 2.1938e-02 1.0715e+00 5.46710e-02 1.0000 1.0715e+00 3\n",
" 7 1.6603e+00 7.7000e-02 1.3849e-04 1.5819e-01 1.77305e-01 0.0156 1.5819e-01 3\n",
" 8 1.6446e+00 7.7000e-02 1.3628e-04 1.5663e-01 1.61867e-01 0.0312 1.5663e-01 3\n",
" 9 1.6437e+00 7.6999e-02 1.3184e-04 1.5654e-01 1.33185e-01 0.0312 1.5654e-01 3\n",
" 10 1.6153e+00 7.6998e-02 1.2759e-04 1.5370e-01 1.09470e-01 0.0625 1.5370e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.5892e+00 7.6997e-02 1.1921e-04 1.5110e-01 7.08816e-02 0.1250 1.5110e-01 3\n",
" 12 1.4750e+00 7.6997e-02 1.0361e-04 1.3970e-01 2.40872e-02 1.0000 1.3970e-01 3\n",
" 13 4.1007e-01 7.6998e-02 8.5004e-06 3.3299e-02 8.46834e-02 0.0625 3.3299e-02 3\n",
" 14 4.0017e-01 7.6998e-02 8.2001e-06 3.2309e-02 5.64197e-02 0.0625 3.2309e-02 3\n",
" 15 3.9022e-01 7.6998e-02 7.7976e-06 3.1315e-02 3.85879e-02 0.1250 3.1315e-02 3\n",
" 16 3.6849e-01 7.6997e-02 7.0442e-06 2.9142e-02 1.52956e-02 1.0000 2.9142e-02 3\n",
" 17 2.8179e-01 7.6997e-02 3.3317e-06 2.0476e-02 5.22645e-02 0.0625 2.0476e-02 2\n",
" 18 2.7064e-01 7.6997e-02 3.2110e-06 1.9361e-02 3.63462e-02 0.1250 1.9361e-02 2\n",
" 19 2.5128e-01 7.6997e-02 3.0186e-06 1.7425e-02 1.50314e-02 0.5000 1.7425e-02 2\n",
" END 1.6645e-01 7.6997e-02 2.3739e-06 8.9433e-03 ---- ---- 8.9433e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8248e+01 8.2380e-02 3.6299e+00 1.8669e-01 5.49469e-01 0.5000 ---- 3\n",
" 2 2.9862e+01 7.7608e-02 1.8362e+00 1.1422e+00 3.66295e-01 1.0000 1.1422e+00 3\n",
" 3 1.7165e+01 7.6794e-02 2.4830e-02 1.6840e+00 7.18484e-01 0.0312 1.6840e+00 3\n",
" 4 1.6798e+01 7.6789e-02 2.4056e-02 1.6481e+00 5.71038e-01 0.0625 1.6481e+00 3\n",
" 5 1.6422e+01 7.6783e-02 2.2557e-02 1.6120e+00 3.34291e-01 0.1250 1.6120e+00 3\n",
" 6 1.5119e+01 7.6780e-02 1.9740e-02 1.4845e+00 7.70427e-02 1.0000 1.4845e+00 3\n",
" 7 2.8450e+00 7.6797e-02 1.6416e-04 2.7665e-01 2.83121e-01 0.0312 2.7665e-01 3\n",
" 8 2.8075e+00 7.6795e-02 1.5838e-04 2.7291e-01 2.34079e-01 0.0312 2.7291e-01 3\n",
" 9 2.7565e+00 7.6794e-02 1.5298e-04 2.6782e-01 1.93249e-01 0.0625 2.6782e-01 3\n",
" 10 2.7139e+00 7.6793e-02 1.4218e-04 2.6357e-01 1.25837e-01 0.1250 2.6357e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.5498e+00 7.6792e-02 1.2253e-04 2.4718e-01 4.43164e-02 1.0000 2.4718e-01 3\n",
" 12 1.6072e+00 7.6794e-02 3.6745e-05 1.5301e-01 1.51958e-01 0.0625 1.5301e-01 3\n",
" 13 1.5971e+00 7.6793e-02 3.5745e-05 1.5200e-01 1.04820e-01 0.1250 1.5200e-01 3\n",
" 14 1.5723e+00 7.6792e-02 3.3877e-05 1.4952e-01 4.30258e-02 0.5000 1.4952e-01 3\n",
" 15 1.1866e+00 7.6792e-02 2.3828e-05 1.1096e-01 4.98470e-02 0.2500 1.1096e-01 3\n",
" 16 1.0285e+00 7.6792e-02 1.9781e-05 9.5149e-02 1.28395e-02 1.0000 9.5149e-02 3\n",
" 17 3.6543e-01 7.6792e-02 1.6053e-06 2.8862e-02 6.47516e-02 0.0156 2.8862e-02 3\n",
" 18 3.6214e-01 7.6792e-02 1.5894e-06 2.8533e-02 5.83641e-02 0.0312 2.8533e-02 3\n",
" 19 3.6008e-01 7.6792e-02 1.5706e-06 2.8327e-02 4.68187e-02 0.0312 2.8327e-02 3\n",
" 20 3.5376e-01 7.6792e-02 1.5428e-06 2.7695e-02 3.75541e-02 0.0625 2.7695e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.4522e-01 7.6792e-02 1.5049e-06 2.6841e-02 2.26794e-02 0.2500 2.6841e-02 3\n",
" 22 3.3867e-01 7.6792e-02 1.5232e-06 2.6186e-02 1.38902e-02 0.5000 2.6186e-02 3\n",
" 23 3.0977e-01 7.6792e-02 9.2398e-07 2.3297e-02 3.25988e-02 0.0625 2.3297e-02 3\n",
" 24 3.0232e-01 7.6792e-02 8.9319e-07 2.2552e-02 1.89620e-02 0.2500 2.2552e-02 3\n",
" 25 2.9548e-01 7.6792e-02 8.5894e-07 2.1868e-02 1.29645e-02 0.5000 2.1868e-02 3\n",
" 26 2.7164e-01 7.6792e-02 7.8356e-07 1.9484e-02 3.21965e-02 0.0625 1.9484e-02 3\n",
" 27 2.6594e-01 7.6792e-02 7.6568e-07 1.8914e-02 1.81920e-02 0.2500 1.8914e-02 3\n",
" 28 2.6298e-01 7.6791e-02 7.5476e-07 1.8618e-02 1.33854e-02 0.5000 1.8618e-02 3\n",
" 29 2.5126e-01 7.6791e-02 6.7972e-07 1.7446e-02 3.48491e-02 0.0625 1.7446e-02 3\n",
" 30 2.4737e-01 7.6791e-02 6.6428e-07 1.7058e-02 1.90243e-02 0.2500 1.7058e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.4684e-01 7.6791e-02 6.5418e-07 1.7004e-02 1.51475e-02 0.2500 1.7004e-02 3\n",
" 32 2.2013e-01 7.6791e-02 5.5447e-07 1.4333e-02 1.34452e-02 0.5000 1.4333e-02 3\n",
" 33 2.1743e-01 7.6791e-02 4.1608e-07 1.4063e-02 4.19411e-02 0.0020 1.4063e-02 1\n",
" 34 2.1743e-01 7.6791e-02 4.1608e-07 1.4063e-02 4.19201e-02 0.0312 1.4063e-02 1\n",
" 35 2.1624e-01 7.6791e-02 3.9004e-07 1.3945e-02 3.26579e-02 0.0156 1.3945e-02 1\n",
" 36 2.1582e-01 7.6791e-02 3.8147e-07 1.3903e-02 2.92253e-02 0.0625 1.3903e-02 1\n",
" 37 2.1463e-01 7.6791e-02 3.2737e-07 1.3784e-02 1.72961e-02 0.1250 1.3784e-02 1\n",
" 38 2.1056e-01 7.6791e-02 2.3696e-07 1.3377e-02 7.12608e-03 1.0000 1.3377e-02 1\n",
" END 1.2641e-01 7.6791e-02 2.6807e-06 4.9593e-03 ---- ---- 4.9593e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7914e+01 8.2168e-02 3.6144e+00 1.6882e-01 5.48467e-01 0.5000 ---- 3\n",
" 2 2.9913e+01 7.7436e-02 1.8265e+00 1.1571e+00 3.53867e-01 1.0000 1.1571e+00 3\n",
" 3 1.4545e+01 7.6666e-02 2.2367e-02 1.4244e+00 6.62908e-01 0.0312 1.4244e+00 3\n",
" 4 1.4280e+01 7.6662e-02 2.1669e-02 1.3987e+00 5.16749e-01 0.0625 1.3987e+00 3\n",
" 5 1.3998e+01 7.6657e-02 2.0317e-02 1.3718e+00 2.86574e-01 0.1250 1.3718e+00 3\n",
" 6 1.2761e+01 7.6656e-02 1.7777e-02 1.2506e+00 5.70700e-02 1.0000 1.2506e+00 3\n",
" 7 1.9369e+00 7.6672e-02 1.2955e-04 1.8589e-01 2.06682e-01 0.0156 1.8589e-01 3\n",
" 8 1.9202e+00 7.6671e-02 1.2752e-04 1.8423e-01 1.87875e-01 0.0156 1.8423e-01 3\n",
" 9 1.8985e+00 7.6671e-02 1.2552e-04 1.8205e-01 1.70642e-01 0.0312 1.8205e-01 3\n",
" 10 1.8859e+00 7.6670e-02 1.2156e-04 1.8080e-01 1.38580e-01 0.0312 1.8080e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.8455e+00 7.6669e-02 1.1773e-04 1.7677e-01 1.12344e-01 0.0625 1.7677e-01 3\n",
" 12 1.7957e+00 7.6669e-02 1.1029e-04 1.7179e-01 6.96107e-02 0.1250 1.7179e-01 3\n",
" 13 1.6347e+00 7.6669e-02 9.6437e-05 1.5571e-01 2.17517e-02 1.0000 1.5571e-01 3\n",
" 14 3.7097e-01 7.6670e-02 7.2006e-06 2.9423e-02 7.80450e-02 0.0625 2.9423e-02 3\n",
" 15 3.6051e-01 7.6670e-02 6.8880e-06 2.8377e-02 5.02480e-02 0.1250 2.8377e-02 3\n",
" 16 3.4384e-01 7.6669e-02 6.3569e-06 2.6711e-02 1.84346e-02 0.5000 2.6711e-02 3\n",
" 17 2.7504e-01 7.6669e-02 4.7844e-06 1.9832e-02 2.39884e-02 0.2500 1.9832e-02 3\n",
" 18 2.3201e-01 7.6669e-02 3.7789e-06 1.5531e-02 9.08082e-03 1.0000 1.5531e-02 3\n",
" END 1.2948e-01 7.6669e-02 1.0286e-06 5.2801e-03 ---- ---- 6.9164e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7905e+01 8.2042e-02 3.6073e+00 1.7505e-01 5.60822e-01 0.5000 ---- 3\n",
" 2 3.1752e+01 7.7339e-02 1.8212e+00 1.3462e+00 4.09316e-01 0.2500 1.3462e+00 3\n",
" 3 2.7138e+01 7.6778e-02 1.3669e+00 1.3392e+00 3.43106e-01 0.2500 1.3392e+00 3\n",
" 4 2.3889e+01 7.6519e-02 1.0258e+00 1.3555e+00 3.08078e-01 0.2500 1.3555e+00 3\n",
" 5 2.2470e+01 7.6416e-02 7.6964e-01 1.4697e+00 2.88875e-01 0.2500 1.4697e+00 3\n",
" 6 2.2016e+01 7.6389e-02 5.7742e-01 1.6166e+00 2.68792e-01 0.2500 1.6166e+00 3\n",
" 7 2.1958e+01 7.6397e-02 4.3316e-01 1.7550e+00 2.68236e-01 0.2500 1.7550e+00 3\n",
" 8 2.1739e+01 7.6420e-02 3.2493e-01 1.8413e+00 2.61743e-01 0.2500 1.8413e+00 3\n",
" 9 2.1576e+01 7.6446e-02 2.4373e-01 1.9062e+00 2.73395e-01 0.1250 1.9062e+00 3\n",
" 10 1.9579e+01 7.6458e-02 2.1327e-01 1.7370e+00 7.00940e-02 1.0000 1.7370e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 8.3024e-01 7.6605e-02 3.4728e-04 7.5016e-02 1.56988e-01 0.0156 7.5016e-02 3\n",
" 12 8.2311e-01 7.6605e-02 3.4186e-04 7.4308e-02 1.35722e-01 0.0312 7.4308e-02 3\n",
" 13 8.1912e-01 7.6604e-02 3.3120e-04 7.3920e-02 9.90310e-02 0.0625 7.3920e-02 3\n",
" 14 8.1074e-01 7.6604e-02 3.1054e-04 7.3103e-02 4.62500e-02 0.2500 7.3103e-02 3\n",
" 15 6.9485e-01 7.6604e-02 2.3311e-04 6.1591e-02 5.65713e-02 0.1250 6.1591e-02 3\n",
" 16 6.5608e-01 7.6604e-02 2.0391e-04 5.7743e-02 1.51460e-02 1.0000 5.7743e-02 3\n",
" END 1.4292e-01 7.6604e-02 1.2664e-06 6.6307e-03 ---- ---- 7.5950e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7835e+01 8.1969e-02 3.5953e+00 1.7997e-01 6.09425e-01 0.5000 ---- 3\n",
" 2 3.5391e+01 7.7284e-02 1.8142e+00 1.7173e+00 6.19963e-01 0.1250 1.7173e+00 3\n",
" 3 3.5178e+01 7.6963e-02 1.5876e+00 1.9225e+00 2.79335e-01 1.0000 1.9225e+00 3\n",
" 4 5.4356e+00 7.6568e-02 1.4572e-02 5.2133e-01 4.71972e-01 0.0156 5.2133e-01 3\n",
" 5 5.4025e+00 7.6565e-02 1.4344e-02 5.1825e-01 4.10187e-01 0.0156 5.1825e-01 3\n",
" 6 5.3463e+00 7.6563e-02 1.4120e-02 5.1285e-01 3.57063e-01 0.0312 5.1285e-01 3\n",
" 7 5.3111e+00 7.6561e-02 1.3679e-02 5.0978e-01 2.65613e-01 0.0625 5.0978e-01 3\n",
" 8 5.2683e+00 7.6558e-02 1.2824e-02 5.0635e-01 1.29012e-01 0.2500 5.0635e-01 3\n",
" 9 4.6747e+00 7.6558e-02 9.6152e-03 4.5020e-01 1.41640e-01 0.1250 4.5020e-01 3\n",
" 10 4.3716e+00 7.6557e-02 8.4135e-03 4.2109e-01 3.48820e-02 1.0000 4.2109e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 8.2638e-01 7.6564e-02 1.9614e-05 7.4962e-02 2.00100e-01 0.0078 7.4962e-02 3\n",
" 12 8.2366e-01 7.6564e-02 1.9463e-05 7.4690e-02 1.85807e-01 0.0078 7.4690e-02 3\n",
" 13 8.2005e-01 7.6563e-02 1.9313e-05 7.4330e-02 1.71848e-01 0.0078 7.4330e-02 3\n",
" 14 8.1567e-01 7.6563e-02 1.9164e-05 7.3892e-02 1.53154e-01 0.0156 7.3892e-02 3\n",
" 15 8.1240e-01 7.6563e-02 1.8869e-05 7.3565e-02 1.32226e-01 0.0156 7.3565e-02 3\n",
" 16 8.0532e-01 7.6563e-02 1.8578e-05 7.2857e-02 1.14203e-01 0.0312 7.2857e-02 3\n",
" 17 8.0152e-01 7.6562e-02 1.8008e-05 7.2478e-02 8.31364e-02 0.0625 7.2478e-02 3\n",
" 18 7.9500e-01 7.6562e-02 1.6907e-05 7.1827e-02 3.84676e-02 0.2500 7.1827e-02 3\n",
" 19 6.9622e-01 7.6562e-02 1.2813e-05 6.1953e-02 4.50592e-02 0.1250 6.1953e-02 3\n",
" 20 6.5727e-01 7.6562e-02 1.1201e-05 5.8060e-02 8.97573e-03 1.0000 5.8060e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 9.8663e-02 7.6563e-02 3.3616e-07 2.2097e-03 ---- ---- 2.2097e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7972e+01 8.1929e-02 3.6323e+00 1.5671e-01 5.93901e-01 0.5000 ---- 3\n",
" 2 3.2215e+01 7.7438e-02 1.8273e+00 1.3864e+00 5.07336e-01 0.1250 1.3864e+00 3\n",
" 3 3.0117e+01 7.7155e-02 1.5991e+00 1.4050e+00 2.70369e-01 1.0000 1.4050e+00 3\n",
" 4 4.5541e+00 7.6956e-02 1.0371e-02 4.3735e-01 3.79439e-01 0.0156 4.3735e-01 3\n",
" 5 4.5363e+00 7.6954e-02 1.0209e-02 4.3572e-01 3.30626e-01 0.0156 4.3572e-01 3\n",
" 6 4.4952e+00 7.6953e-02 1.0049e-02 4.3177e-01 2.87867e-01 0.0312 4.3177e-01 3\n",
" 7 4.4793e+00 7.6951e-02 9.7353e-03 4.3050e-01 2.14968e-01 0.0625 4.3050e-01 3\n",
" 8 4.4537e+00 7.6950e-02 9.1265e-03 4.2854e-01 1.06001e-01 0.2500 4.2854e-01 3\n",
" 9 3.8490e+00 7.6950e-02 6.8419e-03 3.7036e-01 1.10984e-01 0.1250 3.7036e-01 3\n",
" 10 3.6212e+00 7.6949e-02 5.9870e-03 3.4843e-01 2.96512e-02 1.0000 3.4843e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.3042e-01 7.6954e-02 1.8401e-05 2.5328e-02 1.68997e-01 0.0039 2.5328e-02 3\n",
" 12 3.2981e-01 7.6954e-02 1.8330e-05 2.5267e-02 1.62901e-01 0.0039 2.5267e-02 3\n",
" 13 3.2913e-01 7.6954e-02 1.8258e-05 2.5200e-02 1.56417e-01 0.0078 2.5200e-02 3\n",
" 14 3.2909e-01 7.6953e-02 1.8117e-05 2.5196e-02 1.39352e-01 0.0078 2.5196e-02 3\n",
" 15 3.2845e-01 7.6953e-02 1.7977e-05 2.5132e-02 9.25272e-02 0.0156 2.5132e-02 3\n",
" 16 3.2660e-01 7.6953e-02 1.7698e-05 2.4947e-02 8.35004e-02 0.0156 2.4947e-02 3\n",
" 17 3.2404e-01 7.6953e-02 1.7423e-05 2.4691e-02 7.53630e-02 0.0312 2.4691e-02 3\n",
" 18 3.2275e-01 7.6953e-02 1.6883e-05 2.4563e-02 6.06762e-02 0.0312 2.4563e-02 3\n",
" 19 3.1786e-01 7.6953e-02 1.6359e-05 2.4074e-02 4.89621e-02 0.0625 2.4074e-02 3\n",
" 20 3.1209e-01 7.6952e-02 1.5344e-05 2.3499e-02 3.02415e-02 0.1250 2.3499e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.9154e-01 7.6952e-02 1.3442e-05 2.1446e-02 8.25949e-03 1.0000 2.1446e-02 3\n",
" END 1.0269e-01 7.6953e-02 5.0965e-07 2.5736e-03 ---- ---- 2.5736e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8299e+01 8.2347e-02 3.6679e+00 1.5382e-01 5.79087e-01 0.5000 ---- 3\n",
" 2 3.0824e+01 7.7884e-02 1.8451e+00 1.2294e+00 4.23365e-01 0.2500 1.2294e+00 3\n",
" 3 2.6461e+01 7.7394e-02 1.3846e+00 1.2538e+00 3.75146e-01 0.2500 1.2538e+00 3\n",
" 4 2.5352e+01 7.7188e-02 1.0388e+00 1.4887e+00 3.80327e-01 0.1250 1.4887e+00 3\n",
" 5 2.3924e+01 7.7145e-02 9.0901e-01 1.4757e+00 1.66059e-01 1.0000 1.4757e+00 3\n",
" 6 1.6428e+00 7.7437e-02 3.3164e-03 1.5322e-01 3.29610e-01 0.0078 1.5322e-01 3\n",
" 7 1.6374e+00 7.7437e-02 3.2905e-03 1.5271e-01 3.05948e-01 0.0078 1.5271e-01 3\n",
" 8 1.6303e+00 7.7436e-02 3.2648e-03 1.5202e-01 2.82800e-01 0.0078 1.5202e-01 3\n",
" 9 1.6216e+00 7.7436e-02 3.2393e-03 1.5118e-01 2.51995e-01 0.0156 1.5118e-01 3\n",
" 10 1.6160e+00 7.7435e-02 3.1887e-03 1.5067e-01 2.18220e-01 0.0156 1.5067e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.6020e+00 7.7434e-02 3.1389e-03 1.4932e-01 1.89045e-01 0.0312 1.4932e-01 3\n",
" 12 1.5959e+00 7.7433e-02 3.0408e-03 1.4880e-01 1.38160e-01 0.0625 1.4880e-01 3\n",
" 13 1.5839e+00 7.7433e-02 2.8507e-03 1.4780e-01 6.47261e-02 0.2500 1.4780e-01 3\n",
" 14 1.3666e+00 7.7433e-02 2.1378e-03 1.2678e-01 7.49958e-02 0.1250 1.2678e-01 3\n",
" 15 1.2830e+00 7.7432e-02 1.8706e-03 1.1869e-01 1.80151e-02 1.0000 1.1869e-01 3\n",
" END 9.7957e-02 7.7434e-02 3.2278e-06 2.0491e-03 ---- ---- 2.0491e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8301e+01 8.2851e-02 3.6553e+00 1.6649e-01 6.31775e-01 0.2500 ---- 3\n",
" 2 3.2946e+01 7.9942e-02 2.7471e+00 5.3955e-01 5.46803e-01 0.2500 8.7073e-01 3\n",
" 3 3.0576e+01 7.8412e-02 2.0634e+00 9.8641e-01 4.97038e-01 0.1250 9.8641e-01 3\n",
" 4 2.8749e+01 7.7979e-02 1.8059e+00 1.0612e+00 3.01114e-01 1.0000 1.0612e+00 3\n",
" 5 5.3956e+00 7.7195e-02 2.4926e-02 5.0692e-01 4.20772e-01 0.0156 5.0692e-01 3\n",
" 6 5.3462e+00 7.7193e-02 2.4537e-02 5.0236e-01 3.61807e-01 0.0312 5.0236e-01 3\n",
" 7 5.3223e+00 7.7190e-02 2.3770e-02 5.0075e-01 2.60728e-01 0.0625 5.0075e-01 3\n",
" 8 5.2796e+00 7.7188e-02 2.2284e-02 4.9796e-01 1.20274e-01 0.2500 4.9796e-01 3\n",
" 9 4.9688e+00 7.7187e-02 1.6714e-02 4.7245e-01 1.45389e-01 0.1250 4.7245e-01 3\n",
" 10 4.8936e+00 7.7186e-02 1.4625e-02 4.6702e-01 4.35317e-02 1.0000 4.6702e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.1438e+00 7.7193e-02 3.0486e-05 1.0663e-01 2.65738e-01 0.0039 1.0663e-01 3\n",
" 12 1.1409e+00 7.7193e-02 3.0368e-05 1.0634e-01 2.55565e-01 0.0039 1.0634e-01 3\n",
" 13 1.1378e+00 7.7193e-02 3.0250e-05 1.0603e-01 2.45054e-01 0.0078 1.0603e-01 3\n",
" 14 1.1371e+00 7.7192e-02 3.0017e-05 1.0596e-01 2.17975e-01 0.0078 1.0596e-01 3\n",
" 15 1.1339e+00 7.7192e-02 2.9785e-05 1.0564e-01 1.45272e-01 0.0156 1.0564e-01 3\n",
" 16 1.1251e+00 7.7191e-02 2.9323e-05 1.0477e-01 1.30558e-01 0.0156 1.0477e-01 3\n",
" 17 1.1133e+00 7.7191e-02 2.8868e-05 1.0358e-01 1.17307e-01 0.0312 1.0358e-01 3\n",
" 18 1.1065e+00 7.7191e-02 2.7973e-05 1.0290e-01 9.39025e-02 0.0312 1.0290e-01 3\n",
" 19 1.0837e+00 7.7190e-02 2.7103e-05 1.0062e-01 7.52256e-02 0.0625 1.0062e-01 3\n",
" 20 1.0533e+00 7.7190e-02 2.5417e-05 9.7582e-02 4.55346e-02 0.2500 9.7582e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.0360e+00 7.7190e-02 1.9090e-05 9.5867e-02 3.00507e-02 0.5000 9.5867e-02 3\n",
" 22 1.0317e+00 7.7190e-02 9.6204e-06 9.5442e-02 6.77631e-02 0.0625 9.5442e-02 3\n",
" 23 1.0070e+00 7.7190e-02 9.0269e-06 9.2974e-02 3.93474e-02 0.2500 9.2974e-02 3\n",
" 24 1.0026e+00 7.7190e-02 6.7886e-06 9.2538e-02 2.75788e-02 0.2500 9.2538e-02 3\n",
" 25 8.2706e-01 7.7190e-02 5.1010e-06 7.4981e-02 1.97653e-02 0.5000 7.4981e-02 3\n",
" 26 6.2665e-01 7.7190e-02 2.5558e-06 5.4943e-02 4.69531e-02 0.0625 5.4943e-02 3\n",
" 27 6.0940e-01 7.7190e-02 2.3962e-06 5.3219e-02 2.69296e-02 0.2500 5.3219e-02 3\n",
" 28 5.8970e-01 7.7190e-02 1.8077e-06 5.1250e-02 1.92834e-02 0.5000 5.1250e-02 3\n",
" 29 5.8007e-01 7.7190e-02 9.3288e-07 5.0287e-02 4.65361e-02 0.0625 5.0287e-02 3\n",
" 30 5.6765e-01 7.7190e-02 8.7752e-07 4.9045e-02 2.66646e-02 0.2500 4.9045e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 5.6559e-01 7.7190e-02 6.7947e-07 4.8839e-02 1.91171e-02 0.2500 4.8839e-02 3\n",
" 32 4.7675e-01 7.7190e-02 5.2206e-07 3.9955e-02 1.37195e-02 0.5000 3.9955e-02 3\n",
" 33 3.7900e-01 7.7190e-02 2.9244e-07 3.0181e-02 3.33045e-02 0.1250 3.0181e-02 2\n",
" 34 3.7725e-01 7.7190e-02 2.6742e-07 3.0006e-02 4.82868e-03 1.0000 3.0006e-02 2\n",
" END 8.2020e-02 7.7191e-02 2.9742e-08 4.8290e-04 ---- ---- 9.2033e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8096e+01 8.2602e-02 3.6428e+00 1.5850e-01 5.80022e-01 0.5000 ---- 3\n",
" 2 3.2311e+01 7.7835e-02 1.8413e+00 1.3820e+00 4.83618e-01 0.2500 1.3820e+00 3\n",
" 3 3.0250e+01 7.7258e-02 1.3821e+00 1.6351e+00 4.54632e-01 0.1250 1.6351e+00 3\n",
" 4 2.8792e+01 7.7103e-02 1.2095e+00 1.6620e+00 2.22485e-01 1.0000 1.6620e+00 3\n",
" 5 3.6196e+00 7.7051e-02 1.0014e-02 3.4424e-01 4.45790e-01 0.0078 3.4424e-01 3\n",
" 6 3.6047e+00 7.7050e-02 9.9358e-03 3.4283e-01 4.12467e-01 0.0078 3.4283e-01 3\n",
" 7 3.5861e+00 7.7049e-02 9.8581e-03 3.4104e-01 3.80446e-01 0.0156 3.4104e-01 3\n",
" 8 3.5837e+00 7.7047e-02 9.7041e-03 3.4096e-01 3.24277e-01 0.0156 3.4096e-01 3\n",
" 9 3.5587e+00 7.7045e-02 9.5525e-03 3.3862e-01 2.76240e-01 0.0156 3.3862e-01 3\n",
" 10 3.5176e+00 7.7044e-02 9.4032e-03 3.3465e-01 2.36365e-01 0.0312 3.3465e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.4769e+00 7.7043e-02 9.1094e-03 3.3088e-01 1.70067e-01 0.0625 3.3088e-01 3\n",
" 12 3.4398e+00 7.7042e-02 8.5400e-03 3.2773e-01 7.52510e-02 0.2500 3.2773e-01 3\n",
" 13 2.9059e+00 7.7042e-02 6.4048e-03 2.7648e-01 9.59773e-02 0.1250 2.7648e-01 3\n",
" 14 2.7952e+00 7.7042e-02 5.6044e-03 2.6621e-01 2.84405e-02 1.0000 2.6621e-01 3\n",
" 15 6.3170e-01 7.7046e-02 1.0200e-05 5.5455e-02 1.88271e-01 0.0078 5.5455e-02 3\n",
" 16 6.2975e-01 7.7046e-02 1.0122e-05 5.5260e-02 1.73092e-01 0.0078 5.5260e-02 3\n",
" 17 6.2707e-01 7.7045e-02 1.0045e-05 5.4993e-02 1.53532e-01 0.0156 5.4993e-02 3\n",
" 18 6.2610e-01 7.7045e-02 9.8931e-06 5.4896e-02 1.31287e-01 0.0156 5.4896e-02 3\n",
" 19 6.2176e-01 7.7045e-02 9.7418e-06 5.4462e-02 1.12192e-01 0.0312 5.4462e-02 3\n",
" 20 6.2078e-01 7.7044e-02 9.4457e-06 5.4364e-02 7.95017e-02 0.0625 5.4364e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 6.1710e-01 7.7044e-02 8.8680e-06 5.3997e-02 3.35281e-02 0.2500 5.3997e-02 3\n",
" 22 5.5250e-01 7.7044e-02 6.6728e-06 4.7539e-02 4.34149e-02 0.1250 4.7539e-02 3\n",
" 23 5.5022e-01 7.7044e-02 5.8408e-06 4.7312e-02 1.00560e-02 1.0000 4.7312e-02 3\n",
" 24 2.2703e-01 7.7045e-02 1.1908e-07 1.4998e-02 7.99234e-02 0.0156 1.4998e-02 2\n",
" 25 2.2603e-01 7.7044e-02 1.1829e-07 1.4898e-02 6.74971e-02 0.0156 1.4898e-02 2\n",
" 26 2.2381e-01 7.7044e-02 1.1718e-07 1.4677e-02 5.70199e-02 0.0312 1.4677e-02 2\n",
" 27 2.2200e-01 7.7044e-02 1.1557e-07 1.4495e-02 3.93654e-02 0.0625 1.4495e-02 2\n",
" 28 2.2011e-01 7.7044e-02 1.1215e-07 1.4307e-02 1.50626e-02 0.2500 1.4307e-02 2\n",
" 29 1.8155e-01 7.7044e-02 9.3038e-08 1.0450e-02 2.78973e-02 0.0020 1.0450e-02 1\n",
" 30 1.8155e-01 7.7044e-02 9.3038e-08 1.0450e-02 2.66098e-02 0.0625 1.0450e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.7901e-01 7.7044e-02 9.0981e-08 1.0196e-02 1.39243e-02 0.1250 1.0196e-02 1\n",
" 32 1.7800e-01 7.7044e-02 1.2101e-07 1.0095e-02 7.30870e-03 1.0000 1.0095e-02 1\n",
" END 1.0835e-01 7.7044e-02 2.3781e-06 3.1278e-03 ---- ---- 3.1278e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8145e+01 8.2450e-02 3.6285e+00 1.7775e-01 5.63353e-01 0.5000 ---- 3\n",
" 2 3.0687e+01 7.7717e-02 1.8325e+00 1.2284e+00 4.04415e-01 0.5000 1.2284e+00 3\n",
" 3 2.1937e+01 7.6840e-02 9.2052e-01 1.2655e+00 5.92832e-01 0.0625 1.2655e+00 3\n",
" 4 2.1757e+01 7.6812e-02 8.6300e-01 1.3050e+00 3.16767e-01 0.2500 1.3050e+00 3\n",
" 5 2.0007e+01 7.6763e-02 6.4748e-01 1.3455e+00 3.45512e-01 0.1250 1.3455e+00 3\n",
" 6 1.8582e+01 7.6757e-02 5.6658e-01 1.2840e+00 1.19277e-01 1.0000 1.2840e+00 3\n",
" 7 1.1914e+00 7.6967e-02 2.0362e-03 1.0940e-01 3.58082e-01 0.0039 1.0940e-01 3\n",
" 8 1.1908e+00 7.6966e-02 2.0282e-03 1.0936e-01 3.45202e-01 0.0039 1.0936e-01 3\n",
" 9 1.1899e+00 7.6966e-02 2.0203e-03 1.0927e-01 3.31580e-01 0.0039 1.0927e-01 3\n",
" 10 1.1884e+00 7.6965e-02 2.0124e-03 1.0913e-01 3.06837e-01 0.0039 1.0913e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.1862e+00 7.6965e-02 2.0046e-03 1.0892e-01 2.11574e-01 0.0078 1.0892e-01 3\n",
" 12 1.1814e+00 7.6965e-02 1.9889e-03 1.0845e-01 5.43359e-02 0.1250 1.0845e-01 3\n",
" 13 1.1083e+00 7.6963e-02 1.7403e-03 1.0139e-01 4.25783e-02 0.2500 1.0139e-01 3\n",
" 14 1.0503e+00 7.6961e-02 1.3052e-03 9.6024e-02 2.51515e-02 0.5000 9.6024e-02 3\n",
" 15 7.0017e-01 7.6961e-02 6.5255e-04 6.1669e-02 6.68127e-03 1.0000 6.1669e-02 3\n",
" END 9.0832e-02 7.6962e-02 4.5872e-07 1.3866e-03 ---- ---- 1.3866e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8019e+01 8.2365e-02 3.6232e+00 1.7054e-01 5.46829e-01 0.5000 ---- 3\n",
" 2 3.0565e+01 7.7656e-02 1.8283e+00 1.2204e+00 3.68839e-01 1.0000 1.2204e+00 3\n",
" 3 2.0277e+01 7.6921e-02 1.9736e-02 2.0003e+00 8.89212e-01 0.0312 2.0003e+00 3\n",
" 4 2.0009e+01 7.6914e-02 1.9122e-02 1.9741e+00 6.71336e-01 0.0625 1.9741e+00 3\n",
" 5 1.9784e+01 7.6906e-02 1.7933e-02 1.9528e+00 3.41226e-01 0.1250 1.9528e+00 3\n",
" 6 1.7932e+01 7.6904e-02 1.5694e-02 1.7698e+00 7.01203e-02 1.0000 1.7698e+00 3\n",
" 7 8.4822e-01 7.6931e-02 1.5258e-04 7.6976e-02 3.14190e-01 0.0078 7.6976e-02 3\n",
" 8 8.4702e-01 7.6930e-02 1.5139e-04 7.6857e-02 2.94988e-01 0.0078 7.6857e-02 3\n",
" 9 8.4495e-01 7.6930e-02 1.5022e-04 7.6651e-02 2.75933e-01 0.0078 7.6651e-02 3\n",
" 10 8.4205e-01 7.6930e-02 1.4905e-04 7.6363e-02 2.48787e-01 0.0078 7.6363e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 8.3802e-01 7.6929e-02 1.4790e-04 7.5961e-02 1.67363e-01 0.0156 7.5961e-02 3\n",
" 12 8.2903e-01 7.6929e-02 1.4560e-04 7.5064e-02 1.53741e-01 0.0312 7.5064e-02 3\n",
" 13 8.2545e-01 7.6928e-02 1.4109e-04 7.4711e-02 1.29016e-01 0.0312 7.4711e-02 3\n",
" 14 8.1275e-01 7.6928e-02 1.3671e-04 7.3446e-02 1.08611e-01 0.0625 7.3446e-02 3\n",
" 15 8.1003e-01 7.6927e-02 1.2824e-04 7.3182e-02 7.46389e-02 0.1250 7.3182e-02 3\n",
" 16 8.0163e-01 7.6926e-02 1.1235e-04 7.2358e-02 3.12386e-02 0.5000 7.2358e-02 3\n",
" 17 6.0355e-01 7.6926e-02 5.7433e-05 5.2605e-02 3.55908e-02 0.2500 5.2605e-02 3\n",
" 18 5.4218e-01 7.6926e-02 4.2659e-05 4.6483e-02 1.27951e-02 1.0000 4.6483e-02 3\n",
" END 8.3110e-02 7.6926e-02 6.9016e-07 6.1763e-04 ---- ---- 6.4012e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8216e+01 8.2324e-02 3.6600e+00 1.5342e-01 6.02012e-01 0.5000 ---- 3\n",
" 2 3.2476e+01 7.7807e-02 1.8412e+00 1.3986e+00 4.98332e-01 0.2500 1.3986e+00 3\n",
" 3 3.0157e+01 7.7304e-02 1.3816e+00 1.6264e+00 4.49675e-01 0.1250 1.6264e+00 3\n",
" 4 2.7891e+01 7.7178e-02 1.2090e+00 1.5725e+00 2.12474e-01 1.0000 1.5725e+00 3\n",
" 5 2.3569e+00 7.7320e-02 5.6689e-03 2.2229e-01 3.75875e-01 0.0078 2.2229e-01 3\n",
" 6 2.3448e+00 7.7319e-02 5.6246e-03 2.2112e-01 3.50643e-01 0.0156 2.2112e-01 3\n",
" 7 2.3435e+00 7.7317e-02 5.5367e-03 2.2109e-01 3.04091e-01 0.0156 2.2109e-01 3\n",
" 8 2.3273e+00 7.7316e-02 5.4502e-03 2.1955e-01 2.64898e-01 0.0156 2.1955e-01 3\n",
" 9 2.3004e+00 7.7316e-02 5.3651e-03 2.1694e-01 2.30607e-01 0.0312 2.1694e-01 3\n",
" 10 2.2722e+00 7.7315e-02 5.1974e-03 2.1429e-01 1.69899e-01 0.0625 2.1429e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.2105e+00 7.7314e-02 4.8727e-03 2.0845e-01 8.02613e-02 0.5000 2.0845e-01 3\n",
" 12 1.9351e+00 7.7314e-02 2.4389e-03 1.8334e-01 2.63876e-01 0.0312 1.8334e-01 3\n",
" 13 1.9263e+00 7.7313e-02 2.3627e-03 1.8254e-01 1.86762e-01 0.0625 1.8254e-01 3\n",
" 14 1.9071e+00 7.7312e-02 2.2151e-03 1.8076e-01 7.95365e-02 0.2500 1.8076e-01 3\n",
" 15 1.5922e+00 7.7312e-02 1.6613e-03 1.4983e-01 9.61334e-02 0.1250 1.4983e-01 3\n",
" 16 1.5161e+00 7.7312e-02 1.4537e-03 1.4243e-01 1.87736e-02 1.0000 1.4243e-01 3\n",
" 17 4.2256e-01 7.7314e-02 1.2844e-06 3.4523e-02 1.54032e-01 0.0039 3.4523e-02 3\n",
" 18 4.2187e-01 7.7314e-02 1.2793e-06 3.4455e-02 1.48364e-01 0.0039 3.4455e-02 3\n",
" 19 4.2109e-01 7.7314e-02 1.2742e-06 3.4376e-02 1.42334e-01 0.0039 3.4376e-02 3\n",
" 20 4.2021e-01 7.7314e-02 1.2691e-06 3.4288e-02 1.31524e-01 0.0078 3.4288e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.2002e-01 7.7313e-02 1.2589e-06 3.4270e-02 8.70612e-02 0.0156 3.4270e-02 3\n",
" 22 4.1866e-01 7.7313e-02 1.2384e-06 3.4133e-02 7.83437e-02 0.0156 3.4133e-02 3\n",
" 23 4.1596e-01 7.7313e-02 1.2183e-06 3.3864e-02 7.04533e-02 0.0156 3.3864e-02 3\n",
" 24 4.1225e-01 7.7313e-02 1.1985e-06 3.3493e-02 6.33396e-02 0.0312 3.3493e-02 3\n",
" 25 4.0993e-01 7.7313e-02 1.1586e-06 3.3260e-02 5.04554e-02 0.0312 3.3260e-02 3\n",
" 26 4.0253e-01 7.7313e-02 1.1207e-06 3.2521e-02 4.01280e-02 0.0625 3.2521e-02 3\n",
" 27 3.9204e-01 7.7313e-02 1.0457e-06 3.1471e-02 2.35720e-02 0.2500 3.1471e-02 3\n",
" 28 3.7489e-01 7.7313e-02 7.5420e-07 2.9757e-02 1.60235e-02 0.2500 2.9757e-02 3\n",
" 29 3.2121e-01 7.7313e-02 5.5721e-07 2.4389e-02 1.14950e-02 0.5000 2.4389e-02 3\n",
" 30 2.8255e-01 7.7313e-02 2.8574e-07 2.0523e-02 2.82574e-02 0.0625 2.0523e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.7078e-01 7.7313e-02 2.6983e-07 1.9347e-02 1.58284e-02 0.2500 1.9347e-02 2\n",
" 32 2.4131e-01 7.7313e-02 2.1022e-07 1.6399e-02 1.19011e-02 0.2500 1.6399e-02 2\n",
" 33 1.9799e-01 7.7313e-02 1.6421e-07 1.2068e-02 8.93039e-03 0.5000 1.2068e-02 2\n",
" END 1.7650e-01 7.7313e-02 9.6155e-08 9.9184e-03 ---- ---- 9.9184e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8858e+01 8.2744e-02 3.6961e+00 1.8144e-01 5.82678e-01 0.5000 ---- 3\n",
" 2 3.2193e+01 7.8255e-02 1.8593e+00 1.3521e+00 4.37278e-01 0.2500 1.3521e+00 3\n",
" 3 2.9143e+01 7.7761e-02 1.3952e+00 1.5114e+00 4.04614e-01 0.1250 1.5114e+00 3\n",
" 4 2.7531e+01 7.7638e-02 1.2209e+00 1.5244e+00 2.17353e-01 1.0000 1.5244e+00 3\n",
" 5 4.4339e+00 7.7806e-02 5.9011e-03 4.2971e-01 4.40020e-01 0.0078 4.2971e-01 3\n",
" 6 4.4142e+00 7.7804e-02 5.8551e-03 4.2779e-01 4.04190e-01 0.0078 4.2779e-01 3\n",
" 7 4.3903e+00 7.7803e-02 5.8093e-03 4.2544e-01 3.69589e-01 0.0156 4.2544e-01 3\n",
" 8 4.3857e+00 7.7801e-02 5.7186e-03 4.2507e-01 3.11139e-01 0.0156 4.2507e-01 3\n",
" 9 4.3553e+00 7.7799e-02 5.6292e-03 4.2212e-01 2.63625e-01 0.0156 4.2212e-01 3\n",
" 10 4.3093e+00 7.7798e-02 5.5413e-03 4.1761e-01 2.23335e-01 0.0312 4.1761e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 4.2760e+00 7.7797e-02 5.3681e-03 4.1445e-01 1.55998e-01 0.0625 4.1445e-01 3\n",
" 12 4.2224e+00 7.7796e-02 5.0327e-03 4.0943e-01 6.58611e-02 0.2500 4.0943e-01 3\n",
" 13 3.4169e+00 7.7797e-02 3.7746e-03 3.3014e-01 9.01193e-02 0.1250 3.3014e-01 3\n",
" 14 3.2447e+00 7.7796e-02 3.3028e-03 3.1339e-01 2.83676e-02 1.0000 3.1339e-01 3\n",
" 15 6.3562e-01 7.7802e-02 7.2974e-06 5.5774e-02 2.08168e-01 0.0039 5.5774e-02 3\n",
" 16 6.3538e-01 7.7801e-02 7.2698e-06 5.5751e-02 1.98970e-01 0.0039 5.5751e-02 3\n",
" 17 6.3488e-01 7.7801e-02 7.2421e-06 5.5701e-02 1.83404e-01 0.0039 5.5701e-02 3\n",
" 18 6.3397e-01 7.7801e-02 7.2145e-06 5.5610e-02 1.26928e-01 0.0078 5.5610e-02 3\n",
" 19 6.3192e-01 7.7801e-02 7.1594e-06 5.5405e-02 3.28139e-02 0.1250 5.5405e-02 3\n",
" 20 6.0168e-01 7.7800e-02 6.2740e-06 5.2382e-02 2.47741e-02 0.2500 5.2382e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 5.7858e-01 7.7799e-02 4.7240e-06 5.0074e-02 1.31566e-02 0.5000 5.0074e-02 3\n",
" 22 3.9421e-01 7.7799e-02 2.3825e-06 3.1639e-02 2.69420e-03 1.0000 3.1639e-02 3\n",
" END 9.1576e-02 7.7799e-02 7.2669e-08 1.3776e-03 ---- ---- 1.3776e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8491e+01 8.3259e-02 3.6764e+00 1.6433e-01 5.66428e-01 0.5000 ---- 3\n",
" 2 3.0624e+01 7.8409e-02 1.8609e+00 1.1937e+00 3.82514e-01 1.0000 1.1937e+00 3\n",
" 3 2.2778e+01 7.7551e-02 2.6043e-02 2.2440e+00 9.29928e-01 0.0312 2.2440e+00 3\n",
" 4 2.2429e+01 7.7544e-02 2.5233e-02 2.2099e+00 7.06027e-01 0.0625 2.2099e+00 3\n",
" 5 2.2109e+01 7.7535e-02 2.3664e-02 2.1795e+00 3.62631e-01 0.1250 2.1795e+00 3\n",
" 6 2.0111e+01 7.7532e-02 2.0710e-02 1.9827e+00 7.77033e-02 1.0000 1.9827e+00 3\n",
" 7 1.6231e+00 7.7561e-02 1.7752e-04 1.5438e-01 3.55898e-01 0.0078 1.5438e-01 3\n",
" 8 1.6156e+00 7.7561e-02 1.7615e-04 1.5363e-01 3.34639e-01 0.0078 1.5363e-01 3\n",
" 9 1.6070e+00 7.7560e-02 1.7479e-04 1.5277e-01 3.13412e-01 0.0156 1.5277e-01 3\n",
" 10 1.6064e+00 7.7559e-02 1.7210e-04 1.5271e-01 2.76331e-01 0.0156 1.5271e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.5981e+00 7.7558e-02 1.6944e-04 1.5189e-01 2.43819e-01 0.0156 1.5189e-01 3\n",
" 12 1.5840e+00 7.7558e-02 1.6682e-04 1.5048e-01 2.15677e-01 0.0312 1.5048e-01 3\n",
" 13 1.5837e+00 7.7557e-02 1.6167e-04 1.5045e-01 1.65658e-01 0.0312 1.5045e-01 3\n",
" 14 1.5536e+00 7.7556e-02 1.5665e-04 1.4745e-01 1.27816e-01 0.0625 1.4745e-01 3\n",
" 15 1.5142e+00 7.7556e-02 1.4695e-04 1.4352e-01 7.05538e-02 0.2500 1.4352e-01 3\n",
" 16 1.4717e+00 7.7555e-02 1.1099e-04 1.3930e-01 5.65179e-02 0.2500 1.3930e-01 3\n",
" 17 1.2149e+00 7.7555e-02 8.2908e-05 1.1365e-01 6.01594e-02 0.2500 1.1365e-01 3\n",
" 18 9.9136e-01 7.7555e-02 6.2704e-05 9.1318e-02 6.89205e-02 0.2500 9.1318e-02 3\n",
" 19 8.2525e-01 7.7555e-02 4.7149e-05 7.4722e-02 8.84406e-02 0.1250 7.4722e-02 3\n",
" 20 7.3768e-01 7.7555e-02 4.1497e-05 6.5971e-02 1.49871e-02 1.0000 6.5971e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.4793e-01 7.7556e-02 2.4424e-07 1.7037e-02 1.17844e-01 0.0156 1.7037e-02 2\n",
" 22 2.4714e-01 7.7555e-02 2.4215e-07 1.6958e-02 1.00217e-01 0.0156 1.6958e-02 2\n",
" 23 2.4491e-01 7.7555e-02 2.4009e-07 1.6735e-02 8.52539e-02 0.0312 1.6735e-02 2\n",
" 24 2.4336e-01 7.7555e-02 2.4310e-07 1.6580e-02 5.98716e-02 0.0625 1.6580e-02 2\n",
" 25 2.3955e-01 7.7555e-02 2.5726e-07 1.6200e-02 2.45057e-02 0.2500 1.6200e-02 2\n",
" 26 1.9589e-01 7.7555e-02 2.7793e-07 1.1833e-02 3.54772e-02 0.0020 1.1833e-02 1\n",
" 27 1.9589e-01 7.7555e-02 2.7793e-07 1.1833e-02 3.53907e-02 0.0625 1.1833e-02 1\n",
" 28 1.9343e-01 7.7555e-02 3.3540e-07 1.1587e-02 1.70497e-02 0.1250 1.1587e-02 1\n",
" 29 1.9194e-01 7.7555e-02 3.9542e-07 1.1438e-02 8.71111e-03 1.0000 1.1438e-02 1\n",
" END 1.1372e-01 7.7555e-02 3.2301e-06 3.6132e-03 ---- ---- 3.6132e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8190e+01 8.3005e-02 3.6589e+00 1.5172e-01 5.71189e-01 0.5000 ---- 3\n",
" 2 3.0195e+01 7.8211e-02 1.8498e+00 1.1619e+00 3.73499e-01 1.0000 1.1619e+00 3\n",
" 3 2.9370e+01 7.7408e-02 2.3468e-02 2.9057e+00 8.53937e-01 0.0312 2.9057e+00 3\n",
" 4 2.8974e+01 7.7397e-02 2.2738e-02 2.8670e+00 6.47692e-01 0.0625 2.8670e+00 3\n",
" 5 2.8845e+01 7.7385e-02 2.1324e-02 2.8554e+00 3.68278e-01 0.1250 2.8554e+00 3\n",
" 6 2.6835e+01 7.7381e-02 1.8663e-02 2.6571e+00 8.90447e-02 1.0000 2.6571e+00 3\n",
" 7 5.4967e+00 7.7422e-02 1.7550e-04 5.4176e-01 3.61044e-01 0.0156 5.4176e-01 3\n",
" 8 5.4357e+00 7.7420e-02 1.7281e-04 5.3565e-01 3.14916e-01 0.0312 5.3565e-01 3\n",
" 9 5.4169e+00 7.7417e-02 1.6755e-04 5.3378e-01 2.48025e-01 0.0312 5.3378e-01 3\n",
" 10 5.3157e+00 7.7416e-02 1.6237e-04 5.2366e-01 1.98700e-01 0.0625 5.2366e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 5.2159e+00 7.7414e-02 1.5229e-04 5.1370e-01 1.19249e-01 0.1250 5.1370e-01 3\n",
" 12 4.7494e+00 7.7413e-02 1.3318e-04 4.6707e-01 4.25701e-02 1.0000 4.6707e-01 3\n",
" 13 5.2713e-01 7.7419e-02 1.1540e-05 4.4960e-02 1.49169e-01 0.0625 4.4960e-02 3\n",
" 14 5.0805e-01 7.7418e-02 1.1370e-05 4.3052e-02 8.35548e-02 0.2500 4.3052e-02 3\n",
" 15 5.0378e-01 7.7417e-02 1.2801e-05 4.2623e-02 5.68649e-02 0.2500 4.2623e-02 3\n",
" 16 4.0539e-01 7.7417e-02 9.5590e-06 3.2787e-02 7.04376e-02 0.0625 3.2787e-02 3\n",
" 17 3.8877e-01 7.7417e-02 8.9907e-06 3.1126e-02 2.96573e-02 0.5000 3.1126e-02 3\n",
" 18 3.2675e-01 7.7417e-02 5.0230e-06 2.4929e-02 9.81469e-02 0.0312 2.4929e-02 2\n",
" 19 3.2236e-01 7.7416e-02 4.8838e-06 2.4489e-02 6.86082e-02 0.0625 2.4489e-02 2\n",
" 20 3.1109e-01 7.7416e-02 4.6225e-06 2.3363e-02 2.76851e-02 0.5000 2.3363e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.2932e-01 7.7416e-02 3.2116e-06 1.5187e-02 1.02356e-01 0.0156 1.5187e-02 2\n",
" 22 2.2713e-01 7.7416e-02 3.1690e-06 1.4968e-02 8.72537e-02 0.0312 1.4968e-02 2\n",
" 23 2.2507e-01 7.7416e-02 3.0925e-06 1.4762e-02 6.15954e-02 0.0625 1.4762e-02 2\n",
" 24 2.1976e-01 7.7416e-02 2.9476e-06 1.4231e-02 2.60818e-02 0.2500 1.4231e-02 2\n",
" END 1.7251e-01 7.7416e-02 2.3905e-06 9.5074e-03 ---- ---- 9.5074e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8171e+01 8.2852e-02 3.6440e+00 1.6472e-01 5.67502e-01 0.5000 ---- 3\n",
" 2 3.0683e+01 7.8102e-02 1.8403e+00 1.2202e+00 3.95484e-01 0.5000 1.2202e+00 3\n",
" 3 2.1820e+01 7.7219e-02 9.2449e-01 1.2498e+00 6.71893e-01 0.0312 1.2498e+00 3\n",
" 4 2.1413e+01 7.7203e-02 8.9560e-01 1.2380e+00 5.00556e-01 0.0625 1.2380e+00 3\n",
" 5 2.0693e+01 7.7179e-02 8.3964e-01 1.2219e+00 2.59540e-01 0.2500 1.2219e+00 3\n",
" 6 1.7204e+01 7.7139e-02 6.2995e-01 1.0827e+00 2.89674e-01 0.1250 1.0827e+00 3\n",
" 7 1.5772e+01 7.7135e-02 5.5124e-01 1.0182e+00 1.20576e-01 1.0000 1.0182e+00 3\n",
" 8 2.6363e+00 7.7340e-02 1.9075e-03 2.5398e-01 4.24856e-01 0.0078 2.5398e-01 3\n",
" 9 2.6358e+00 7.7339e-02 1.8926e-03 2.5396e-01 3.92182e-01 0.0078 2.5396e-01 3\n",
" 10 2.6307e+00 7.7338e-02 1.8778e-03 2.5346e-01 3.60398e-01 0.0078 2.5346e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.6215e+00 7.7338e-02 1.8631e-03 2.5256e-01 3.19376e-01 0.0078 2.5256e-01 3\n",
" 12 2.6076e+00 7.7337e-02 1.8485e-03 2.5118e-01 2.12946e-01 0.0312 2.5118e-01 3\n",
" 13 2.6028e+00 7.7336e-02 1.7908e-03 2.5075e-01 1.69157e-01 0.0312 2.5075e-01 3\n",
" 14 2.5517e+00 7.7335e-02 1.7348e-03 2.4570e-01 1.35425e-01 0.0625 2.4570e-01 3\n",
" 15 2.4855e+00 7.7334e-02 1.6263e-03 2.3919e-01 8.06348e-02 0.2500 2.3919e-01 3\n",
" 16 2.4036e+00 7.7334e-02 1.2195e-03 2.3141e-01 5.90923e-02 0.2500 2.3141e-01 3\n",
" 17 2.0382e+00 7.7334e-02 9.1462e-04 1.9517e-01 4.83958e-02 0.5000 1.9517e-01 3\n",
" 18 1.9489e+00 7.7335e-02 4.5708e-04 1.8670e-01 1.22233e-01 0.0625 1.8670e-01 3\n",
" 19 1.9222e+00 7.7334e-02 4.2852e-04 1.8406e-01 6.56862e-02 0.1250 1.8406e-01 3\n",
" 20 1.7506e+00 7.7334e-02 3.7497e-04 1.6696e-01 7.72873e-03 1.0000 1.6696e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 9.1740e-02 7.7337e-02 2.6328e-07 1.4401e-03 ---- ---- 1.4401e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8227e+01 8.2778e-02 3.6471e+00 1.6732e-01 5.91722e-01 0.5000 ---- 3\n",
" 2 3.2560e+01 7.8045e-02 1.8404e+00 1.4077e+00 5.24278e-01 0.1250 1.4077e+00 3\n",
" 3 3.0156e+01 7.7729e-02 1.6106e+00 1.3972e+00 2.92207e-01 1.0000 1.3972e+00 3\n",
" 4 1.4528e+01 7.7307e-02 1.4943e-02 1.4301e+00 7.54552e-01 0.0156 1.4301e+00 3\n",
" 5 1.4436e+01 7.7303e-02 1.4710e-02 1.4211e+00 6.35051e-01 0.0156 1.4211e+00 3\n",
" 6 1.4279e+01 7.7299e-02 1.4480e-02 1.4056e+00 5.34902e-01 0.0312 1.4056e+00 3\n",
" 7 1.4162e+01 7.7295e-02 1.4028e-02 1.3944e+00 3.70779e-01 0.0625 1.3944e+00 3\n",
" 8 1.3935e+01 7.7291e-02 1.3151e-02 1.3727e+00 1.50506e-01 0.2500 1.3727e+00 3\n",
" 9 1.1207e+01 7.7294e-02 9.8631e-03 1.1032e+00 2.04152e-01 0.1250 1.1032e+00 3\n",
" 10 1.0689e+01 7.7293e-02 8.6306e-03 1.0525e+00 6.74403e-02 1.0000 1.0525e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 4.2936e+00 7.7312e-02 3.0006e-05 4.2160e-01 5.52245e-01 0.0078 4.2160e-01 3\n",
" 12 4.2931e+00 7.7311e-02 2.9794e-05 4.2155e-01 5.05048e-01 0.0078 4.2155e-01 3\n",
" 13 4.2854e+00 7.7309e-02 2.9579e-05 4.2078e-01 4.61107e-01 0.0078 4.2078e-01 3\n",
" 14 4.2710e+00 7.7308e-02 2.9362e-05 4.1934e-01 4.06996e-01 0.0078 4.1934e-01 3\n",
" 15 4.2488e+00 7.7307e-02 2.9143e-05 4.1712e-01 2.69994e-01 0.0156 4.1712e-01 3\n",
" 16 4.1976e+00 7.7306e-02 2.8706e-05 4.1200e-01 2.39310e-01 0.0312 4.1200e-01 3\n",
" 17 4.1594e+00 7.7304e-02 2.7861e-05 4.0819e-01 1.85382e-01 0.0312 4.0819e-01 3\n",
" 18 4.0591e+00 7.7303e-02 2.7013e-05 3.9815e-01 1.44513e-01 0.0625 3.9815e-01 3\n",
" 19 3.9110e+00 7.7302e-02 2.5366e-05 3.8334e-01 8.28428e-02 0.2500 3.8334e-01 3\n",
" 20 3.5752e+00 7.7302e-02 1.9111e-05 3.4978e-01 6.53041e-02 0.2500 3.4978e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.0214e+00 7.7303e-02 1.4427e-05 2.9440e-01 5.52634e-02 0.2500 2.9440e-01 3\n",
" 22 2.4542e+00 7.7303e-02 1.0864e-05 2.3768e-01 4.47775e-02 0.5000 2.3768e-01 3\n",
" 23 2.3190e+00 7.7304e-02 5.6722e-06 2.2417e-01 1.19761e-01 0.0625 2.2417e-01 3\n",
" 24 2.2960e+00 7.7304e-02 5.3584e-06 2.2187e-01 6.46910e-02 0.1250 2.2187e-01 3\n",
" 25 2.0927e+00 7.7303e-02 4.7145e-06 2.0153e-01 7.55175e-03 1.0000 2.0153e-01 3\n",
" END 9.6887e-02 7.7307e-02 3.0582e-07 1.9577e-03 ---- ---- 1.9577e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.7940e+01 8.2744e-02 3.6382e+00 1.4751e-01 5.79087e-01 0.5000 ---- 3\n",
" 2 3.1277e+01 7.8030e-02 1.8349e+00 1.2849e+00 4.88869e-01 0.2500 1.2849e+00 3\n",
" 3 3.0838e+01 7.7468e-02 1.3770e+00 1.6990e+00 5.35255e-01 0.0625 1.6990e+00 3\n",
" 4 2.9688e+01 7.7387e-02 1.2910e+00 1.6701e+00 3.01251e-01 0.5000 1.6701e+00 3\n",
" 5 2.2453e+01 7.7100e-02 6.4742e-01 1.5902e+00 6.28228e-01 0.0312 1.5902e+00 3\n",
" 6 2.2206e+01 7.7093e-02 6.2719e-01 1.5857e+00 4.60195e-01 0.0625 1.5857e+00 3\n",
" 7 2.1753e+01 7.7087e-02 5.8800e-01 1.5796e+00 2.31121e-01 0.5000 1.5796e+00 3\n",
" 8 2.1040e+01 7.7145e-02 2.9434e-01 1.8019e+00 6.51891e-01 0.0312 1.8019e+00 3\n",
" 9 2.0886e+01 7.7141e-02 2.8514e-01 1.7957e+00 4.56144e-01 0.0625 1.7957e+00 3\n",
" 10 2.0695e+01 7.7141e-02 2.6732e-01 1.7945e+00 1.94554e-01 0.2500 1.7945e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.7787e+01 7.7173e-02 2.0051e-01 1.5705e+00 2.66339e-01 0.1250 1.5705e+00 3\n",
" 12 1.7163e+01 7.7183e-02 1.7545e-01 1.5331e+00 8.90102e-02 1.0000 1.5331e+00 3\n",
" 13 4.6526e+00 7.7315e-02 2.0555e-04 4.5733e-01 6.26221e-01 0.0039 4.5733e-01 3\n",
" 14 4.6413e+00 7.7313e-02 2.0476e-04 4.5619e-01 5.98159e-01 0.0039 4.5619e-01 3\n",
" 15 4.6288e+00 7.7312e-02 2.0397e-04 4.5495e-01 5.69004e-01 0.0039 4.5495e-01 3\n",
" 16 4.6153e+00 7.7311e-02 2.0318e-04 4.5359e-01 5.21800e-01 0.0078 4.5359e-01 3\n",
" 17 4.6091e+00 7.7309e-02 2.0161e-04 4.5297e-01 3.42284e-01 0.0156 4.5297e-01 3\n",
" 18 4.5848e+00 7.7307e-02 1.9849e-04 4.5055e-01 3.00147e-01 0.0156 4.5055e-01 3\n",
" 19 4.5445e+00 7.7306e-02 1.9541e-04 4.4653e-01 2.66312e-01 0.0156 4.4653e-01 3\n",
" 20 4.4914e+00 7.7304e-02 1.9236e-04 4.4121e-01 2.36181e-01 0.0312 4.4121e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.4564e+00 7.7302e-02 1.8637e-04 4.3772e-01 1.82220e-01 0.0312 4.3772e-01 3\n",
" 22 4.3524e+00 7.7301e-02 1.8056e-04 4.2733e-01 1.40851e-01 0.0625 4.2733e-01 3\n",
" 23 4.2017e+00 7.7300e-02 1.6928e-04 4.1227e-01 8.03367e-02 0.2500 4.1227e-01 3\n",
" 24 3.8663e+00 7.7300e-02 1.2714e-04 3.7878e-01 6.39299e-02 0.2500 3.7878e-01 3\n",
" 25 3.3172e+00 7.7301e-02 9.5338e-05 3.2390e-01 5.58045e-02 0.2500 3.2390e-01 3\n",
" 26 2.7471e+00 7.7301e-02 7.1602e-05 2.6691e-01 4.59948e-02 0.2500 2.6691e-01 3\n",
" 27 2.2347e+00 7.7302e-02 5.3715e-05 2.1569e-01 4.06707e-02 0.5000 2.1569e-01 3\n",
" 28 2.1942e+00 7.7303e-02 2.7062e-05 2.1166e-01 1.10396e-01 0.0625 2.1166e-01 3\n",
" 29 2.1796e+00 7.7302e-02 2.5392e-05 2.1021e-01 5.82169e-02 0.1250 2.1021e-01 3\n",
" 30 1.9919e+00 7.7302e-02 2.2230e-05 1.9144e-01 6.22239e-03 1.0000 1.9144e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 1.0016e-01 7.7306e-02 2.7836e-07 2.2853e-03 ---- ---- 2.2853e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8185e+01 8.2741e-02 3.6390e+00 1.7121e-01 5.70144e-01 0.5000 ---- 3\n",
" 2 3.2302e+01 7.8037e-02 1.8345e+00 1.3879e+00 4.53143e-01 0.2500 1.3879e+00 3\n",
" 3 3.0041e+01 7.7479e-02 1.3767e+00 1.6197e+00 4.70919e-01 0.1250 1.6197e+00 3\n",
" 4 2.9776e+01 7.7329e-02 1.2047e+00 1.7652e+00 2.54542e-01 1.0000 1.7652e+00 3\n",
" 5 2.0328e+01 7.7332e-02 7.6223e-03 2.0175e+00 8.84144e-01 0.0156 2.0175e+00 3\n",
" 6 2.0230e+01 7.7325e-02 7.5034e-03 2.0077e+00 7.45213e-01 0.0156 2.0077e+00 3\n",
" 7 2.0049e+01 7.7319e-02 7.3863e-03 1.9898e+00 6.22844e-01 0.0156 1.9898e+00 3\n",
" 8 1.9802e+01 7.7314e-02 7.2710e-03 1.9652e+00 5.22012e-01 0.0312 1.9652e+00 3\n",
" 9 1.9582e+01 7.7308e-02 7.0440e-03 1.9435e+00 3.60691e-01 0.0625 1.9435e+00 3\n",
" 10 1.9248e+01 7.7303e-02 6.6040e-03 1.9105e+00 1.50994e-01 0.2500 1.9105e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.5909e+01 7.7309e-02 4.9522e-03 1.5783e+00 2.16209e-01 0.1250 1.5783e+00 3\n",
" 12 1.5255e+01 7.7307e-02 4.3335e-03 1.5134e+00 7.35308e-02 1.0000 1.5134e+00 3\n",
" 13 4.6098e+00 7.7337e-02 2.0881e-05 4.5322e-01 6.12531e-01 0.0078 4.5322e-01 3\n",
" 14 4.5997e+00 7.7335e-02 2.0752e-05 4.5222e-01 5.57331e-01 0.0078 4.5222e-01 3\n",
" 15 4.5833e+00 7.7333e-02 2.0616e-05 4.5057e-01 5.04812e-01 0.0078 4.5057e-01 3\n",
" 16 4.5610e+00 7.7331e-02 2.0473e-05 4.4835e-01 4.42546e-01 0.0156 4.4835e-01 3\n",
" 17 4.5552e+00 7.7328e-02 2.0205e-05 4.4777e-01 3.69291e-01 0.0156 4.4777e-01 3\n",
" 18 4.5204e+00 7.7326e-02 1.9917e-05 4.4429e-01 3.09920e-01 0.0156 4.4429e-01 3\n",
" 19 4.4649e+00 7.7325e-02 1.9622e-05 4.3874e-01 2.59818e-01 0.0312 4.3874e-01 3\n",
" 20 4.4118e+00 7.7323e-02 1.9043e-05 4.3343e-01 1.79024e-01 0.0625 4.3343e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.3599e+00 7.7322e-02 1.7901e-05 4.2824e-01 6.94489e-02 0.2500 4.2824e-01 3\n",
" 22 3.4604e+00 7.7323e-02 1.3624e-05 3.3830e-01 9.95834e-02 0.1250 3.3830e-01 3\n",
" 23 3.3074e+00 7.7323e-02 1.1964e-05 3.2300e-01 3.35479e-02 1.0000 3.2300e-01 3\n",
" 24 1.8347e+00 7.7329e-02 1.9259e-06 1.7573e-01 3.05775e-01 0.0078 1.7573e-01 3\n",
" 25 1.8338e+00 7.7328e-02 1.9176e-06 1.7565e-01 2.68261e-01 0.0078 1.7565e-01 3\n",
" 26 1.8285e+00 7.7327e-02 1.9074e-06 1.7511e-01 1.76763e-01 0.0156 1.7511e-01 3\n",
" 27 1.8137e+00 7.7327e-02 1.8861e-06 1.7363e-01 1.55459e-01 0.0156 1.7363e-01 3\n",
" 28 1.7933e+00 7.7326e-02 1.8625e-06 1.7160e-01 1.36638e-01 0.0312 1.7160e-01 3\n",
" 29 1.7788e+00 7.7326e-02 1.8204e-06 1.7014e-01 1.04001e-01 0.0625 1.7014e-01 3\n",
" 30 1.7767e+00 7.7325e-02 1.7357e-06 1.6994e-01 5.60257e-02 0.1250 1.6994e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.6318e+00 7.7325e-02 1.5320e-06 1.5545e-01 6.05839e-03 1.0000 1.5545e-01 3\n",
" END 8.8894e-02 7.7328e-02 2.2000e-07 1.1564e-03 ---- ---- 1.1564e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8097e+01 8.2765e-02 3.6384e+00 1.6307e-01 5.63589e-01 0.5000 ---- 3\n",
" 2 3.1304e+01 7.8064e-02 1.8335e+00 1.2891e+00 4.24514e-01 0.2500 1.2891e+00 3\n",
" 3 2.6417e+01 7.7509e-02 1.3759e+00 1.2581e+00 4.32206e-01 0.1250 1.2581e+00 3\n",
" 4 2.5268e+01 7.7362e-02 1.2040e+00 1.3150e+00 2.45347e-01 1.0000 1.3150e+00 3\n",
" 5 1.7446e+01 7.7361e-02 7.2780e-03 1.7295e+00 7.99694e-01 0.0156 1.7295e+00 3\n",
" 6 1.7369e+01 7.7355e-02 7.1644e-03 1.7220e+00 6.69728e-01 0.0156 1.7220e+00 3\n",
" 7 1.7213e+01 7.7350e-02 7.0525e-03 1.7065e+00 5.57465e-01 0.0312 1.7065e+00 3\n",
" 8 1.7192e+01 7.7343e-02 6.8323e-03 1.7046e+00 3.84250e-01 0.0312 1.7046e+00 3\n",
" 9 1.6775e+01 7.7340e-02 6.6189e-03 1.6631e+00 2.66335e-01 0.0625 1.6631e+00 3\n",
" 10 1.5983e+01 7.7338e-02 6.2052e-03 1.5843e+00 1.09747e-01 0.5000 1.5843e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.1865e+01 7.7351e-02 3.1003e-03 1.1757e+00 4.14841e-01 0.0312 1.1757e+00 3\n",
" 12 1.1805e+01 7.7347e-02 3.0034e-03 1.1698e+00 2.82026e-01 0.0625 1.1698e+00 3\n",
" 13 1.1701e+01 7.7343e-02 2.8158e-03 1.1595e+00 1.07604e-01 0.2500 1.1595e+00 3\n",
" 14 9.5996e+00 7.7347e-02 2.1126e-03 9.5011e-01 1.60420e-01 0.1250 9.5011e-01 3\n",
" 15 9.2945e+00 7.7346e-02 1.8486e-03 9.1986e-01 5.50162e-02 1.0000 9.1986e-01 3\n",
" 16 4.6592e+00 7.7364e-02 6.9413e-06 4.5817e-01 5.07735e-01 0.0078 4.5817e-01 3\n",
" 17 4.6549e+00 7.7362e-02 6.9111e-06 4.5775e-01 4.61344e-01 0.0078 4.5775e-01 3\n",
" 18 4.6435e+00 7.7361e-02 6.8763e-06 4.5661e-01 4.18642e-01 0.0078 4.5661e-01 3\n",
" 19 4.6256e+00 7.7359e-02 6.8376e-06 4.5482e-01 3.68260e-01 0.0078 4.5482e-01 3\n",
" 20 4.5998e+00 7.7358e-02 6.7953e-06 4.5223e-01 2.43908e-01 0.0312 4.5223e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.5896e+00 7.7356e-02 6.6587e-06 4.5122e-01 1.86624e-01 0.0312 4.5122e-01 3\n",
" 22 4.4987e+00 7.7354e-02 6.4867e-06 4.4213e-01 1.42172e-01 0.0625 4.4213e-01 3\n",
" 23 4.3829e+00 7.7353e-02 6.1510e-06 4.3055e-01 7.87906e-02 0.2500 4.3055e-01 3\n",
" 24 4.2110e+00 7.7353e-02 4.8558e-06 4.1336e-01 6.48972e-02 0.2500 4.1336e-01 3\n",
" 25 3.7197e+00 7.7353e-02 3.7712e-06 3.6423e-01 5.77545e-02 0.2500 3.6423e-01 3\n",
" 26 3.1493e+00 7.7354e-02 2.9541e-06 3.0719e-01 4.94675e-02 0.2500 3.0719e-01 3\n",
" 27 2.6135e+00 7.7355e-02 2.2958e-06 2.5361e-01 4.45326e-02 0.2500 2.5361e-01 3\n",
" 28 2.1501e+00 7.7355e-02 1.7944e-06 2.0727e-01 3.92062e-02 0.2500 2.0727e-01 3\n",
" 29 1.7678e+00 7.7356e-02 1.3971e-06 1.6905e-01 3.54117e-02 0.2500 1.6905e-01 3\n",
" 30 1.4588e+00 7.7356e-02 1.0946e-06 1.3815e-01 3.17077e-02 0.2500 1.3815e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.2124e+00 7.7356e-02 8.5562e-07 1.1350e-01 2.87936e-02 0.2500 1.1350e-01 3\n",
" 32 1.0163e+00 7.7357e-02 6.7314e-07 9.3896e-02 2.60408e-02 0.2500 9.3896e-02 3\n",
" 33 8.6019e-01 7.7357e-02 5.2864e-07 7.8283e-02 2.37725e-02 0.2500 7.8283e-02 3\n",
" 34 7.3512e-01 7.7357e-02 4.1778e-07 6.5776e-02 2.16482e-02 0.2500 6.5776e-02 3\n",
" 35 6.3431e-01 7.7357e-02 3.3019e-07 5.5695e-02 1.98529e-02 0.2500 5.5695e-02 3\n",
" 36 5.5239e-01 7.7357e-02 2.6261e-07 4.7503e-02 1.81745e-02 0.2500 4.7503e-02 3\n",
" 37 4.8537e-01 7.7357e-02 2.0917e-07 4.0801e-02 1.67259e-02 0.2500 4.0801e-02 3\n",
" 38 4.3003e-01 7.7357e-02 1.6765e-07 3.5267e-02 1.53800e-02 0.2500 3.5267e-02 3\n",
" 39 3.8417e-01 7.7357e-02 1.3472e-07 3.0681e-02 1.41952e-02 0.2500 3.0681e-02 3\n",
" 40 3.4580e-01 7.7357e-02 1.0892e-07 2.6845e-02 1.31029e-02 0.2500 2.6845e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 3.1333e-01 7.7357e-02 8.8362e-08 2.3598e-02 1.21377e-02 0.5000 2.3598e-02 2\n",
" 42 2.8453e-01 7.7358e-02 6.9248e-08 2.0717e-02 3.45286e-02 0.0625 2.0717e-02 2\n",
" 43 2.8427e-01 7.7358e-02 6.8670e-08 2.0691e-02 1.78973e-02 0.1250 2.0691e-02 2\n",
" 44 2.5930e-01 7.7358e-02 6.3606e-08 1.8194e-02 9.68751e-04 1.0000 1.8194e-02 2\n",
" END 7.7380e-02 7.7358e-02 5.3429e-09 2.2396e-06 ---- ---- 6.2302e-04 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8666e+01 8.2787e-02 3.6846e+00 1.7373e-01 5.65479e-01 0.5000 ---- 3\n",
" 2 3.1333e+01 7.8203e-02 1.8535e+00 1.2720e+00 3.39712e-01 1.0000 1.2720e+00 3\n",
" 3 2.3787e+01 7.7669e-02 1.4588e-02 2.3563e+00 7.33420e-01 0.0312 2.3563e+00 3\n",
" 4 2.3562e+01 7.7661e-02 1.4133e-02 2.3343e+00 5.36130e-01 0.0625 2.3343e+00 3\n",
" 5 2.3442e+01 7.7652e-02 1.3251e-02 2.3232e+00 2.71323e-01 0.1250 2.3232e+00 3\n",
" 6 2.1137e+01 7.7650e-02 1.1597e-02 2.0943e+00 8.24909e-02 1.0000 2.0943e+00 3\n",
" 7 1.2427e+00 7.7682e-02 1.5328e-04 1.1634e-01 4.25793e-01 0.0039 1.1634e-01 3\n",
" 8 1.2395e+00 7.7681e-02 1.5268e-04 1.1603e-01 4.10222e-01 0.0039 1.1603e-01 3\n",
" 9 1.2361e+00 7.7681e-02 1.5209e-04 1.1569e-01 3.93766e-01 0.0078 1.1569e-01 3\n",
" 10 1.2353e+00 7.7680e-02 1.5091e-04 1.1561e-01 3.50938e-01 0.0078 1.1561e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.2316e+00 7.7679e-02 1.4974e-04 1.1524e-01 2.34886e-01 0.0156 1.1524e-01 3\n",
" 12 1.2216e+00 7.7679e-02 1.4741e-04 1.1424e-01 2.11762e-01 0.0156 1.1424e-01 3\n",
" 13 1.2080e+00 7.7678e-02 1.4512e-04 1.1289e-01 1.91055e-01 0.0312 1.1289e-01 3\n",
" 14 1.1979e+00 7.7677e-02 1.4062e-04 1.1188e-01 1.54042e-01 0.0625 1.1188e-01 3\n",
" 15 1.1965e+00 7.7676e-02 1.3195e-04 1.1175e-01 9.65669e-02 0.1250 1.1175e-01 3\n",
" 16 1.1085e+00 7.7675e-02 1.1574e-04 1.0297e-01 3.42303e-02 1.0000 1.0297e-01 3\n",
" 17 6.9654e-01 7.7676e-02 1.8211e-05 6.1868e-02 1.27891e-01 0.0625 6.1868e-02 3\n",
" 18 6.7392e-01 7.7675e-02 1.7599e-05 5.9607e-02 7.52688e-02 0.0625 5.9607e-02 3\n",
" 19 6.4644e-01 7.7675e-02 1.6794e-05 5.6860e-02 4.74224e-02 0.1250 5.6860e-02 3\n",
" 20 5.9297e-01 7.7674e-02 1.5332e-05 5.1514e-02 1.86807e-02 1.0000 5.1514e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.6908e-01 7.7675e-02 9.9625e-06 2.9130e-02 6.78434e-02 0.0625 2.9130e-02 3\n",
" 22 3.6421e-01 7.7674e-02 9.6823e-06 2.8644e-02 4.39720e-02 0.1250 2.8644e-02 3\n",
" 23 3.5863e-01 7.7674e-02 9.1334e-06 2.8086e-02 1.92883e-02 0.5000 2.8086e-02 3\n",
" 24 3.1223e-01 7.7674e-02 6.1015e-06 2.3449e-02 2.54781e-02 0.2500 2.3449e-02 3\n",
" 25 2.8009e-01 7.7674e-02 5.0329e-06 2.0237e-02 1.21329e-02 1.0000 2.0237e-02 3\n",
" 26 2.4025e-01 7.7674e-02 3.2855e-07 1.6257e-02 9.69640e-02 0.0156 1.6257e-02 2\n",
" 27 2.3998e-01 7.7674e-02 3.2613e-07 1.6231e-02 8.19831e-02 0.0156 1.6231e-02 2\n",
" 28 2.3797e-01 7.7674e-02 3.2306e-07 1.6029e-02 6.93420e-02 0.0312 1.6029e-02 2\n",
" 29 2.3657e-01 7.7674e-02 3.1907e-07 1.5889e-02 4.80113e-02 0.0625 1.5889e-02 2\n",
" 30 2.3166e-01 7.7673e-02 3.1235e-07 1.5399e-02 1.89792e-02 0.5000 1.5399e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.8405e-01 7.7674e-02 3.6969e-07 1.0638e-02 6.70822e-02 0.0156 1.0638e-02 2\n",
" 32 1.8253e-01 7.7674e-02 3.6546e-07 1.0485e-02 5.64320e-02 0.0312 1.0485e-02 2\n",
" 33 1.8119e-01 7.7674e-02 3.5851e-07 1.0352e-02 3.87029e-02 0.0625 1.0352e-02 2\n",
" 34 1.7754e-01 7.7673e-02 3.4522e-07 9.9864e-03 1.98988e-02 0.0020 1.0454e-02 1\n",
" END 1.7754e-01 7.7673e-02 3.4522e-07 9.9864e-03 ---- ---- 9.9864e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.9031e+01 8.3142e-02 3.7259e+00 1.6884e-01 5.65017e-01 0.5000 ---- 3\n",
" 2 3.1425e+01 7.8586e-02 1.8742e+00 1.2604e+00 3.45072e-01 1.0000 1.2604e+00 3\n",
" 3 1.7292e+01 7.8096e-02 1.4790e-02 1.7066e+00 8.55435e-01 0.0312 1.7066e+00 3\n",
" 4 1.7202e+01 7.8088e-02 1.4330e-02 1.6981e+00 6.19741e-01 0.0625 1.6981e+00 3\n",
" 5 1.7189e+01 7.8080e-02 1.3439e-02 1.6977e+00 2.79991e-01 0.2500 1.6977e+00 3\n",
" 6 1.6567e+01 7.8079e-02 1.0086e-02 1.6388e+00 3.06057e-01 0.1250 1.6388e+00 3\n",
" 7 1.5791e+01 7.8077e-02 8.8306e-03 1.5624e+00 7.63066e-02 1.0000 1.5624e+00 3\n",
" 8 5.5936e+00 7.8103e-02 3.5008e-05 5.5152e-01 5.22395e-01 0.0078 5.5152e-01 3\n",
" 9 5.5813e+00 7.8101e-02 3.4727e-05 5.5028e-01 4.86815e-01 0.0078 5.5028e-01 3\n",
" 10 5.5606e+00 7.8099e-02 3.4448e-05 5.4822e-01 4.53586e-01 0.0078 5.4822e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 5.5326e+00 7.8098e-02 3.4170e-05 5.4542e-01 4.06471e-01 0.0156 5.4542e-01 3\n",
" 12 5.5216e+00 7.8096e-02 3.3603e-05 5.4432e-01 3.55619e-01 0.0156 5.4432e-01 3\n",
" 13 5.4754e+00 7.8094e-02 3.3046e-05 5.3970e-01 3.10420e-01 0.0312 5.3970e-01 3\n",
" 14 5.4606e+00 7.8092e-02 3.1899e-05 5.3821e-01 2.27317e-01 0.0625 5.3821e-01 3\n",
" 15 5.4011e+00 7.8090e-02 2.9609e-05 5.3227e-01 1.00814e-01 0.2500 5.3227e-01 3\n",
" 16 4.3416e+00 7.8092e-02 2.1756e-05 4.2633e-01 1.26880e-01 0.1250 4.2633e-01 3\n",
" 17 4.0795e+00 7.8092e-02 1.9299e-05 4.0012e-01 3.42453e-02 1.0000 4.0012e-01 3\n",
" 18 4.5016e-01 7.8100e-02 3.3237e-06 3.7203e-02 2.69129e-01 0.0078 3.7203e-02 3\n",
" 19 4.4937e-01 7.8100e-02 3.3107e-06 3.7124e-02 2.36655e-01 0.0078 3.7124e-02 3\n",
" 20 4.4775e-01 7.8099e-02 3.2949e-06 3.6962e-02 1.55135e-01 0.0156 3.6962e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.4376e-01 7.8099e-02 3.2642e-06 3.6562e-02 1.37532e-01 0.0312 3.6562e-02 3\n",
" 22 4.4182e-01 7.8098e-02 3.2262e-06 3.6369e-02 1.06447e-01 0.0312 3.6369e-02 3\n",
" 23 4.3956e-01 7.8098e-02 3.1620e-06 3.6143e-02 8.27735e-02 0.0312 3.6143e-02 3\n",
" 24 4.3335e-01 7.8097e-02 3.0842e-06 3.5522e-02 6.45610e-02 0.0625 3.5522e-02 3\n",
" 25 4.2547e-01 7.8097e-02 2.9399e-06 3.4734e-02 3.65095e-02 0.1250 3.4734e-02 3\n",
" 26 3.9268e-01 7.8097e-02 2.6259e-06 3.1455e-02 6.73708e-03 1.0000 3.1455e-02 3\n",
" END 9.7833e-02 7.8098e-02 2.5035e-07 1.9733e-03 ---- ---- 1.9733e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8459e+01 8.3601e-02 3.6965e+00 1.4097e-01 5.86481e-01 0.5000 ---- 3\n",
" 2 3.0543e+01 7.8755e-02 1.8692e+00 1.1772e+00 4.10361e-01 0.5000 1.1772e+00 3\n",
" 3 2.2951e+01 7.7834e-02 9.3945e-01 1.3479e+00 6.32730e-01 0.0312 1.3479e+00 3\n",
" 4 2.2569e+01 7.7816e-02 9.1010e-01 1.3391e+00 4.97666e-01 0.0625 1.3391e+00 3\n",
" 5 2.1762e+01 7.7790e-02 8.5324e-01 1.3152e+00 2.72379e-01 0.5000 1.3152e+00 3\n",
" 6 2.0510e+01 7.7751e-02 4.2768e-01 1.6155e+00 6.96109e-01 0.0312 1.6155e+00 3\n",
" 7 2.0223e+01 7.7747e-02 4.1432e-01 1.6002e+00 5.09324e-01 0.0625 1.6002e+00 3\n",
" 8 1.9778e+01 7.7747e-02 3.8843e-01 1.5816e+00 2.36693e-01 0.2500 1.5816e+00 3\n",
" 9 1.7105e+01 7.7773e-02 2.9136e-01 1.4114e+00 2.72610e-01 0.1250 1.4114e+00 3\n",
" 10 1.5906e+01 7.7783e-02 2.5495e-01 1.3279e+00 8.67648e-02 1.0000 1.3279e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.8909e+00 7.7930e-02 4.7097e-04 2.8082e-01 4.48014e-01 0.0039 2.8082e-01 3\n",
" 12 2.8843e+00 7.7929e-02 4.6913e-04 2.8017e-01 4.32410e-01 0.0039 2.8017e-01 3\n",
" 13 2.8769e+00 7.7929e-02 4.6729e-04 2.7943e-01 4.17236e-01 0.0039 2.7943e-01 3\n",
" 14 2.8687e+00 7.7928e-02 4.6547e-04 2.7861e-01 3.87712e-01 0.0078 2.7861e-01 3\n",
" 15 2.8648e+00 7.7927e-02 4.6183e-04 2.7823e-01 2.58141e-01 0.0156 2.7823e-01 3\n",
" 16 2.8473e+00 7.7925e-02 4.5461e-04 2.7649e-01 2.36254e-01 0.0156 2.7649e-01 3\n",
" 17 2.8196e+00 7.7924e-02 4.4751e-04 2.7372e-01 2.15438e-01 0.0312 2.7372e-01 3\n",
" 18 2.8121e+00 7.7923e-02 4.3351e-04 2.7299e-01 1.75437e-01 0.0312 2.7299e-01 3\n",
" 19 2.7543e+00 7.7922e-02 4.1996e-04 2.6722e-01 1.41636e-01 0.0625 2.6722e-01 3\n",
" 20 2.6767e+00 7.7921e-02 3.9370e-04 2.5948e-01 8.49216e-02 0.2500 2.5948e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.5753e+00 7.7921e-02 2.9521e-04 2.4944e-01 5.96499e-02 0.2500 2.4944e-01 3\n",
" 22 2.1287e+00 7.7921e-02 2.2134e-04 2.0486e-01 4.67780e-02 0.5000 2.0486e-01 3\n",
" 23 1.7518e+00 7.7922e-02 1.1077e-04 1.6728e-01 1.15652e-01 0.0625 1.6728e-01 3\n",
" 24 1.7101e+00 7.7922e-02 1.0387e-04 1.6312e-01 6.35076e-02 0.2500 1.6312e-01 3\n",
" 25 1.7033e+00 7.7922e-02 7.8025e-05 1.6246e-01 4.86472e-02 0.2500 1.6246e-01 3\n",
" 26 1.4480e+00 7.7922e-02 5.8671e-05 1.3695e-01 3.68386e-02 0.5000 1.3695e-01 3\n",
" 27 1.3336e+00 7.7923e-02 2.9697e-05 1.2554e-01 9.48881e-02 0.0625 1.2554e-01 3\n",
" 28 1.3150e+00 7.7922e-02 2.7884e-05 1.2368e-01 5.34042e-02 0.1250 1.2368e-01 3\n",
" 29 1.1985e+00 7.7922e-02 2.4471e-05 1.1203e-01 6.91599e-03 1.0000 1.1203e-01 3\n",
" END 1.0809e-01 7.7924e-02 2.3184e-07 3.0159e-03 ---- ---- 3.0159e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8486e+01 8.3415e-02 3.6834e+00 1.5682e-01 5.86854e-01 0.5000 ---- 3\n",
" 2 3.2913e+01 7.8616e-02 1.8605e+00 1.4230e+00 4.64866e-01 0.2500 1.4230e+00 3\n",
" 3 3.0020e+01 7.8037e-02 1.3964e+00 1.5977e+00 5.01089e-01 0.1250 1.5977e+00 3\n",
" 4 2.8707e+01 7.7881e-02 1.2220e+00 1.6408e+00 2.49212e-01 1.0000 1.6408e+00 3\n",
" 5 6.9867e+00 7.7842e-02 9.6421e-03 6.8125e-01 7.27335e-01 0.0078 6.8125e-01 3\n",
" 6 6.9533e+00 7.7840e-02 9.5669e-03 6.7797e-01 6.69647e-01 0.0078 6.7797e-01 3\n",
" 7 6.9135e+00 7.7838e-02 9.4922e-03 6.7407e-01 6.13599e-01 0.0156 6.7407e-01 3\n",
" 8 6.9000e+00 7.7835e-02 9.3441e-03 6.7288e-01 5.15323e-01 0.0156 6.7288e-01 3\n",
" 9 6.8472e+00 7.7833e-02 9.1982e-03 6.6774e-01 4.31691e-01 0.0156 6.6774e-01 3\n",
" 10 6.7653e+00 7.7832e-02 9.0546e-03 6.5969e-01 3.60996e-01 0.0312 6.5969e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 6.6840e+00 7.7830e-02 8.7718e-03 6.5184e-01 2.42790e-01 0.0625 6.5184e-01 3\n",
" 12 6.5208e+00 7.7828e-02 8.2238e-03 6.3607e-01 8.65264e-02 0.5000 6.3607e-01 3\n",
" 13 5.9435e+00 7.7832e-02 4.1111e-03 5.8245e-01 3.09635e-01 0.0156 5.8245e-01 3\n",
" 14 5.8786e+00 7.7831e-02 4.0469e-03 5.7603e-01 2.68967e-01 0.0312 5.7603e-01 3\n",
" 15 5.8147e+00 7.7829e-02 3.9205e-03 5.6976e-01 1.93263e-01 0.0625 5.6976e-01 3\n",
" 16 5.6453e+00 7.7828e-02 3.6757e-03 5.5307e-01 7.87446e-02 0.5000 5.5307e-01 3\n",
" 17 4.2276e+00 7.7833e-02 1.8418e-03 4.1313e-01 3.03331e-01 0.0312 4.1313e-01 3\n",
" 18 4.2124e+00 7.7831e-02 1.7844e-03 4.1167e-01 2.03481e-01 0.0625 4.1167e-01 3\n",
" 19 4.1739e+00 7.7830e-02 1.6732e-03 4.0794e-01 7.13797e-02 0.2500 4.0794e-01 3\n",
" 20 3.3942e+00 7.7831e-02 1.2550e-03 3.3038e-01 1.11948e-01 0.1250 3.3038e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.2843e+00 7.7831e-02 1.0985e-03 3.1955e-01 3.21906e-02 1.0000 3.1955e-01 3\n",
" 22 1.7941e+00 7.7837e-02 1.0139e-06 1.7162e-01 2.90164e-01 0.0078 1.7162e-01 3\n",
" 23 1.7939e+00 7.7837e-02 1.0106e-06 1.7161e-01 2.69412e-01 0.0078 1.7161e-01 3\n",
" 24 1.7902e+00 7.7836e-02 1.0072e-06 1.7124e-01 2.40472e-01 0.0078 1.7124e-01 3\n",
" 25 1.7826e+00 7.7836e-02 1.0033e-06 1.7048e-01 1.59556e-01 0.0156 1.7048e-01 3\n",
" 26 1.7632e+00 7.7835e-02 9.9649e-07 1.6853e-01 1.43939e-01 0.0312 1.6853e-01 3\n",
" 27 1.7501e+00 7.7834e-02 9.9705e-07 1.6723e-01 1.14770e-01 0.0312 1.6723e-01 3\n",
" 28 1.7098e+00 7.7834e-02 9.8926e-07 1.6319e-01 9.09541e-02 0.0625 1.6319e-01 3\n",
" 29 1.6484e+00 7.7833e-02 9.9263e-07 1.5706e-01 5.21489e-02 0.2500 1.5706e-01 3\n",
" 30 1.4994e+00 7.7833e-02 1.1502e-06 1.4216e-01 4.02845e-02 0.2500 1.4216e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.2460e+00 7.7834e-02 1.1177e-06 1.1682e-01 3.38533e-02 0.5000 1.1682e-01 3\n",
" 32 1.1340e+00 7.7834e-02 1.3264e-06 1.0562e-01 8.98945e-02 0.0625 1.0562e-01 3\n",
" 33 1.1180e+00 7.7834e-02 1.3323e-06 1.0402e-01 4.78964e-02 0.1250 1.0402e-01 3\n",
" 34 1.0235e+00 7.7834e-02 1.2569e-06 9.4569e-02 3.92510e-03 1.0000 9.4569e-02 3\n",
" END 1.0031e-01 7.7836e-02 1.9891e-08 2.2476e-03 ---- ---- 2.2476e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8442e+01 8.3323e-02 3.6753e+00 1.6048e-01 5.91457e-01 0.5000 ---- 3\n",
" 2 3.2701e+01 7.8553e-02 1.8549e+00 1.4073e+00 4.86725e-01 0.2500 1.4073e+00 3\n",
" 3 2.9562e+01 7.7981e-02 1.3921e+00 1.5563e+00 4.65691e-01 0.1250 1.5563e+00 3\n",
" 4 2.8268e+01 7.7828e-02 1.2182e+00 1.6008e+00 2.47248e-01 1.0000 1.6008e+00 3\n",
" 5 1.3404e+01 7.7805e-02 8.5640e-03 1.3240e+00 6.40356e-01 0.0078 1.3240e+00 3\n",
" 6 1.3321e+01 7.7802e-02 8.4971e-03 1.3159e+00 5.78168e-01 0.0156 1.3159e+00 3\n",
" 7 1.3280e+01 7.7797e-02 8.3643e-03 1.3119e+00 4.76182e-01 0.0156 1.3119e+00 3\n",
" 8 1.3163e+01 7.7793e-02 8.2337e-03 1.3003e+00 4.21935e-01 0.0312 1.3003e+00 3\n",
" 9 1.3120e+01 7.7788e-02 7.9764e-03 1.2963e+00 3.20784e-01 0.0625 1.2963e+00 3\n",
" 10 1.2974e+01 7.7784e-02 7.4779e-03 1.2821e+00 1.43868e-01 0.5000 1.2821e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.2711e+01 7.7793e-02 3.7367e-03 1.2595e+00 5.50015e-01 0.0312 1.2595e+00 3\n",
" 12 1.2613e+01 7.7788e-02 3.6199e-03 1.2499e+00 3.82950e-01 0.0625 1.2499e+00 3\n",
" 13 1.2516e+01 7.7784e-02 3.3937e-03 1.2404e+00 1.45406e-01 0.2500 1.2404e+00 3\n",
" 14 1.1052e+01 7.7786e-02 2.5459e-03 1.0949e+00 2.08526e-01 0.1250 1.0949e+00 3\n",
" 15 1.0858e+01 7.7785e-02 2.2276e-03 1.0758e+00 4.90104e-02 1.0000 1.0758e+00 3\n",
" 16 4.2788e+00 7.7807e-02 3.3585e-06 4.2010e-01 4.47569e-01 0.0039 4.2010e-01 3\n",
" 17 4.2676e+00 7.7806e-02 3.3471e-06 4.1897e-01 4.25796e-01 0.0039 4.1897e-01 3\n",
" 18 4.2552e+00 7.7805e-02 3.3356e-06 4.1774e-01 4.03991e-01 0.0078 4.1774e-01 3\n",
" 19 4.2531e+00 7.7803e-02 3.3154e-06 4.1752e-01 3.55677e-01 0.0078 4.1752e-01 3\n",
" 20 4.2399e+00 7.7802e-02 3.2943e-06 4.1621e-01 2.36736e-01 0.0156 4.1621e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.2017e+00 7.7800e-02 3.2541e-06 4.1239e-01 2.17439e-01 0.0156 4.1239e-01 3\n",
" 22 4.1511e+00 7.7799e-02 3.2126e-06 4.0732e-01 1.98855e-01 0.0312 4.0732e-01 3\n",
" 23 4.1111e+00 7.7797e-02 3.1429e-06 4.0333e-01 1.62410e-01 0.0625 4.0333e-01 3\n",
" 24 4.0985e+00 7.7795e-02 3.0294e-06 4.0206e-01 9.77013e-02 0.1250 4.0206e-01 3\n",
" 25 3.7616e+00 7.7794e-02 2.8708e-06 3.6837e-01 1.70032e-02 1.0000 3.6837e-01 3\n",
" END 1.1378e-01 7.7802e-02 1.7227e-06 3.5962e-03 ---- ---- 3.5962e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8365e+01 8.3285e-02 3.6692e+00 1.5895e-01 6.21811e-01 0.5000 ---- 3\n",
" 2 3.5821e+01 7.8533e-02 1.8505e+00 1.7237e+00 5.65094e-01 0.0625 1.7237e+00 3\n",
" 3 3.4679e+01 7.8362e-02 1.7349e+00 1.7251e+00 3.69058e-01 0.5000 1.7251e+00 3\n",
" 4 2.6396e+01 7.7638e-02 8.7074e-01 1.7611e+00 6.79481e-01 0.0312 1.7611e+00 3\n",
" 5 2.5844e+01 7.7624e-02 8.4354e-01 1.7331e+00 5.07747e-01 0.0625 1.7331e+00 3\n",
" 6 2.4943e+01 7.7605e-02 7.9082e-01 1.6957e+00 2.61501e-01 0.2500 1.6957e+00 3\n",
" 7 2.0953e+01 7.7583e-02 5.9327e-01 1.4942e+00 2.95068e-01 0.1250 1.4942e+00 3\n",
" 8 1.9550e+01 7.7584e-02 5.1914e-01 1.4281e+00 1.19510e-01 1.0000 1.4281e+00 3\n",
" 9 3.8686e+00 7.7808e-02 1.4469e-03 3.7763e-01 4.45643e-01 0.0039 3.7763e-01 3\n",
" 10 3.8587e+00 7.7807e-02 1.4412e-03 3.7665e-01 4.25372e-01 0.0039 3.7665e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.8478e+00 7.7806e-02 1.4356e-03 3.7557e-01 4.04421e-01 0.0078 3.7557e-01 3\n",
" 12 3.8469e+00 7.7804e-02 1.4244e-03 3.7549e-01 3.53796e-01 0.0078 3.7549e-01 3\n",
" 13 3.8360e+00 7.7803e-02 1.4132e-03 3.7440e-01 2.31258e-01 0.0156 3.7440e-01 3\n",
" 14 3.8031e+00 7.7802e-02 1.3912e-03 3.7114e-01 2.11666e-01 0.0156 3.7114e-01 3\n",
" 15 3.7587e+00 7.7800e-02 1.3694e-03 3.6672e-01 1.94228e-01 0.0312 3.6672e-01 3\n",
" 16 3.7272e+00 7.7798e-02 1.3267e-03 3.6361e-01 1.59679e-01 0.0312 3.6361e-01 3\n",
" 17 3.6376e+00 7.7797e-02 1.2852e-03 3.5469e-01 1.29679e-01 0.0625 3.5469e-01 3\n",
" 18 3.5020e+00 7.7796e-02 1.2049e-03 3.4122e-01 7.82809e-02 0.2500 3.4122e-01 3\n",
" 19 3.1736e+00 7.7797e-02 9.0412e-04 3.0867e-01 5.65467e-02 0.2500 3.0867e-01 3\n",
" 20 2.5994e+00 7.7797e-02 6.7821e-04 2.5148e-01 4.66794e-02 0.5000 2.5148e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 2.2556e+00 7.7799e-02 3.3998e-04 2.1744e-01 1.21359e-01 0.0625 2.1744e-01 3\n",
" 22 2.2096e+00 7.7798e-02 3.1882e-04 2.1286e-01 6.46746e-02 0.1250 2.1286e-01 3\n",
" 23 2.0095e+00 7.7798e-02 2.7904e-04 1.9289e-01 7.43823e-03 1.0000 1.9289e-01 3\n",
" END 9.5342e-02 7.7802e-02 1.3734e-07 1.7539e-03 ---- ---- 1.7539e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8316e+01 8.3285e-02 3.6698e+00 1.5349e-01 5.99332e-01 0.5000 ---- 3\n",
" 2 3.5124e+01 7.8543e-02 1.8500e+00 1.6545e+00 5.86796e-01 0.1250 1.6545e+00 3\n",
" 3 3.3723e+01 7.8224e-02 1.6189e+00 1.7456e+00 2.92285e-01 1.0000 1.7456e+00 3\n",
" 4 1.3434e+01 7.7827e-02 1.3533e-02 1.3221e+00 7.41564e-01 0.0156 1.3221e+00 3\n",
" 5 1.3336e+01 7.7823e-02 1.3321e-02 1.3125e+00 6.38794e-01 0.0156 1.3125e+00 3\n",
" 6 1.3182e+01 7.7820e-02 1.3113e-02 1.2973e+00 5.47594e-01 0.0312 1.2973e+00 3\n",
" 7 1.3047e+01 7.7815e-02 1.2704e-02 1.2842e+00 3.86278e-01 0.0625 1.2842e+00 3\n",
" 8 1.2808e+01 7.7812e-02 1.1910e-02 1.2611e+00 1.54607e-01 0.2500 1.2611e+00 3\n",
" 9 1.0463e+01 7.7815e-02 8.9309e-03 1.0296e+00 2.02299e-01 0.1250 1.0296e+00 3\n",
" 10 1.0065e+01 7.7813e-02 7.8147e-03 9.9086e-01 6.47097e-02 1.0000 9.9086e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 5.5431e+00 7.7832e-02 2.8295e-05 5.4650e-01 5.49823e-01 0.0078 5.4650e-01 3\n",
" 12 5.5334e+00 7.7830e-02 2.8097e-05 5.4553e-01 4.97565e-01 0.0078 5.4553e-01 3\n",
" 13 5.5154e+00 7.7828e-02 2.7898e-05 5.4373e-01 4.48469e-01 0.0078 5.4373e-01 3\n",
" 14 5.4900e+00 7.7827e-02 2.7697e-05 5.4119e-01 3.89839e-01 0.0156 5.4119e-01 3\n",
" 15 5.4865e+00 7.7825e-02 2.7322e-05 5.4084e-01 3.33213e-01 0.0156 5.4084e-01 3\n",
" 16 5.4463e+00 7.7823e-02 2.6937e-05 5.3682e-01 2.88318e-01 0.0156 5.3682e-01 3\n",
" 17 5.3801e+00 7.7822e-02 2.6546e-05 5.3020e-01 2.48277e-01 0.0312 5.3020e-01 3\n",
" 18 5.3064e+00 7.7820e-02 2.5799e-05 5.2283e-01 1.76880e-01 0.0625 5.2283e-01 3\n",
" 19 5.1371e+00 7.7819e-02 2.4342e-05 5.0590e-01 7.18207e-02 0.5000 5.0590e-01 3\n",
" 20 4.0008e+00 7.7824e-02 1.4991e-05 3.9228e-01 2.72912e-01 0.0312 3.9228e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.9884e+00 7.7822e-02 1.4681e-05 3.9104e-01 1.84993e-01 0.0625 3.9104e-01 3\n",
" 22 3.9573e+00 7.7821e-02 1.4028e-05 3.8793e-01 6.64454e-02 0.2500 3.8793e-01 3\n",
" 23 3.2540e+00 7.7822e-02 1.0934e-05 3.1761e-01 1.03248e-01 0.1250 3.1761e-01 3\n",
" 24 3.1420e+00 7.7822e-02 9.8900e-06 3.0641e-01 2.88981e-02 1.0000 3.0641e-01 3\n",
" 25 1.2824e+00 7.7828e-02 8.7826e-07 1.2046e-01 2.73338e-01 0.0039 1.2046e-01 3\n",
" 26 1.2797e+00 7.7828e-02 8.7612e-07 1.2018e-01 2.52252e-01 0.0039 1.2018e-01 3\n",
" 27 1.2763e+00 7.7828e-02 8.7385e-07 1.1985e-01 1.72244e-01 0.0156 1.1985e-01 3\n",
" 28 1.2752e+00 7.7827e-02 8.7161e-07 1.1974e-01 1.54616e-01 0.0156 1.1974e-01 3\n",
" 29 1.2682e+00 7.7826e-02 8.6765e-07 1.1903e-01 1.38508e-01 0.0156 1.1903e-01 3\n",
" 30 1.2566e+00 7.7826e-02 8.6221e-07 1.1788e-01 1.23854e-01 0.0312 1.1788e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.2528e+00 7.7825e-02 8.6237e-07 1.1750e-01 9.70495e-02 0.0312 1.1750e-01 3\n",
" 32 1.2276e+00 7.7825e-02 8.5348e-07 1.1497e-01 7.57628e-02 0.0625 1.1497e-01 3\n",
" 33 1.1901e+00 7.7825e-02 8.4719e-07 1.1122e-01 4.18301e-02 0.2500 1.1122e-01 3\n",
" 34 1.0921e+00 7.7825e-02 8.9686e-07 1.0143e-01 3.52703e-02 0.2500 1.0143e-01 3\n",
" 35 9.4957e-01 7.7825e-02 8.4101e-07 8.7174e-02 3.12737e-02 0.2500 8.7174e-02 3\n",
" 36 8.0395e-01 7.7825e-02 7.7000e-07 7.2612e-02 2.66683e-02 0.2500 7.2612e-02 3\n",
" 37 6.7451e-01 7.7825e-02 6.6196e-07 5.9668e-02 2.40111e-02 0.2500 5.9668e-02 3\n",
" 38 5.6629e-01 7.7825e-02 5.7510e-07 4.8846e-02 2.07327e-02 0.2500 4.8846e-02 3\n",
" 39 4.7925e-01 7.7825e-02 4.7926e-07 4.0142e-02 1.87462e-02 0.2500 4.0142e-02 3\n",
" 40 4.1052e-01 7.7825e-02 4.0663e-07 3.3269e-02 1.63956e-02 0.2500 3.3269e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 3.5725e-01 7.7825e-02 3.3470e-07 2.7942e-02 1.48584e-02 0.2500 2.7942e-02 3\n",
" 42 3.1645e-01 7.7825e-02 2.8026e-07 2.3862e-02 1.32592e-02 0.2500 2.3862e-02 3\n",
" 43 2.8536e-01 7.7825e-02 2.2993e-07 2.0753e-02 1.23141e-02 0.2500 2.0753e-02 3\n",
" 44 2.6113e-01 7.7825e-02 1.9165e-07 1.8330e-02 1.13495e-02 0.5000 1.8330e-02 2\n",
" 45 2.2243e-01 7.7826e-02 1.5079e-07 1.4460e-02 3.20265e-02 0.0312 1.4460e-02 3\n",
" 46 2.2144e-01 7.7826e-02 1.4759e-07 1.4362e-02 2.44636e-02 0.0625 1.4362e-02 3\n",
" 47 2.2021e-01 7.7825e-02 1.4218e-07 1.4238e-02 1.28086e-02 0.2500 1.4238e-02 3\n",
" 48 2.1799e-01 7.7825e-02 1.2622e-07 1.4016e-02 1.20411e-02 0.2500 1.4016e-02 3\n",
" 49 2.1457e-01 7.7825e-02 1.1062e-07 1.3674e-02 1.14918e-02 0.2500 1.3674e-02 3\n",
" 50 2.1095e-01 7.7825e-02 9.7733e-08 1.3312e-02 1.09233e-02 0.2500 1.3312e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8891e+01 8.3302e-02 3.7211e+00 1.5972e-01 5.79537e-01 0.5000 ---- 3\n",
" 2 3.1818e+01 7.8685e-02 1.8718e+00 1.3021e+00 3.80923e-01 0.5000 1.3021e+00 3\n",
" 3 2.4665e+01 7.7911e-02 9.3891e-01 1.5198e+00 6.24454e-01 0.0312 1.5198e+00 3\n",
" 4 2.4218e+01 7.7897e-02 9.0957e-01 1.5044e+00 4.56616e-01 0.0625 1.5044e+00 3\n",
" 5 2.3389e+01 7.7880e-02 8.5273e-01 1.4784e+00 2.50330e-01 0.2500 1.4784e+00 3\n",
" 6 1.9030e+01 7.7867e-02 6.3966e-01 1.2555e+00 2.80349e-01 0.1250 1.2555e+00 3\n",
" 7 1.7716e+01 7.7873e-02 5.5973e-01 1.2041e+00 1.35106e-01 1.0000 1.2041e+00 3\n",
" 8 6.5760e+00 7.8151e-02 1.3416e-03 6.4844e-01 5.61828e-01 0.0078 6.4844e-01 3\n",
" 9 6.5579e+00 7.8149e-02 1.3312e-03 6.4664e-01 5.11811e-01 0.0078 6.4664e-01 3\n",
" 10 6.5311e+00 7.8147e-02 1.3208e-03 6.4398e-01 4.64795e-01 0.0078 6.4398e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 6.4967e+00 7.8145e-02 1.3105e-03 6.4054e-01 4.08309e-01 0.0156 6.4054e-01 3\n",
" 12 6.4801e+00 7.8143e-02 1.2900e-03 6.3890e-01 3.45730e-01 0.0156 6.3890e-01 3\n",
" 13 6.4243e+00 7.8141e-02 1.2699e-03 6.3334e-01 2.91469e-01 0.0312 6.3334e-01 3\n",
" 14 6.4031e+00 7.8139e-02 1.2302e-03 6.3126e-01 2.03205e-01 0.0625 6.3126e-01 3\n",
" 15 6.3191e+00 7.8137e-02 1.1532e-03 6.2295e-01 7.86561e-02 0.5000 6.2295e-01 3\n",
" 16 5.4750e+00 7.8142e-02 5.7446e-04 5.3911e-01 2.94082e-01 0.0156 5.3911e-01 3\n",
" 17 5.4117e+00 7.8141e-02 5.6546e-04 5.3279e-01 2.46136e-01 0.0312 5.3279e-01 3\n",
" 18 5.3490e+00 7.8139e-02 5.4775e-04 5.2654e-01 1.67307e-01 0.0625 5.2654e-01 3\n",
" 19 5.2042e+00 7.8138e-02 5.1346e-04 5.1210e-01 6.09581e-02 0.5000 5.1210e-01 3\n",
" 20 4.1002e+00 7.8143e-02 2.5678e-04 4.0195e-01 2.46622e-01 0.0156 4.0195e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.0551e+00 7.8142e-02 2.5277e-04 3.9745e-01 2.07811e-01 0.0312 3.9745e-01 3\n",
" 22 4.0106e+00 7.8141e-02 2.4486e-04 3.9300e-01 1.42644e-01 0.0625 3.9300e-01 3\n",
" 23 3.8954e+00 7.8140e-02 2.2953e-04 3.8150e-01 5.17402e-02 0.5000 3.8150e-01 3\n",
" 24 2.8348e+00 7.8143e-02 1.1426e-04 2.7556e-01 2.06442e-01 0.0156 2.7556e-01 3\n",
" 25 2.8030e+00 7.8143e-02 1.1246e-04 2.7237e-01 1.73088e-01 0.0312 2.7237e-01 3\n",
" 26 2.7696e+00 7.8142e-02 1.0893e-04 2.6904e-01 1.18119e-01 0.0625 2.6904e-01 3\n",
" 27 2.6864e+00 7.8141e-02 1.0209e-04 2.6072e-01 4.25655e-02 0.5000 2.6072e-01 3\n",
" 28 1.9197e+00 7.8144e-02 5.1116e-05 1.8411e-01 1.72403e-01 0.0156 1.8411e-01 3\n",
" 29 1.8993e+00 7.8143e-02 5.0320e-05 1.8207e-01 1.45003e-01 0.0312 1.8207e-01 3\n",
" 30 1.8781e+00 7.8143e-02 4.8755e-05 1.7995e-01 9.86025e-02 0.0625 1.7995e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.8217e+00 7.8142e-02 4.5719e-05 1.7431e-01 3.53032e-02 0.5000 1.7431e-01 3\n",
" 32 1.2636e+00 7.8144e-02 2.3057e-05 1.1853e-01 1.40239e-01 0.0156 1.1853e-01 3\n",
" 33 1.2505e+00 7.8144e-02 2.2701e-05 1.1721e-01 1.18099e-01 0.0312 1.1721e-01 3\n",
" 34 1.2363e+00 7.8143e-02 2.2005e-05 1.1579e-01 8.05890e-02 0.0625 1.1579e-01 3\n",
" 35 1.1995e+00 7.8143e-02 2.0655e-05 1.1212e-01 2.92399e-02 0.5000 1.1212e-01 3\n",
" 36 8.3865e-01 7.8144e-02 1.0567e-05 7.6040e-02 1.16859e-01 0.0156 7.6040e-02 3\n",
" 37 8.3045e-01 7.8144e-02 1.0406e-05 7.5221e-02 9.82071e-02 0.0312 7.5221e-02 3\n",
" 38 8.2160e-01 7.8144e-02 1.0092e-05 7.4335e-02 6.68371e-02 0.0625 7.4335e-02 3\n",
" 39 7.9822e-01 7.8144e-02 9.4833e-06 7.1998e-02 2.43467e-02 0.5000 7.1998e-02 3\n",
" 40 5.6729e-01 7.8144e-02 4.9380e-06 4.8910e-02 9.51055e-02 0.0156 4.8910e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 5.6218e-01 7.8144e-02 4.8643e-06 4.8399e-02 8.01326e-02 0.0312 4.8399e-02 3\n",
" 42 5.5657e-01 7.8144e-02 4.7220e-06 4.7838e-02 5.48748e-02 0.0625 4.7838e-02 3\n",
" 43 5.4201e-01 7.8144e-02 4.4442e-06 4.6382e-02 2.04065e-02 0.5000 4.6382e-02 3\n",
" 44 4.0076e-01 7.8144e-02 2.3662e-06 3.2259e-02 7.93652e-02 0.0156 3.2259e-02 3\n",
" 45 3.9756e-01 7.8144e-02 2.3314e-06 3.1939e-02 6.68536e-02 0.0312 3.1939e-02 3\n",
" 46 3.9397e-01 7.8144e-02 2.2649e-06 3.1580e-02 4.58321e-02 0.0625 3.1580e-02 3\n",
" 47 3.8477e-01 7.8144e-02 2.1350e-06 3.0660e-02 1.72742e-02 0.5000 3.0660e-02 3\n",
" 48 2.9742e-01 7.8144e-02 1.1713e-06 2.1926e-02 6.55026e-02 0.0312 2.1926e-02 2\n",
" 49 2.9198e-01 7.8144e-02 1.1414e-06 2.1382e-02 4.51519e-02 0.0625 2.1382e-02 2\n",
" 50 2.7798e-01 7.8144e-02 1.0823e-06 1.9983e-02 1.72792e-02 0.5000 1.9983e-02 2\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8603e+01 8.3663e-02 3.6958e+00 1.5612e-01 5.99126e-01 0.5000 ---- 3\n",
" 2 3.4591e+01 7.8857e-02 1.8658e+00 1.5854e+00 4.68871e-01 0.2500 1.5854e+00 3\n",
" 3 3.3682e+01 7.8278e-02 1.4003e+00 1.9601e+00 5.17555e-01 0.1250 1.9601e+00 3\n",
" 4 3.3356e+01 7.8120e-02 1.2254e+00 2.1024e+00 2.76315e-01 1.0000 2.1024e+00 3\n",
" 5 2.6271e+01 7.8094e-02 8.9947e-03 2.6102e+00 1.00644e+00 0.0156 2.6102e+00 3\n",
" 6 2.6118e+01 7.8084e-02 8.8541e-03 2.5951e+00 8.31998e-01 0.0156 2.5951e+00 3\n",
" 7 2.5857e+01 7.8076e-02 8.7157e-03 2.5691e+00 6.86288e-01 0.0312 2.5691e+00 3\n",
" 8 2.5752e+01 7.8065e-02 8.4431e-03 2.5590e+00 4.63677e-01 0.0625 2.5590e+00 3\n",
" 9 2.5682e+01 7.8056e-02 7.9150e-03 2.5524e+00 2.18488e-01 0.2500 2.5524e+00 3\n",
" 10 2.2428e+01 7.8061e-02 5.9340e-03 2.2291e+00 3.07428e-01 0.1250 2.2291e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.1586e+01 7.8059e-02 5.1917e-03 2.1456e+00 9.89296e-02 1.0000 2.1456e+00 3\n",
" 12 7.5957e+00 7.8104e-02 4.9642e-05 7.5171e-01 7.64942e-01 0.0078 7.5171e-01 3\n",
" 13 7.5586e+00 7.8101e-02 4.9301e-05 7.4800e-01 6.94528e-01 0.0078 7.4800e-01 3\n",
" 14 7.5134e+00 7.8099e-02 4.8955e-05 7.4348e-01 6.29756e-01 0.0156 7.4348e-01 3\n",
" 15 7.4890e+00 7.8095e-02 4.8323e-05 7.4104e-01 5.20166e-01 0.0156 7.4104e-01 3\n",
" 16 7.4175e+00 7.8093e-02 4.7660e-05 7.3389e-01 4.32095e-01 0.0312 7.3389e-01 3\n",
" 17 7.3665e+00 7.8089e-02 4.6423e-05 7.2879e-01 2.88334e-01 0.0625 7.2879e-01 3\n",
" 18 7.1940e+00 7.8087e-02 4.3954e-05 7.1155e-01 9.94042e-02 0.5000 7.1155e-01 3\n",
" 19 5.1863e+00 7.8093e-02 2.5604e-05 5.1079e-01 3.90716e-01 0.0156 5.1079e-01 3\n",
" 20 5.1279e+00 7.8091e-02 2.5232e-05 5.0495e-01 3.24717e-01 0.0312 5.0495e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 5.0676e+00 7.8089e-02 2.4530e-05 4.9893e-01 2.14274e-01 0.0625 4.9893e-01 3\n",
" 22 4.9113e+00 7.8088e-02 2.3157e-05 4.8329e-01 6.63229e-02 0.5000 4.8329e-01 3\n",
" 23 3.4547e+00 7.8092e-02 1.2130e-05 3.3765e-01 2.92185e-01 0.0156 3.3765e-01 3\n",
" 24 3.4169e+00 7.8091e-02 1.1967e-05 3.3387e-01 2.44102e-01 0.0312 3.3387e-01 3\n",
" 25 3.3774e+00 7.8090e-02 1.1670e-05 3.2992e-01 1.62808e-01 0.0625 3.2992e-01 3\n",
" 26 3.2713e+00 7.8089e-02 1.1089e-05 3.1931e-01 5.38070e-02 0.5000 3.1931e-01 3\n",
" 27 2.2460e+00 7.8092e-02 6.9548e-06 2.1678e-01 2.33507e-01 0.0156 2.1678e-01 3\n",
" 28 2.2199e+00 7.8092e-02 6.8648e-06 2.1417e-01 1.93392e-01 0.0312 2.1417e-01 3\n",
" 29 2.1901e+00 7.8091e-02 6.7016e-06 2.1119e-01 1.26879e-01 0.0625 2.1119e-01 3\n",
" 30 2.1194e+00 7.8091e-02 6.3688e-06 2.0413e-01 3.98228e-02 0.5000 2.0413e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.5559e+00 7.8092e-02 3.5968e-06 1.4778e-01 1.71496e-01 0.0156 1.4778e-01 3\n",
" 32 1.5390e+00 7.8092e-02 3.5501e-06 1.4608e-01 1.43586e-01 0.0312 1.4608e-01 3\n",
" 33 1.5200e+00 7.8092e-02 3.4675e-06 1.4419e-01 9.65681e-02 0.0625 1.4419e-01 3\n",
" 34 1.4726e+00 7.8091e-02 3.3075e-06 1.3944e-01 3.27073e-02 0.5000 1.3944e-01 3\n",
" 35 1.0722e+00 7.8092e-02 2.1974e-06 9.9410e-02 1.40194e-01 0.0312 9.9410e-02 3\n",
" 36 1.0700e+00 7.8092e-02 2.1631e-06 9.9185e-02 9.25570e-02 0.0625 9.9185e-02 3\n",
" 37 1.0595e+00 7.8092e-02 2.0856e-06 9.8139e-02 3.00425e-02 0.5000 9.8139e-02 3\n",
" 38 1.0072e+00 7.8092e-02 1.3575e-06 9.2913e-02 1.25187e-01 0.0156 9.2913e-02 3\n",
" 39 9.9725e-01 7.8092e-02 1.3418e-06 9.1915e-02 1.05231e-01 0.0312 9.1915e-02 3\n",
" 40 9.8709e-01 7.8092e-02 1.3163e-06 9.0898e-02 7.12781e-02 0.0625 9.0898e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 9.6155e-01 7.8092e-02 1.2675e-06 8.8345e-02 2.50489e-02 0.5000 8.8345e-02 3\n",
" 42 7.7166e-01 7.8092e-02 9.7329e-07 6.9356e-02 1.04651e-01 0.0312 6.9356e-02 3\n",
" 43 7.6974e-01 7.8092e-02 9.6624e-07 6.9164e-02 6.97447e-02 0.0625 6.9164e-02 3\n",
" 44 7.6316e-01 7.8092e-02 9.4534e-07 6.8506e-02 2.40961e-02 0.2500 6.8506e-02 3\n",
" 45 6.3854e-01 7.8092e-02 7.6720e-07 5.6044e-02 3.76796e-02 0.1250 5.6044e-02 3\n",
" 46 6.1335e-01 7.8092e-02 7.1849e-07 5.3525e-02 1.19613e-02 1.0000 5.3525e-02 3\n",
" 47 2.6273e-01 7.8093e-02 1.7249e-07 1.8464e-02 1.21031e-01 0.0020 1.8464e-02 1\n",
" 48 2.6273e-01 7.8093e-02 1.7249e-07 1.8464e-02 1.20558e-01 0.0039 1.8464e-02 1\n",
" 49 2.6256e-01 7.8093e-02 1.7175e-07 1.8446e-02 1.10752e-01 0.0020 1.8446e-02 1\n",
" 50 2.6254e-01 7.8093e-02 1.7140e-07 1.8445e-02 7.64039e-02 0.0078 1.8445e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8592e+01 8.3601e-02 3.6833e+00 1.6750e-01 6.18852e-01 0.5000 ---- 3\n",
" 2 3.4884e+01 7.8823e-02 1.8581e+00 1.6224e+00 6.35684e-01 0.1250 1.6224e+00 3\n",
" 3 3.3727e+01 7.8500e-02 1.6261e+00 1.7388e+00 3.61665e-01 0.5000 1.7388e+00 3\n",
" 4 2.4282e+01 7.7902e-02 8.1606e-01 1.6044e+00 7.13214e-01 0.0312 1.6044e+00 3\n",
" 5 2.4192e+01 7.7888e-02 7.9056e-01 1.6209e+00 5.23741e-01 0.0625 1.6209e+00 3\n",
" 6 2.3793e+01 7.7872e-02 7.4116e-01 1.6304e+00 2.45180e-01 0.5000 1.6304e+00 3\n",
" 7 1.8230e+01 7.7896e-02 3.7119e-01 1.4440e+00 6.67782e-01 0.0312 1.4440e+00 3\n",
" 8 1.8194e+01 7.7893e-02 3.5959e-01 1.4520e+00 4.55708e-01 0.0625 1.4520e+00 3\n",
" 9 1.8023e+01 7.7894e-02 3.3711e-01 1.4574e+00 1.78949e-01 0.5000 1.4574e+00 3\n",
" 10 1.5846e+01 7.7969e-02 1.6869e-01 1.4081e+00 6.10778e-01 0.0156 1.4081e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.5680e+01 7.7967e-02 1.6605e-01 1.3941e+00 5.20711e-01 0.0312 1.3941e+00 3\n",
" 12 1.5522e+01 7.7966e-02 1.6086e-01 1.3836e+00 3.62481e-01 0.0625 1.3836e+00 3\n",
" 13 1.5087e+01 7.7968e-02 1.5081e-01 1.3501e+00 1.37278e-01 0.5000 1.3501e+00 3\n",
" 14 1.0592e+01 7.8021e-02 7.5433e-02 9.7592e-01 5.10187e-01 0.0156 9.7592e-01 3\n",
" 15 1.0468e+01 7.8020e-02 7.4255e-02 9.6472e-01 4.25357e-01 0.0312 9.6472e-01 3\n",
" 16 1.0332e+01 7.8018e-02 7.1934e-02 9.5350e-01 2.86546e-01 0.0625 9.5350e-01 3\n",
" 17 9.9989e+00 7.8019e-02 6.7438e-02 9.2464e-01 9.99248e-02 0.5000 9.2464e-01 3\n",
" 18 6.7960e+00 7.8048e-02 3.3725e-02 6.3807e-01 4.03909e-01 0.0156 6.3807e-01 3\n",
" 19 6.7217e+00 7.8047e-02 3.3198e-02 6.3116e-01 3.38871e-01 0.0312 6.3116e-01 3\n",
" 20 6.6426e+00 7.8046e-02 3.2161e-02 6.2430e-01 2.27970e-01 0.0625 6.2430e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 6.4224e+00 7.8047e-02 3.0151e-02 6.0429e-01 7.73540e-02 0.5000 6.0429e-01 3\n",
" 22 4.0660e+00 7.8062e-02 1.5078e-02 3.8371e-01 3.15589e-01 0.0156 3.8371e-01 3\n",
" 23 4.0193e+00 7.8061e-02 1.4842e-02 3.7928e-01 2.63403e-01 0.0312 3.7928e-01 3\n",
" 24 3.9655e+00 7.8061e-02 1.4379e-02 3.7437e-01 1.75612e-01 0.0625 3.7437e-01 3\n",
" 25 3.8259e+00 7.8061e-02 1.3480e-02 3.6130e-01 5.85549e-02 0.5000 3.6130e-01 3\n",
" 26 2.4295e+00 7.8068e-02 6.7404e-03 2.2841e-01 2.45281e-01 0.0156 2.2841e-01 3\n",
" 27 2.4023e+00 7.8068e-02 6.6351e-03 2.2579e-01 2.04405e-01 0.0312 2.2579e-01 3\n",
" 28 2.3700e+00 7.8068e-02 6.4278e-03 2.2277e-01 1.36078e-01 0.0625 2.2277e-01 3\n",
" 29 2.2852e+00 7.8068e-02 6.0261e-03 2.1468e-01 4.54201e-02 0.5000 2.1468e-01 3\n",
" 30 1.4626e+00 7.8071e-02 3.0134e-03 1.3544e-01 1.88285e-01 0.0312 1.3544e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.4592e+00 7.8071e-02 2.9192e-03 1.3519e-01 1.25473e-01 0.0625 1.3519e-01 3\n",
" 32 1.4364e+00 7.8071e-02 2.7368e-03 1.3310e-01 4.23021e-02 0.5000 1.3310e-01 3\n",
" 33 1.1635e+00 7.8073e-02 1.3686e-03 1.0717e-01 1.74158e-01 0.0156 1.0717e-01 3\n",
" 34 1.1510e+00 7.8073e-02 1.3472e-03 1.0595e-01 1.45397e-01 0.0312 1.0595e-01 3\n",
" 35 1.1362e+00 7.8072e-02 1.3051e-03 1.0451e-01 9.74094e-02 0.0625 1.0451e-01 3\n",
" 36 1.0994e+00 7.8072e-02 1.2236e-03 1.0091e-01 3.36710e-02 0.5000 1.0091e-01 3\n",
" 37 8.1782e-01 7.8073e-02 6.1206e-04 7.3363e-02 1.36003e-01 0.0312 7.3363e-02 3\n",
" 38 8.1264e-01 7.8073e-02 5.9295e-04 7.2864e-02 9.16211e-02 0.0625 7.2864e-02 3\n",
" 39 7.9729e-01 7.8073e-02 5.5591e-04 7.1366e-02 3.23515e-02 0.5000 7.1366e-02 3\n",
" 40 7.1890e-01 7.8074e-02 2.7829e-04 6.3805e-02 1.28196e-01 0.0312 6.3805e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 7.1436e-01 7.8073e-02 2.6961e-04 6.3359e-02 8.68293e-02 0.0625 6.3359e-02 3\n",
" 42 7.0160e-01 7.8073e-02 2.5279e-04 6.2100e-02 3.11636e-02 0.5000 6.2100e-02 3\n",
" 43 6.7655e-01 7.8074e-02 1.2681e-04 5.9721e-02 1.22569e-01 0.0312 5.9721e-02 3\n",
" 44 6.7050e-01 7.8073e-02 1.2286e-04 5.9120e-02 8.32680e-02 0.0625 5.9120e-02 3\n",
" 45 6.5679e-01 7.8073e-02 1.1522e-04 5.7756e-02 3.04856e-02 0.2500 5.7756e-02 3\n",
" 46 5.5590e-01 7.8073e-02 8.6536e-05 4.7696e-02 4.35137e-02 0.2500 4.7696e-02 3\n",
" 47 5.4788e-01 7.8073e-02 6.5007e-05 4.6916e-02 6.95228e-02 0.0625 4.6916e-02 3\n",
" 48 5.3376e-01 7.8073e-02 6.0959e-05 4.5508e-02 2.33702e-02 0.5000 4.5508e-02 3\n",
" 49 3.9359e-01 7.8074e-02 3.0603e-05 3.1521e-02 9.95484e-02 0.0020 3.1521e-02 1\n",
" 50 3.9359e-01 7.8074e-02 3.0603e-05 3.1521e-02 9.95724e-02 0.0156 3.1521e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8869e+01 8.3585e-02 3.6856e+00 1.9287e-01 6.06594e-01 0.5000 ---- 3\n",
" 2 3.5972e+01 7.8819e-02 1.8582e+00 1.7311e+00 5.79164e-01 0.1250 1.7311e+00 3\n",
" 3 3.3785e+01 7.8500e-02 1.6262e+00 1.7445e+00 3.26796e-01 1.0000 1.7445e+00 3\n",
" 4 2.3583e+01 7.8092e-02 1.3817e-02 2.3366e+00 1.06351e+00 0.0156 2.3366e+00 3\n",
" 5 2.3454e+01 7.8084e-02 1.3601e-02 2.3240e+00 8.95102e-01 0.0156 2.3240e+00 3\n",
" 6 2.3211e+01 7.8078e-02 1.3388e-02 2.2999e+00 7.54828e-01 0.0312 2.2999e+00 3\n",
" 7 2.3045e+01 7.8070e-02 1.2970e-02 2.2837e+00 5.29147e-01 0.0625 2.2837e+00 3\n",
" 8 2.2670e+01 7.8064e-02 1.2158e-02 2.2470e+00 2.14585e-01 0.5000 2.2470e+00 3\n",
" 9 2.1472e+01 7.8079e-02 6.0652e-03 2.1333e+00 7.71871e-01 0.0312 2.1333e+00 3\n",
" 10 2.1459e+01 7.8070e-02 5.8754e-03 2.1322e+00 5.46640e-01 0.0625 2.1322e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.1401e+01 7.8063e-02 5.5078e-03 2.1268e+00 2.06538e-01 0.2500 2.1268e+00 3\n",
" 12 1.7763e+01 7.8069e-02 4.1306e-03 1.7644e+00 3.08411e-01 0.1250 1.7644e+00 3\n",
" 13 1.7330e+01 7.8067e-02 3.6140e-03 1.7215e+00 9.95904e-02 1.0000 1.7215e+00 3\n",
" 14 1.1268e+01 7.8102e-02 9.1877e-06 1.1190e+00 9.18204e-01 0.0078 1.1190e+00 3\n",
" 15 1.1236e+01 7.8098e-02 9.1306e-06 1.1158e+00 8.37798e-01 0.0078 1.1158e+00 3\n",
" 16 1.1190e+01 7.8095e-02 9.0751e-06 1.1111e+00 7.61693e-01 0.0078 1.1111e+00 3\n",
" 17 1.1131e+01 7.8092e-02 9.0203e-06 1.1052e+00 6.70006e-01 0.0156 1.1052e+00 3\n",
" 18 1.1105e+01 7.8088e-02 8.9394e-06 1.1027e+00 5.63793e-01 0.0156 1.1027e+00 3\n",
" 19 1.1014e+01 7.8084e-02 8.8527e-06 1.0936e+00 4.72648e-01 0.0312 1.0936e+00 3\n",
" 20 1.0998e+01 7.8080e-02 8.7474e-06 1.0920e+00 3.29862e-01 0.0625 1.0920e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.0914e+01 7.8077e-02 8.5615e-06 1.0836e+00 1.29992e-01 0.5000 1.0836e+00 3\n",
" 22 1.0560e+01 7.8085e-02 1.3133e-05 1.0482e+00 5.22907e-01 0.0156 1.0482e+00 3\n",
" 23 1.0431e+01 7.8083e-02 1.3134e-05 1.0353e+00 4.42289e-01 0.0312 1.0353e+00 3\n",
" 24 1.0299e+01 7.8080e-02 1.3271e-05 1.0221e+00 3.01999e-01 0.0625 1.0221e+00 3\n",
" 25 1.0028e+01 7.8077e-02 1.3304e-05 9.9499e-01 1.06436e-01 0.5000 9.9499e-01 3\n",
" 26 8.8965e+00 7.8085e-02 1.1139e-05 8.8183e-01 4.20027e-01 0.0156 8.8183e-01 3\n",
" 27 8.7908e+00 7.8083e-02 1.1006e-05 8.7126e-01 3.54371e-01 0.0312 8.7126e-01 3\n",
" 28 8.6825e+00 7.8081e-02 1.0807e-05 8.6043e-01 2.51023e-01 0.0625 8.6043e-01 3\n",
" 29 8.4375e+00 7.8079e-02 1.0547e-05 8.3593e-01 9.85609e-02 0.5000 8.3593e-01 3\n",
" 30 6.8717e+00 7.8086e-02 1.3272e-05 6.7934e-01 3.99670e-01 0.0312 6.7934e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 6.8686e+00 7.8083e-02 1.3409e-05 6.7904e-01 2.72017e-01 0.0625 6.7904e-01 3\n",
" 32 6.8455e+00 7.8081e-02 1.3451e-05 6.7673e-01 9.52032e-02 0.2500 6.7673e-01 3\n",
" 33 5.6165e+00 7.8083e-02 1.1224e-05 5.5383e-01 1.51868e-01 0.1250 5.5383e-01 3\n",
" 34 5.4800e+00 7.8083e-02 1.0626e-05 5.4018e-01 4.31346e-02 1.0000 5.4018e-01 3\n",
" 35 3.6237e+00 7.8094e-02 2.1713e-06 3.5456e-01 4.35949e-01 0.0078 3.5456e-01 3\n",
" 36 3.6189e+00 7.8093e-02 2.1620e-06 3.5408e-01 3.80270e-01 0.0078 3.5408e-01 3\n",
" 37 3.6056e+00 7.8091e-02 2.1515e-06 3.5275e-01 2.46435e-01 0.0156 3.5275e-01 3\n",
" 38 3.5703e+00 7.8090e-02 2.1334e-06 3.4922e-01 2.16575e-01 0.0312 3.4922e-01 3\n",
" 39 3.5622e+00 7.8088e-02 2.1219e-06 3.4841e-01 1.67797e-01 0.0312 3.4841e-01 3\n",
" 40 3.4897e+00 7.8087e-02 2.0991e-06 3.4116e-01 1.32509e-01 0.0625 3.4116e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 3.3887e+00 7.8086e-02 2.0998e-06 3.3106e-01 7.53094e-02 0.2500 3.3106e-01 3\n",
" 42 3.1398e+00 7.8086e-02 2.5361e-06 3.0617e-01 6.43541e-02 0.2500 3.0617e-01 3\n",
" 43 2.8237e+00 7.8087e-02 2.6007e-06 2.7456e-01 5.95519e-02 0.2500 2.7456e-01 3\n",
" 44 2.5226e+00 7.8087e-02 2.4839e-06 2.4445e-01 5.38918e-02 0.2500 2.4445e-01 3\n",
" 45 2.2812e+00 7.8087e-02 2.2044e-06 2.2031e-01 5.33821e-02 0.2500 2.2031e-01 3\n",
" 46 2.1071e+00 7.8088e-02 1.9318e-06 2.0290e-01 5.15732e-02 0.2500 2.0290e-01 3\n",
" 47 1.9973e+00 7.8088e-02 1.6472e-06 1.9192e-01 5.15862e-02 0.2500 1.9192e-01 3\n",
" 48 1.9438e+00 7.8088e-02 1.4113e-06 1.8657e-01 5.10434e-02 0.2500 1.8657e-01 3\n",
" 49 1.9426e+00 7.8088e-02 1.2180e-06 1.8645e-01 5.15367e-02 0.1250 1.8645e-01 3\n",
" 50 1.7734e+00 7.8088e-02 1.1003e-06 1.6953e-01 4.84535e-03 1.0000 1.6953e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.9008e+01 8.3596e-02 3.7398e+00 1.5264e-01 5.85630e-01 0.5000 ---- 3\n",
" 2 3.2444e+01 7.8959e-02 1.8812e+00 1.3553e+00 3.94783e-01 0.5000 1.3553e+00 3\n",
" 3 2.5571e+01 7.8184e-02 9.4356e-01 1.6057e+00 7.89975e-01 0.0312 1.6057e+00 3\n",
" 4 2.5287e+01 7.8169e-02 9.1408e-01 1.6068e+00 5.61701e-01 0.0625 1.6068e+00 3\n",
" 5 2.4715e+01 7.8150e-02 8.5696e-01 1.6067e+00 2.53685e-01 0.5000 1.6067e+00 3\n",
" 6 2.3769e+01 7.8180e-02 4.2898e-01 1.9401e+00 6.18047e-01 0.0312 1.9401e+00 3\n",
" 7 2.3552e+01 7.8177e-02 4.1557e-01 1.9319e+00 4.44660e-01 0.0625 1.9319e+00 3\n",
" 8 2.3143e+01 7.8180e-02 3.8960e-01 1.9169e+00 2.22016e-01 0.2500 1.9169e+00 3\n",
" 9 1.9057e+01 7.8222e-02 2.9225e-01 1.6056e+00 2.63051e-01 0.1250 1.6056e+00 3\n",
" 10 1.7757e+01 7.8238e-02 2.5572e-01 1.5122e+00 1.22276e-01 1.0000 1.5122e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.3129e+01 7.8427e-02 3.3627e-04 1.3048e+00 8.64039e-01 0.0078 1.3048e+00 3\n",
" 12 1.3065e+01 7.8423e-02 3.3382e-04 1.2983e+00 7.95317e-01 0.0078 1.2983e+00 3\n",
" 13 1.2989e+01 7.8420e-02 3.3135e-04 1.2907e+00 7.28012e-01 0.0156 1.2907e+00 3\n",
" 14 1.2960e+01 7.8415e-02 3.2665e-04 1.2878e+00 6.08878e-01 0.0156 1.2878e+00 3\n",
" 15 1.2854e+01 7.8411e-02 3.2185e-04 1.2773e+00 5.07300e-01 0.0312 1.2773e+00 3\n",
" 16 1.2842e+01 7.8406e-02 3.1255e-04 1.2760e+00 3.32004e-01 0.0625 1.2760e+00 3\n",
" 17 1.2751e+01 7.8403e-02 2.9404e-04 1.2670e+00 9.89512e-02 0.5000 1.2670e+00 3\n",
" 18 1.1482e+01 7.8413e-02 1.4888e-04 1.1402e+00 4.17043e-01 0.0156 1.1402e+00 3\n",
" 19 1.1358e+01 7.8411e-02 1.4657e-04 1.1278e+00 3.40487e-01 0.0312 1.1278e+00 3\n",
" 20 1.1259e+01 7.8407e-02 1.4204e-04 1.1179e+00 2.33714e-01 0.0625 1.1179e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.1009e+01 7.8405e-02 1.3336e-04 1.0929e+00 9.34345e-02 0.5000 1.0929e+00 3\n",
" 22 8.5926e+00 7.8414e-02 7.3929e-05 8.5135e-01 4.00356e-01 0.0156 8.5135e-01 3\n",
" 23 8.4923e+00 7.8413e-02 7.2929e-05 8.4131e-01 3.30365e-01 0.0312 8.4131e-01 3\n",
" 24 8.3952e+00 7.8410e-02 7.1036e-05 8.3161e-01 2.12778e-01 0.0625 8.3161e-01 3\n",
" 25 8.1756e+00 7.8409e-02 6.7095e-05 8.0965e-01 6.19931e-02 0.5000 8.0965e-01 3\n",
" 26 6.4784e+00 7.8416e-02 3.4473e-05 6.3997e-01 2.64176e-01 0.0156 6.3997e-01 3\n",
" 27 6.4101e+00 7.8414e-02 3.3945e-05 6.3314e-01 2.19143e-01 0.0312 6.3314e-01 3\n",
" 28 6.3522e+00 7.8412e-02 3.2926e-05 6.2734e-01 1.55962e-01 0.0625 6.2734e-01 3\n",
" 29 6.1905e+00 7.8411e-02 3.1026e-05 6.1118e-01 6.22534e-02 0.5000 6.1118e-01 3\n",
" 30 4.4299e+00 7.8417e-02 1.9908e-05 4.3513e-01 2.54867e-01 0.0156 4.3513e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 4.3804e+00 7.8416e-02 1.9675e-05 4.3018e-01 2.10023e-01 0.0312 4.3018e-01 3\n",
" 32 4.3322e+00 7.8415e-02 1.9250e-05 4.2536e-01 1.35990e-01 0.0625 4.2536e-01 3\n",
" 33 4.2118e+00 7.8414e-02 1.8292e-05 4.1332e-01 4.26831e-02 0.5000 4.1332e-01 3\n",
" 34 3.0239e+00 7.8418e-02 9.6768e-06 2.9454e-01 1.76176e-01 0.0156 2.9454e-01 3\n",
" 35 2.9942e+00 7.8417e-02 9.5357e-06 2.9156e-01 1.50706e-01 0.0312 2.9156e-01 3\n",
" 36 2.9672e+00 7.8416e-02 9.2743e-06 2.8886e-01 1.05860e-01 0.0625 2.8886e-01 3\n",
" 37 2.8796e+00 7.8416e-02 8.7965e-06 2.8011e-01 3.95664e-02 0.5000 2.8011e-01 3\n",
" 38 1.8189e+00 7.8418e-02 5.9445e-06 1.7404e-01 1.57580e-01 0.0156 1.7404e-01 3\n",
" 39 1.7987e+00 7.8418e-02 5.8749e-06 1.7203e-01 1.30159e-01 0.0312 1.7203e-01 3\n",
" 40 1.7759e+00 7.8418e-02 5.7486e-06 1.6974e-01 8.52017e-02 0.0625 1.6974e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 1.7160e+00 7.8418e-02 5.4682e-06 1.6376e-01 2.75842e-02 0.5000 1.6376e-01 3\n",
" 42 1.1086e+00 7.8419e-02 3.0204e-06 1.0301e-01 1.14235e-01 0.0156 1.0301e-01 3\n",
" 43 1.0970e+00 7.8419e-02 2.9797e-06 1.0185e-01 9.63934e-02 0.0312 1.0185e-01 3\n",
" 44 1.0830e+00 7.8419e-02 2.9064e-06 1.0046e-01 6.57585e-02 0.0625 1.0046e-01 3\n",
" 45 1.0454e+00 7.8418e-02 2.7659e-06 9.6698e-02 2.35940e-02 0.5000 9.6698e-02 3\n",
" 46 6.9308e-01 7.8419e-02 1.8275e-06 6.1464e-02 9.12957e-02 0.0312 6.1464e-02 3\n",
" 47 6.8930e-01 7.8419e-02 1.7928e-06 6.1086e-02 6.08220e-02 0.0625 6.1086e-02 3\n",
" 48 6.7632e-01 7.8419e-02 1.7162e-06 5.9788e-02 2.11318e-02 0.5000 5.9788e-02 3\n",
" 49 5.7351e-01 7.8419e-02 1.1048e-06 4.9508e-02 8.24832e-02 0.0312 4.9508e-02 3\n",
" 50 5.7063e-01 7.8419e-02 1.0852e-06 4.9220e-02 5.61723e-02 0.0625 4.9220e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.9092e+01 8.3972e-02 3.7093e+00 1.9158e-01 5.80640e-01 0.5000 ---- 3\n",
" 2 3.3902e+01 7.9137e-02 1.8728e+00 1.5095e+00 3.85609e-01 0.5000 1.5095e+00 3\n",
" 3 2.7429e+01 7.8235e-02 9.4064e-01 1.7945e+00 6.01119e-01 0.0625 1.7945e+00 3\n",
" 4 2.6710e+01 7.8204e-02 8.8187e-01 1.7813e+00 3.02044e-01 0.2500 1.7813e+00 3\n",
" 5 2.5299e+01 7.8152e-02 6.6166e-01 1.8604e+00 3.44841e-01 0.2500 1.8604e+00 3\n",
" 6 2.4440e+01 7.8149e-02 4.9639e-01 1.9398e+00 4.21812e-01 0.1250 1.9398e+00 3\n",
" 7 2.4026e+01 7.8152e-02 4.3436e-01 1.9605e+00 1.88297e-01 1.0000 1.9605e+00 3\n",
" 8 2.3510e+01 7.8361e-02 1.2551e-03 2.3419e+00 1.16396e+00 0.0156 2.3419e+00 3\n",
" 9 2.3364e+01 7.8352e-02 1.2365e-03 2.3274e+00 9.73697e-01 0.0156 2.3274e+00 3\n",
" 10 2.3127e+01 7.8345e-02 1.2179e-03 2.3037e+00 8.10063e-01 0.0312 2.3037e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.3039e+01 7.8335e-02 1.1817e-03 2.2949e+00 5.34660e-01 0.0312 2.2949e+00 3\n",
" 12 2.2477e+01 7.8330e-02 1.1455e-03 2.2387e+00 3.68464e-01 0.0625 2.2387e+00 3\n",
" 13 2.1464e+01 7.8327e-02 1.0750e-03 2.1375e+00 1.49116e-01 0.5000 2.1375e+00 3\n",
" 14 1.7971e+01 7.8344e-02 5.3839e-04 1.7888e+00 6.17716e-01 0.0312 1.7888e+00 3\n",
" 15 1.7828e+01 7.8338e-02 5.2171e-04 1.7744e+00 4.29558e-01 0.0625 1.7744e+00 3\n",
" 16 1.7654e+01 7.8332e-02 4.8967e-04 1.7571e+00 1.55321e-01 0.2500 1.7571e+00 3\n",
" 17 1.5476e+01 7.8336e-02 3.6958e-04 1.5394e+00 2.28923e-01 0.1250 1.5394e+00 3\n",
" 18 1.5119e+01 7.8334e-02 3.2450e-04 1.5038e+00 8.12441e-02 1.0000 1.5038e+00 3\n",
" 19 5.7994e+00 7.8367e-02 2.3451e-05 5.7208e-01 7.81961e-01 0.0039 5.7208e-01 3\n",
" 20 5.7835e+00 7.8365e-02 2.3394e-05 5.7049e-01 7.46472e-01 0.0039 5.7049e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 5.7664e+00 7.8364e-02 2.3334e-05 5.6878e-01 7.09442e-01 0.0078 5.6878e-01 3\n",
" 22 5.7637e+00 7.8361e-02 2.3261e-05 5.6851e-01 6.17534e-01 0.0078 5.6851e-01 3\n",
" 23 5.7475e+00 7.8359e-02 2.3160e-05 5.6689e-01 3.96060e-01 0.0156 5.6689e-01 3\n",
" 24 5.7011e+00 7.8357e-02 2.2960e-05 5.6225e-01 3.45860e-01 0.0156 5.6225e-01 3\n",
" 25 5.6387e+00 7.8355e-02 2.2718e-05 5.5601e-01 3.02437e-01 0.0312 5.5601e-01 3\n",
" 26 5.6115e+00 7.8352e-02 2.2341e-05 5.5329e-01 2.28428e-01 0.0312 5.5329e-01 3\n",
" 27 5.4917e+00 7.8350e-02 2.1813e-05 5.4131e-01 1.75933e-01 0.0625 5.4131e-01 3\n",
" 28 5.3260e+00 7.8348e-02 2.0822e-05 5.2474e-01 9.89835e-02 0.2500 5.2474e-01 3\n",
" 29 4.9125e+00 7.8349e-02 1.7415e-05 4.8339e-01 8.84728e-02 0.2500 4.8339e-01 3\n",
" 30 4.5191e+00 7.8349e-02 1.4417e-05 4.4406e-01 8.14523e-02 0.2500 4.4406e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 4.2220e+00 7.8350e-02 1.1747e-05 4.1435e-01 7.81478e-02 0.2500 4.1435e-01 3\n",
" 32 4.0529e+00 7.8350e-02 9.5544e-06 3.9745e-01 7.50609e-02 0.2500 3.9745e-01 3\n",
" 33 4.0239e+00 7.8350e-02 7.7293e-06 3.9455e-01 7.57514e-02 0.1250 3.9455e-01 3\n",
" 34 3.6639e+00 7.8350e-02 6.8907e-06 3.5855e-01 1.03470e-02 1.0000 3.5855e-01 3\n",
" END 1.3125e-01 7.8358e-02 1.4171e-06 5.2881e-03 ---- ---- 6.3684e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.8594e+01 8.3904e-02 3.7011e+00 1.4995e-01 6.06476e-01 0.5000 ---- 3\n",
" 2 3.4716e+01 7.9098e-02 1.8673e+00 1.5964e+00 6.16149e-01 0.1250 1.5964e+00 3\n",
" 3 3.3814e+01 7.8772e-02 1.6341e+00 1.7394e+00 3.54114e-01 0.5000 1.7394e+00 3\n",
" 4 2.9216e+01 7.8167e-02 8.2016e-01 2.0937e+00 8.55984e-01 0.0312 2.0937e+00 3\n",
" 5 2.9211e+01 7.8149e-02 7.9453e-01 2.1187e+00 5.94121e-01 0.0625 2.1187e+00 3\n",
" 6 2.9151e+01 7.8130e-02 7.4488e-01 2.1624e+00 2.49862e-01 0.5000 2.1624e+00 3\n",
" 7 2.7781e+01 7.8155e-02 3.7304e-01 2.3972e+00 7.28092e-01 0.0312 2.3972e+00 3\n",
" 8 2.7698e+01 7.8147e-02 3.6138e-01 2.4006e+00 4.71642e-01 0.0312 2.4006e+00 3\n",
" 9 2.7046e+01 7.8145e-02 3.5009e-01 2.3467e+00 3.28802e-01 0.0625 2.3467e+00 3\n",
" 10 2.5786e+01 7.8149e-02 3.2821e-01 2.2426e+00 1.61391e-01 0.5000 2.2426e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.7417e+01 7.8233e-02 1.6426e-01 1.5697e+00 5.56945e-01 0.0312 1.5697e+00 3\n",
" 12 1.7352e+01 7.8229e-02 1.5912e-01 1.5683e+00 3.68662e-01 0.0625 1.5683e+00 3\n",
" 13 1.7202e+01 7.8229e-02 1.4918e-01 1.5632e+00 1.16861e-01 0.5000 1.5632e+00 3\n",
" 14 1.6545e+01 7.8282e-02 7.4613e-02 1.5721e+00 4.48690e-01 0.0156 1.5721e+00 3\n",
" 15 1.6347e+01 7.8279e-02 7.3447e-02 1.5534e+00 3.63110e-01 0.0312 1.5534e+00 3\n",
" 16 1.6160e+01 7.8276e-02 7.1152e-02 1.5371e+00 2.45717e-01 0.0625 1.5371e+00 3\n",
" 17 1.5796e+01 7.8275e-02 6.6705e-02 1.5050e+00 1.14111e-01 0.5000 1.5050e+00 3\n",
" 18 1.3800e+01 7.8308e-02 3.3369e-02 1.3388e+00 4.71726e-01 0.0312 1.3388e+00 3\n",
" 19 1.3757e+01 7.8303e-02 3.2327e-02 1.3355e+00 3.12557e-01 0.0625 1.3355e+00 3\n",
" 20 1.3734e+01 7.8300e-02 3.0308e-02 1.3353e+00 9.53452e-02 0.2500 1.3353e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.1748e+01 7.8309e-02 2.2731e-02 1.1442e+00 1.56224e-01 0.0625 1.1442e+00 3\n",
" 22 1.1181e+01 7.8309e-02 2.1310e-02 1.0889e+00 7.06317e-02 0.5000 1.0889e+00 3\n",
" 23 6.9056e+00 7.8326e-02 1.0662e-02 6.7206e-01 2.80697e-01 0.0312 6.7206e-01 3\n",
" 24 6.8943e+00 7.8324e-02 1.0329e-02 6.7127e-01 1.78087e-01 0.0625 6.7127e-01 3\n",
" 25 6.8478e+00 7.8323e-02 9.6841e-03 6.6726e-01 5.18037e-02 0.5000 6.6726e-01 3\n",
" 26 6.2124e+00 7.8332e-02 4.8421e-03 6.0857e-01 2.17716e-01 0.0156 6.0857e-01 3\n",
" 27 6.1538e+00 7.8330e-02 4.7664e-03 6.0278e-01 1.78884e-01 0.0312 6.0278e-01 3\n",
" 28 6.1163e+00 7.8328e-02 4.6175e-03 5.9918e-01 1.30243e-01 0.0625 5.9918e-01 3\n",
" 29 5.9871e+00 7.8327e-02 4.3289e-03 5.8655e-01 5.35769e-02 0.5000 5.8655e-01 3\n",
" 30 4.2214e+00 7.8334e-02 2.1673e-03 4.1214e-01 2.24281e-01 0.0156 4.1214e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 4.1758e+00 7.8333e-02 2.1335e-03 4.0761e-01 1.84185e-01 0.0312 4.0761e-01 3\n",
" 32 4.1340e+00 7.8332e-02 2.0669e-03 4.0350e-01 1.17816e-01 0.0625 4.0350e-01 3\n",
" 33 4.0239e+00 7.8332e-02 1.9379e-03 3.9262e-01 3.35295e-02 0.5000 3.9262e-01 3\n",
" 34 2.7397e+00 7.8336e-02 9.6890e-04 2.6517e-01 1.47835e-01 0.0156 2.6517e-01 3\n",
" 35 2.7160e+00 7.8335e-02 9.5377e-04 2.6281e-01 1.27398e-01 0.0312 2.6281e-01 3\n",
" 36 2.6983e+00 7.8335e-02 9.2397e-04 2.6107e-01 8.94117e-02 0.0625 2.6107e-01 3\n",
" 37 2.6226e+00 7.8334e-02 8.6626e-04 2.5356e-01 3.21942e-02 1.0000 2.5356e-01 3\n",
" 38 2.0707e+00 7.8340e-02 3.3258e-06 1.9924e-01 3.12652e-01 0.0078 1.9924e-01 3\n",
" 39 2.0667e+00 7.8340e-02 3.3231e-06 1.9884e-01 2.85544e-01 0.0078 1.9884e-01 3\n",
" 40 2.0598e+00 7.8339e-02 3.3155e-06 1.9814e-01 2.59545e-01 0.0078 1.9814e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 2.0503e+00 7.8338e-02 3.3038e-06 1.9720e-01 2.27208e-01 0.0156 1.9720e-01 3\n",
" 42 2.0477e+00 7.8338e-02 3.2938e-06 1.9693e-01 1.88807e-01 0.0156 1.9693e-01 3\n",
" 43 2.0323e+00 7.8337e-02 3.2673e-06 1.9540e-01 1.56640e-01 0.0312 1.9540e-01 3\n",
" 44 2.0288e+00 7.8336e-02 3.2246e-06 1.9505e-01 1.03482e-01 0.0625 1.9505e-01 3\n",
" 45 2.0050e+00 7.8336e-02 3.0968e-06 1.9266e-01 3.44213e-02 0.5000 1.9266e-01 3\n",
" 46 1.6866e+00 7.8338e-02 1.7447e-06 1.6083e-01 1.35082e-01 0.0156 1.6083e-01 3\n",
" 47 1.6705e+00 7.8337e-02 1.7198e-06 1.5921e-01 1.15595e-01 0.0312 1.5921e-01 3\n",
" 48 1.6552e+00 7.8337e-02 1.6746e-06 1.5769e-01 8.08919e-02 0.0625 1.5769e-01 3\n",
" 49 1.6078e+00 7.8336e-02 1.5942e-06 1.5294e-01 3.08749e-02 0.5000 1.5294e-01 3\n",
" 50 1.1121e+00 7.8338e-02 1.1893e-06 1.0338e-01 1.20056e-01 0.0312 1.0338e-01 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 3.9593e+01 8.3878e-02 3.6967e+00 2.5420e-01 6.05452e-01 0.2500 ---- 3\n",
" 2 3.4881e+01 8.0978e-02 2.7764e+00 7.0357e-01 5.00401e-01 0.2500 8.5972e-01 3\n",
" 3 3.1184e+01 7.9466e-02 2.0844e+00 1.0261e+00 4.92750e-01 0.1250 1.0261e+00 3\n",
" 4 3.0073e+01 7.9040e-02 1.8242e+00 1.1752e+00 3.20209e-01 1.0000 1.1752e+00 3\n",
" 5 1.6781e+01 7.8360e-02 1.7898e-02 1.6523e+00 6.15068e-01 0.0156 1.6523e+00 3\n",
" 6 1.6604e+01 7.8355e-02 1.7619e-02 1.6349e+00 5.40147e-01 0.0312 1.6349e+00 3\n",
" 7 1.6571e+01 7.8346e-02 1.7070e-02 1.6322e+00 3.99410e-01 0.0312 1.6322e+00 3\n",
" 8 1.6257e+01 7.8341e-02 1.6537e-02 1.6013e+00 2.86132e-01 0.0625 1.6013e+00 3\n",
" 9 1.5861e+01 7.8337e-02 1.5504e-02 1.5628e+00 1.14433e-01 0.2500 1.5628e+00 3\n",
" 10 1.4036e+01 7.8340e-02 1.1628e-02 1.3842e+00 1.53158e-01 0.1250 1.3842e+00 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.3728e+01 7.8338e-02 1.0175e-02 1.3548e+00 5.32676e-02 1.0000 1.3548e+00 3\n"
]
},
{
"ename": "KeyboardInterrupt",
"evalue": "",
"output_type": "error",
"traceback": [
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)",
"Cell \u001b[0;32mIn[19], line 17\u001b[0m\n\u001b[1;32m 15\u001b[0m q \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mhstack([q, np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m2\u001b[39m)])\n\u001b[1;32m 16\u001b[0m dq \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mhstack([dq, np\u001b[38;5;241m.\u001b[39mzeros(\u001b[38;5;241m2\u001b[39m)])\n\u001b[0;32m---> 17\u001b[0m solution \u001b[38;5;241m=\u001b[39m \u001b[43mmpc\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mupdateAndSolve\u001b[49m\u001b[43m(\u001b[49m\u001b[43mt\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mquat\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mq\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mv\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43momega\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mdq\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 18\u001b[0m mpc\u001b[38;5;241m.\u001b[39mmax_iterations\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m50\u001b[39m\n\u001b[1;32m 20\u001b[0m \u001b[38;5;66;03m# solution=mpc.getSolution()\u001b[39;00m\n",
"Cell \u001b[0;32mIn[3], line 273\u001b[0m, in \u001b[0;36mGo2MPC.updateAndSolve\u001b[0;34m(self, t, quat, q, v, omega, dq)\u001b[0m\n\u001b[1;32m 271\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mxs[\u001b[38;5;241m0\u001b[39m] \u001b[38;5;241m=\u001b[39m x\n\u001b[1;32m 272\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mus \u001b[38;5;241m=\u001b[39m \u001b[38;5;28mlist\u001b[39m(\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mus[\u001b[38;5;241m1\u001b[39m:]) \u001b[38;5;241m+\u001b[39m [\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mus[\u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m]] \n\u001b[0;32m--> 273\u001b[0m \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msolver\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msolve\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mxs\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mus\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mmax_iterations\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 274\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mxs, \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mus \u001b[38;5;241m=\u001b[39m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msolver\u001b[38;5;241m.\u001b[39mxs, \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msolver\u001b[38;5;241m.\u001b[39mus\n\u001b[1;32m 275\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mgetSolution()\n",
"File \u001b[0;32m/home/Go2py/examples/friction_utils.py:37\u001b[0m, in \u001b[0;36mResidualFrictionCone.calc\u001b[0;34m(self, data, x, u)\u001b[0m\n\u001b[1;32m 34\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdf_dx \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mzeros((\u001b[38;5;241m3\u001b[39m, \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mstate\u001b[38;5;241m.\u001b[39mndx))\n\u001b[1;32m 35\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdf_du \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39mzeros((\u001b[38;5;241m3\u001b[39m, \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mnu))\n\u001b[0;32m---> 37\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21mcalc\u001b[39m(\u001b[38;5;28mself\u001b[39m, data, x, u\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mNone\u001b[39;00m): \n\u001b[1;32m 38\u001b[0m F \u001b[38;5;241m=\u001b[39m data\u001b[38;5;241m.\u001b[39mshared\u001b[38;5;241m.\u001b[39mcontacts\u001b[38;5;241m.\u001b[39mcontacts[\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mcontact_name]\u001b[38;5;241m.\u001b[39mf\u001b[38;5;241m.\u001b[39mvector[:\u001b[38;5;241m3\u001b[39m] \n\u001b[1;32m 39\u001b[0m data\u001b[38;5;241m.\u001b[39mr[\u001b[38;5;241m0\u001b[39m] \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39marray([\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mmu \u001b[38;5;241m*\u001b[39m F[\u001b[38;5;241m2\u001b[39m] \u001b[38;5;241m-\u001b[39m np\u001b[38;5;241m.\u001b[39msqrt(F[\u001b[38;5;241m0\u001b[39m]\u001b[38;5;241m*\u001b[39m\u001b[38;5;241m*\u001b[39m\u001b[38;5;241m2\u001b[39m \u001b[38;5;241m+\u001b[39m F[\u001b[38;5;241m1\u001b[39m]\u001b[38;5;241m*\u001b[39m\u001b[38;5;241m*\u001b[39m\u001b[38;5;241m2\u001b[39m)])\n",
"\u001b[0;31mKeyboardInterrupt\u001b[0m: "
]
}
],
"source": [
"mpc.max_iterations=250\n",
"measured_forces = []\n",
"forces = np.linspace(0, 30, 500)\n",
"\n",
"for i in range(500):\n",
" for action_model in m:\n",
" action_model.differential.costs.costs['contact_force_track'].cost.residual.reference.linear[:] = np.array([-forces[i], 0, 0])\n",
"\n",
" state = robot.getJointStates()\n",
" q = state['q']\n",
" dq = state['dq']\n",
" t, quat = robot.getPose()\n",
" v = robot.data.qvel[:3]\n",
" omega = robot.data.qvel[3:6]\n",
" q = np.hstack([q, np.zeros(2)])\n",
" dq = np.hstack([dq, np.zeros(2)])\n",
" solution = mpc.updateAndSolve(t, quat, q, v, omega, dq)\n",
" mpc.max_iterations=50\n",
"\n",
" # solution=mpc.getSolution()\n",
" q = solution['q']\n",
" dq = solution['dq']\n",
" tau = solution['tau'].squeeze()\n",
" kp = np.ones(18)*0.\n",
" kv = np.ones(18)*0.\n",
" \n",
" for j in range(1):\n",
" robot.setCommands(q, dq, kp, kv, tau)\n",
" robot.step()\n",
" time.sleep(0.0005)\n",
"\n",
" force_sensor_site = 'EF_force_site'\n",
" measured_forces.append(getForceSensor(robot.model, robot.data, force_sensor_site).squeeze().copy())"
]
},
{
"cell_type": "code",
"execution_count": 20,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"[array([ 2.36041365, -0.01271313, 0.21047075]),\n",
" array([ 2.012907 , -0.01195061, 0.15061032]),\n",
" array([-7.19264913e-02, -2.43680904e-04, 3.20132220e-01]),\n",
" array([-7.35830100e-02, -2.28343417e-04, 3.29105130e-01]),\n",
" array([-7.48201848e-02, -2.51832990e-04, 3.36572438e-01]),\n",
" array([-7.58311067e-02, -2.52655457e-04, 3.42880538e-01]),\n",
" array([-7.67749819e-02, -2.45489330e-04, 3.48139460e-01]),\n",
" array([-7.74147732e-02, -2.45611115e-04, 3.52593208e-01]),\n",
" array([-7.51799907e-02, -2.49595235e-04, 3.52291887e-01]),\n",
" array([-7.27421838e-02, -2.58548279e-04, 3.51466826e-01]),\n",
" array([-7.03909194e-02, -2.51109477e-04, 3.50251399e-01]),\n",
" array([-6.81643896e-02, -2.68337988e-04, 3.48610655e-01]),\n",
" array([-6.58271340e-02, -2.73744534e-04, 3.47002290e-01]),\n",
" array([-6.35328870e-02, -2.80816844e-04, 3.45191680e-01]),\n",
" array([-6.12988905e-02, -2.75578158e-04, 3.43166249e-01]),\n",
" array([-5.90895554e-02, -2.90596650e-04, 3.41010062e-01]),\n",
" array([-5.68938808e-02, -2.88333367e-04, 3.38762650e-01]),\n",
" array([-5.47468894e-02, -2.96789509e-04, 3.36386851e-01]),\n",
" array([-5.26250013e-02, -2.93486916e-04, 3.33934455e-01]),\n",
" array([-5.05062043e-02, -3.00485428e-04, 3.31435860e-01]),\n",
" array([-4.84305943e-02, -3.13226149e-04, 3.28819449e-01]),\n",
" array([-4.63077168e-02, -2.91450021e-04, 3.26305074e-01]),\n",
" array([-4.42597262e-02, -3.21467748e-04, 3.23607752e-01]),\n",
" array([-4.21723108e-02, -2.97206962e-04, 3.21018815e-01]),\n",
" array([-0.04013321, -0.00032462, 0.31828968]),\n",
" array([-0.03815512, -0.00037519, 0.31555252]),\n",
" array([-0.03603842, -0.0003568 , 0.31293405]),\n",
" array([-0.03406683, -0.00032016, 0.31015595]),\n",
" array([-0.03210945, -0.00033781, 0.30735169]),\n",
" array([-0.03030273, -0.00036753, 0.30427516]),\n",
" array([-0.0282078 , -0.0003205 , 0.30175248]),\n",
" array([ 4.25258817, -0.04935582, -1.18920453]),\n",
" array([ 3.84581255, -0.04450924, -0.94333459]),\n",
" array([ 3.8257496 , -0.09105494, -0.63014117]),\n",
" array([ 3.42898381, -0.0800539 , -0.53549505]),\n",
" array([ 2.81181225, -0.02247349, -0.5846897 ]),\n",
" array([ 2.60526997, -0.021634 , -0.51207846]),\n",
" array([-0.01200819, -0.00081854, 0.27537819]),\n",
" array([-0.00955017, -0.00073847, 0.27516538]),\n",
" array([-0.00716449, -0.00066131, 0.27446774]),\n",
" array([-0.00493098, -0.00060512, 0.27322877]),\n",
" array([-0.00275087, -0.00055286, 0.2716535 ]),\n",
" array([-0.0006031 , -0.00052095, 0.26987676]),\n",
" array([ 0.00129831, -0.00047327, 0.26762236]),\n",
" array([ 0.00321895, -0.00045112, 0.26528507]),\n",
" array([ 4.37512333, -0.03384868, -0.86939463]),\n",
" array([ 4.05855381, -0.03112588, -0.6778479 ]),\n",
" array([ 3.78642959, -0.0287511 , -0.53653948]),\n",
" array([ 3.55373276, -0.02692741, -0.43356175]),\n",
" array([ 0.01357152, -0.00049985, 0.24321857]),\n",
" array([ 0.01591732, -0.00046942, 0.24296175]),\n",
" array([ 0.01814359, -0.00040669, 0.24216797]),\n",
" array([ 0.02022318, -0.00038086, 0.24087357]),\n",
" array([ 0.02216796, -0.00039681, 0.23914576]),\n",
" array([ 4.88507991, -0.0332406 , -0.68146232]),\n",
" array([ 4.56795705, -0.03071432, -0.51025025]),\n",
" array([ 4.29576149, -0.0286556 , -0.38480657]),\n",
" array([ 4.06775155, -0.02669228, -0.29205438]),\n",
" array([ 3.87204885, -0.02499788, -0.22714655]),\n",
" array([ 0.0352867 , -0.00045732, 0.21770382]),\n",
" array([ 0.03764866, -0.00043469, 0.21746507]),\n",
" array([ 0.03972341, -0.00041873, 0.21648098]),\n",
" array([ 0.04167871, -0.00040514, 0.21502954]),\n",
" array([ 0.04346281, -0.00038754, 0.21310202]),\n",
" array([ 5.50961951, -0.03241728, -0.56388168]),\n",
" array([ 5.19384095, -0.03021597, -0.39118805]),\n",
" array([ 4.9157313 , -0.02840738, -0.26799363]),\n",
" array([ 4.68472006, -0.02669191, -0.17611489]),\n",
" array([ 4.48721855, -0.02517283, -0.11148384]),\n",
" array([ 0.05650461, -0.00040831, 0.19265057]),\n",
" array([ 0.05866658, -0.00039891, 0.19194492]),\n",
" array([ 0.06055528, -0.00039712, 0.19060425]),\n",
" array([ 5.51608074, -0.02898215, -0.3441006 ]),\n",
" array([ 5.25876106, -0.02748755, -0.21440132]),\n",
" array([ 5.03896702, -0.02601653, -0.12016867]),\n",
" array([ 0.06893377, -0.00038193, 0.17750645]),\n",
" array([ 0.07075639, -0.00038195, 0.17628113]),\n",
" array([ 5.71673208, -0.02838917, -0.25010623]),\n",
" array([ 5.47062837, -0.02690865, -0.13283135]),\n",
" array([ 5.26069847, -0.02559855, -0.04796151]),\n",
" array([ 0.08097225, -0.00036982, 0.17064814]),\n",
" array([ 0.08272164, -0.00037592, 0.16859953]),\n",
" array([ 6.04533914, -0.02843166, -0.16971111]),\n",
" array([ 5.79572859, -0.02702459, -0.05768291]),\n",
" array([ 5.5828431 , -0.02574226, 0.02295742]),\n",
" array([ 0.09307336, -0.00036108, 0.16469485]),\n",
" array([ 5.92012125e+00, -2.65876255e-02, -5.55156537e-03]),\n",
" array([ 0.09779271, -0.00036572, 0.16204656]),\n",
" array([ 6.26464924, -0.02780953, -0.00921896]),\n",
" array([ 6.03316315, -0.02660894, 0.07788308]),\n",
" array([ 5.83641909, -0.02543501, 0.13930205]),\n",
" array([ 0.10813505, -0.00035457, 0.15793298]),\n",
" array([ 0.10978532, -0.00036821, 0.15577105]),\n",
" array([ 6.86174395, -0.02919023, 0.04167674]),\n",
" array([ 6.59312128, -0.02809282, 0.13705436]),\n",
" array([ 6.36453905, -0.02685582, 0.20451428]),\n",
" array([ 6.16765642, -0.02568877, 0.24917255]),\n",
" array([ 6.01111043, -0.02461625, 0.28277568]),\n",
" array([ 0.12554161, -0.00033707, 0.1498187 ]),\n",
" array([ 0.12740266, -0.00036297, 0.14779668]),\n",
" array([ 7.19682123, -0.02915793, 0.18282246]),\n",
" array([ 6.93706753, -0.02799682, 0.25887201]),\n",
" array([ 6.72111105, -0.02696793, 0.3135319 ]),\n",
" array([ 6.53453682, -0.02595765, 0.34804885]),\n",
" array([ 0.14026191, -0.00033703, 0.14245124]),\n",
" array([ 0.14202417, -0.00037135, 0.14035576]),\n",
" array([ 7.8517995 , -0.03102715, 0.28214496]),\n",
" array([ 7.5561335 , -0.02985117, 0.35825324]),\n",
" array([ 7.30574358, -0.02852946, 0.41047303]),\n",
" array([ 7.08170157, -0.02762102, 0.43875987]),\n",
" array([ 6.90720871, -0.02663321, 0.46117161]),\n",
" array([ 6.75830992, -0.02582623, 0.47203579]),\n",
" array([ 6.63789541, -0.02512852, 0.47722521]),\n",
" array([ 0.16291504, -0.00032533, 0.13179121]),\n",
" array([ 0.16445544, -0.00037626, 0.12946446]),\n",
" array([ 8.16854872, -0.03095442, 0.41027932]),\n",
" array([ 7.90661983, -0.02992857, 0.47150088]),\n",
" array([ 7.68107067, -0.0289101 , 0.51075786]),\n",
" array([ 7.49218155, -0.02799975, 0.53579315]),\n",
" array([ 7.33291527, -0.02735284, 0.54934216]),\n",
" array([ 0.17984613, -0.00034653, 0.12266159]),\n",
" array([ 8.07993646, -0.03005768, 0.53201717]),\n",
" array([ 7.8777857 , -0.02926871, 0.56683215]),\n",
" array([ 7.69976549, -0.02850196, 0.5845353 ]),\n",
" array([ 0.18967161, -0.00036301, 0.11745359]),\n",
" array([ 8.48350172, -0.03143146, 0.58062682]),\n",
" array([ 8.2576805 , -0.03054625, 0.61521362]),\n",
" array([ 8.06672995, -0.02972225, 0.63571668])]"
]
},
"execution_count": 20,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"measured_forces"
]
},
{
"cell_type": "code",
"execution_count": 21,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([[ 8.06672995],\n",
" [-0.02972225],\n",
" [ 0.63571668]])"
]
},
"execution_count": 21,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"getForceSensor(robot.model, robot.data, force_sensor_site)"
]
},
{
"cell_type": "code",
"execution_count": 22,
"metadata": {},
"outputs": [],
"source": [
"measured_forces = np.array(measured_forces)"
]
},
{
"cell_type": "code",
"execution_count": 24,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"[<matplotlib.lines.Line2D at 0x7babc01835e0>]"
]
},
"execution_count": 24,
"metadata": {},
"output_type": "execute_result"
},
{
"data": {
"image/png": "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",
"text/plain": [
"<Figure size 640x480 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import matplotlib.pyplot as plt\n",
"plt.plot(measured_forces[:300,0],'.')\n",
"plt.plot(forces,'k')"
]
},
{
"cell_type": "code",
"execution_count": 29,
"metadata": {},
"outputs": [],
"source": [
"for i in range(20):\n",
" m[i].differential.contacts.changeContactStatus('Link6_contact', True)"
]
},
{
"cell_type": "code",
"execution_count": 70,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"3.204797053666826e-05\n",
"2.9483716376834795e-05\n",
"2.7619477044929236e-05\n",
"2.5276355798602247e-05\n",
"2.326922954256662e-05\n",
"2.1509695934123958e-05\n",
"1.974757393335907e-05\n",
"1.791699699688713e-05\n",
"1.6054902037289995e-05\n",
"1.4193750943263636e-05\n",
"1.2351430002573735e-05\n",
"1.0528752339604057e-05\n",
"8.747743455994619e-06\n",
"7.157192535647804e-06\n",
"6.037534948060255e-06\n",
"5.56430443548056e-06\n",
"6.128822190129092e-06\n",
"8.373882310508166e-06\n",
"1.122733115529139e-05\n",
"3.663458594648195e-05\n"
]
}
],
"source": [
"for i in range(len(mpc.solver.problem.runningDatas)):\n",
" print(mpc.solver.problem.runningDatas[i].differential.costs.costs['contact_force_track'].cost)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"for i in range(len(m)):\n",
" print(m[i].differential.costs.costs['contact_force_track'].weight)"
]
},
{
"cell_type": "code",
"execution_count": 102,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([[-35.4984317 ],\n",
" [ 6.85876494],\n",
" [ 36.03953757]])"
]
},
"execution_count": 102,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"getForceInWorld(robot, force_in_body, force_sensor_site)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"site_Fs = robot.data.sensordata[10:].reshape(-1,3)\n",
"site_Fs\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"getForceInWorld(robot, force_in_body, force_sensor_site).squeeze()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"f = np.array(efs)\n",
"f.shape"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"f = np.array(efs)\n",
"import matplotlib.pyplot as plt\n",
"plt.plot(f[:,-1,...])"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"T = np.array([[1, 0, 0., 1], [0, -1, 0, -1], [0, 0, -1, 2], [0, 0, 0, 1]])\n",
"np.linalg.inv(T)@np.array([0, 0, 1, 1]).reshape(4,1)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"np.linalg.norm(np.array([-1.7, 1.7, -0.7])-np.array([0.4, 0.2, -0.15]))"
]
},
{
"cell_type": "code",
"execution_count": 106,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([-2.78942222, 0.09545357, 14.59104325])"
]
},
"execution_count": 106,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"robot.data.sensordata[-3:]"
]
},
{
"cell_type": "code",
"execution_count": 31,
"metadata": {},
"outputs": [],
"source": [
"import mujoco as mj"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": 125,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([[-14.0236976 ],\n",
" [ 0.12457404],\n",
" [ 11.73103634]])"
]
},
"execution_count": 125,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"import mujoco as mj\n",
"\n",
"\n",
"\n",
"def getForceInWorld(robot, force_in_body, force_sensor_site):\n",
" force_site_id = mj.mj_name2id(robot.model, mj.mjtObj.mjOBJ_SITE, force_sensor_site)\n",
" q = robot.data.xquat[force_site_id]\n",
" R = np.zeros(9)\n",
" mj.mju_quat2Mat(R, q)\n",
" R = R.reshape(3,3)\n",
" T = np.eye(4)\n",
" T[:3,:3] = R\n",
" force_in_world = R@force_in_body.reshape(3,1)\n",
" return force_in_world[:3]\n",
"\n",
"\n",
"force_site_to_sensor_idx = {'FL_force_site': 0, 'FR_force_site': 1, 'RL_force_site': 2, 'RR_force_site': 3, 'EF_force_site': 4}\n",
"force_sensor_site = 'EF_force_site'\n",
"\n",
"site_Fs = robot.data.sensordata[10:].reshape(-1,3).T\n",
"force_in_body = site_Fs[:,force_site_to_sensor_idx[force_sensor_site]].copy()\n",
"getForceInWorld(robot, force_in_body, force_sensor_site)"
]
},
{
"cell_type": "code",
"execution_count": 131,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([[ 7.41051792e-01, -2.11092834e-05, -6.71447869e-01],\n",
" [ 1.02491083e-03, 9.99998870e-01, 1.09971716e-03],\n",
" [ 6.71447088e-01, -1.50312157e-03, 7.41050976e-01]])"
]
},
"execution_count": 131,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"force_site_id = mj.mj_name2id(robot.model, mj.mjtObj.mjOBJ_SITE, force_sensor_site)\n",
"q = robot.data.xquat[force_site_id]\n",
"t = robot.data.xpos[force_site_id]\n",
"R = np.zeros(9)\n",
"mj.mju_quat2Mat(R, q)\n",
"R = R.reshape(3,3)\n",
"T = np.eye(4)\n",
"T[:3,:3] = R\n",
"T[:3,3] = t\n",
"R.T@robot.data.sensordata[-3:].reshape(3,1)\n",
"R"
]
},
{
"cell_type": "code",
"execution_count": 132,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([[ 7.41051792e-01, -2.11092834e-05, -6.71447869e-01],\n",
" [ 1.02491083e-03, 9.99998870e-01, 1.09971716e-03],\n",
" [ 6.71447088e-01, -1.50312157e-03, 7.41050976e-01]])"
]
},
"execution_count": 132,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"from scipy.spatial.transform import Rotation as R \n",
"R.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()"
]
},
{
"cell_type": "code",
"execution_count": 133,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([ 9.33019512e-01, -6.97423445e-04, -3.59824993e-01, 2.80278198e-04])"
]
},
"execution_count": 133,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"q"
]
},
{
"cell_type": "code",
"execution_count": 124,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([-2.51538838, 0.10723676, 18.1096148 ])"
]
},
"execution_count": 124,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"robot.data.sensordata[-3:]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "go2py",
"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.8.19"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
Reference in New Issue View Git Blame Copy Permalink