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

1929 lines
121 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": 13,
"metadata": {},
"outputs": [],
"source": [
"map = np.zeros((1200, 1200))\n",
"map[:,649:679] = 400\n",
"robot.updateHeightMap(map)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"crocoddyl.ResidualModelContactForce()"
]
},
{
"cell_type": "code",
"execution_count": 82,
"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",
"\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": 90,
"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": 91,
"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.64436e+01 0.2500 ---- 1\n",
" 2 1.0246e+03 7.9830e+02 1.1818e-01 2.2513e+01 9.00410e+00 1.0000 3.4482e+01 3\n",
" 3 2.0287e+02 4.4096e-02 1.9268e+00 1.8356e+01 4.54558e-01 1.0000 1.8356e+01 3\n",
" 4 2.3121e-01 2.6888e-02 6.4193e-03 1.4013e-02 2.01404e-01 1.0000 1.4013e-02 3\n",
" END 5.2528e-02 2.6883e-02 1.0228e-04 2.4622e-03 ---- ---- 8.7128e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 8.4088e-02 2.6883e-02 3.2584e-03 2.4622e-03 3.53499e-01 0.1250 ---- 1\n",
" END 8.0387e-02 2.6860e-02 2.9069e-03 2.4458e-03 ---- ---- 8.8679e-03 -----\n"
]
},
{
"data": {
"text/plain": [
"{'position': array([8.49096344e-05, 7.12386747e-07, 3.30091126e-01]),\n",
" 'orientation': array([ 1.00000000e+00, -2.95059465e-09, -4.26870805e-07, -8.22838455e-08]),\n",
" 'velocity': array([4.24548366e-03, 3.56196733e-05, 4.55631457e-03]),\n",
" 'omega': array([-2.95059466e-07, -4.26870805e-05, -8.22838454e-06]),\n",
" 'q': array([-2.24171355e-06, 7.78305497e-01, -1.56004499e+00, -2.24171355e-06,\n",
" 7.78307083e-01, -1.56004786e+00, -2.45196847e-06, 7.78306530e-01,\n",
" -1.56004706e+00, -2.45196847e-06, 7.78308250e-01, -1.56005020e+00,\n",
" -3.56746513e-06, 2.99640306e-01, -2.99372996e-01, -2.56088582e-07,\n",
" 2.42239543e-04, -4.17813700e-08]),\n",
" 'dq': array([-1.12085678e-04, -1.14615656e-03, 3.04763598e-02, -1.12085678e-04,\n",
" -1.06685110e-03, 3.03329025e-02, -1.22598424e-04, -1.09451532e-03,\n",
" 3.03728725e-02, -1.22598424e-04, -1.00852323e-03, 3.02160155e-02,\n",
" -1.78373256e-04, -1.79846942e-02, 3.13502042e-02, -1.28044291e-05,\n",
" 1.21119771e-02, -2.08906850e-06]),\n",
" 'tau': array([[ 1.60973043e+00, 6.52793606e-01, 5.80848434e+00,\n",
" -1.60771163e+00, 7.11735771e-01, 5.80232386e+00,\n",
" 1.61326591e+00, 6.22923769e-01, 5.87636783e+00,\n",
" -1.61370361e+00, 6.09937438e-01, 5.89086526e+00,\n",
" -5.39941723e-05, -8.40052031e-01, -5.05972542e-01,\n",
" 3.09532882e-05, -1.15111223e-01, 2.09376529e-06]]),\n",
" 'constraint_norm': 0.0024458497789834333}"
]
},
"execution_count": 91,
"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": 92,
"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": 119,
"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([-20, 0, 0])"
]
},
{
"cell_type": "code",
"execution_count": 120,
"metadata": {},
"outputs": [],
"source": [
"# activated_before = False\n",
"# m = list(mpc.solver.problem.runningModels) + [mpc.solver.problem.terminalModel]\n",
"# ee_x0 = np.array([ 0.28038019, -0.00128244, 0.55])\n",
"# ee_x_offset = np.linspace(0, 0.25, 100)\n",
"# body_center = np.array([0.00060515, 0. , 0.29477433])\n",
"# efs = []"
]
},
{
"cell_type": "code",
"execution_count": 121,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.3784e+02 7.4249e+01 6.3575e+00 1.9538e-03 5.87298e+00 0.1250 ---- 25\n",
" 2 1.2630e+02 5.6854e+01 5.5459e+00 1.3984e+00 6.34059e+00 0.0625 1.1409e+01 6\n",
" 3 1.1952e+02 4.9974e+01 5.1913e+00 1.7636e+00 5.88194e+00 0.0625 1.0682e+01 6\n",
" 4 1.1385e+02 4.3927e+01 4.8567e+00 2.1359e+00 6.25102e+00 0.0625 9.9960e+00 6\n",
" 5 1.1076e+02 3.8611e+01 4.5389e+00 2.6764e+00 5.96031e+00 0.0312 9.3443e+00 7\n",
" 6 1.0920e+02 3.6238e+01 4.3935e+00 2.9031e+00 6.31130e+00 0.0312 9.0368e+00 7\n",
" 7 1.0724e+02 3.4011e+01 4.2519e+00 3.0708e+00 6.02883e+00 0.0312 8.7357e+00 9\n",
" 8 1.0558e+02 3.1921e+01 4.1148e+00 3.2510e+00 6.16074e+00 0.0312 8.4435e+00 7\n",
" 9 1.0491e+02 2.9960e+01 3.9819e+00 3.5131e+00 5.69932e+00 0.0156 8.1591e+00 9\n",
" 10 1.0404e+02 2.9033e+01 3.9188e+00 3.5822e+00 5.79365e+00 0.0625 8.0226e+00 9\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.0242e+02 2.5521e+01 3.6551e+00 4.0343e+00 6.32175e+00 0.0156 7.4776e+00 9\n",
" 12 1.0174e+02 2.4731e+01 3.5970e+00 4.1038e+00 5.39340e+00 0.0312 7.3513e+00 9\n",
" 13 1.0032e+02 2.3213e+01 3.4803e+00 4.2303e+00 5.39414e+00 0.0156 7.1211e+00 7\n",
" 14 1.0010e+02 2.2495e+01 3.4252e+00 4.3349e+00 5.20205e+00 0.0312 7.0290e+00 9\n",
" 15 9.8853e+01 2.1114e+01 3.3144e+00 4.4595e+00 5.25406e+00 0.0078 6.8456e+00 7\n",
" 16 9.8397e+01 2.0786e+01 3.2884e+00 4.4727e+00 4.94180e+00 0.0625 6.8010e+00 8\n",
" 17 9.7534e+01 1.8274e+01 3.0676e+00 4.8584e+00 5.48200e+00 0.0078 6.4440e+00 8\n",
" 18 9.7334e+01 1.7990e+01 3.0437e+00 4.8907e+00 4.48528e+00 0.0156 6.4017e+00 9\n",
" 19 9.7097e+01 1.7434e+01 2.9955e+00 4.9707e+00 4.52978e+00 0.0312 6.3175e+00 7\n",
" 20 9.6083e+01 1.6365e+01 2.8990e+00 5.0727e+00 4.63498e+00 0.0078 6.1512e+00 9\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 9.5732e+01 1.6111e+01 2.8763e+00 5.0858e+00 4.21977e+00 0.0625 6.1100e+00 9\n",
" 22 9.5134e+01 1.4166e+01 2.6848e+00 5.4120e+00 5.35604e+00 0.0078 5.7846e+00 11\n",
" 23 9.4970e+01 1.3946e+01 2.6639e+00 5.4385e+00 3.46395e+00 0.0156 5.7451e+00 10\n",
" 24 9.4431e+01 1.3516e+01 2.6219e+00 5.4696e+00 3.45060e+00 0.2500 5.6674e+00 10\n",
" 25 9.0304e+01 7.6150e+00 1.7986e+00 6.4703e+00 6.23558e+00 0.0078 6.4703e+00 21\n",
" 26 9.0132e+01 7.4972e+00 1.7846e+00 6.4789e+00 2.60786e+00 0.1250 6.4789e+00 17\n",
" 27 8.9634e+01 5.7529e+00 1.5468e+00 6.8414e+00 1.99818e+00 0.0625 6.8414e+00 15\n",
" 28 8.7017e+01 5.0634e+00 1.4486e+00 6.7468e+00 1.81798e+00 1.0000 6.7468e+00 13\n",
" 29 8.1310e+01 6.4522e-02 9.4491e-01 7.1796e+00 6.58006e+00 0.0156 7.1796e+00 7\n",
" 30 8.0704e+01 6.3803e-02 9.3007e-01 7.1340e+00 6.39281e+00 0.0156 7.1340e+00 12\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 8.0180e+01 6.3132e-02 9.1545e-01 7.0963e+00 6.11464e+00 0.0156 7.0963e+00 9\n",
" 32 7.9688e+01 6.2512e-02 9.0106e-01 7.0615e+00 5.72084e+00 0.0156 7.0615e+00 25\n",
" 33 7.9477e+01 6.1945e-02 8.8690e-01 7.0546e+00 5.19832e+00 0.0078 7.0546e+00 9\n",
" 34 7.9178e+01 6.1694e-02 8.7995e-01 7.0317e+00 2.19153e+00 0.0312 7.0317e+00 9\n",
" 35 7.8112e+01 6.1251e-02 8.5242e-01 6.9527e+00 1.56191e+00 0.0625 6.9527e+00 10\n",
" 36 7.7120e+01 6.0829e-02 7.9917e-01 6.9068e+00 8.89700e-01 0.5000 6.9068e+00 10\n",
" 37 6.8495e+01 6.1923e-02 4.0163e-01 6.4417e+00 2.66980e+00 0.0312 6.4417e+00 9\n",
" 38 6.7646e+01 6.1495e-02 3.8904e-01 6.3694e+00 2.03655e+00 0.0625 6.3694e+00 9\n",
" 39 6.6884e+01 6.0997e-02 3.6466e-01 6.3176e+00 8.49487e-01 0.1250 6.3176e+00 10\n",
" 40 6.3032e+01 6.0915e-02 3.1908e-01 5.9780e+00 5.45795e-01 1.0000 5.9780e+00 10\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 2.9186e+01 6.4329e-02 2.0069e-02 2.8921e+00 6.75705e+00 0.0039 2.8921e+00 8\n",
" 42 2.9179e+01 6.4169e-02 2.0012e-02 2.8914e+00 2.89958e+00 0.0156 2.8914e+00 8\n",
" 43 2.9097e+01 6.3842e-02 1.9806e-02 2.8835e+00 2.67010e+00 0.0156 2.8835e+00 9\n",
" 44 2.9004e+01 6.3563e-02 1.9578e-02 2.8745e+00 2.39641e+00 0.0156 2.8745e+00 9\n",
" 45 2.8886e+01 6.3335e-02 1.9330e-02 2.8629e+00 2.08022e+00 0.0156 2.8629e+00 9\n",
" 46 2.8729e+01 6.3158e-02 1.9065e-02 2.8475e+00 1.73978e+00 0.0156 2.8475e+00 9\n",
" 47 2.8520e+01 6.3031e-02 1.8788e-02 2.8269e+00 1.39828e+00 0.0312 2.8269e+00 9\n",
" 48 2.8345e+01 6.2868e-02 1.8239e-02 2.8100e+00 7.62664e-01 0.0625 2.8100e+00 9\n",
" 49 2.7678e+01 6.2807e-02 1.7106e-02 2.7444e+00 3.54331e-01 0.5000 2.7444e+00 9\n",
" 50 2.3654e+01 6.3491e-02 8.7603e-03 2.3503e+00 1.81827e+00 0.0156 2.3503e+00 9\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 51 2.3567e+01 6.3324e-02 8.6515e-03 2.3418e+00 1.42749e+00 0.0156 2.3418e+00 9\n",
" 52 2.3424e+01 6.3211e-02 8.5286e-03 2.3276e+00 1.05954e+00 0.0312 2.3276e+00 9\n",
" 53 2.3323e+01 6.3081e-02 8.2767e-03 2.3177e+00 5.11176e-01 0.0625 2.3177e+00 9\n",
" 54 2.2603e+01 6.3068e-02 7.7604e-03 2.2462e+00 2.96353e-01 0.5000 2.2462e+00 9\n",
" 55 1.7597e+01 6.3696e-02 4.8992e-03 1.7484e+00 1.60621e+00 0.0156 1.7484e+00 8\n",
" 56 1.7587e+01 6.3559e-02 4.8467e-03 1.7475e+00 1.21859e+00 0.0156 1.7475e+00 9\n",
" 57 1.7510e+01 6.3475e-02 4.7816e-03 1.7398e+00 8.69785e-01 0.0312 1.7398e+00 9\n",
" 58 1.7465e+01 6.3390e-02 4.6467e-03 1.7356e+00 3.91667e-01 0.1250 1.7356e+00 9\n",
" 59 1.6949e+01 6.3431e-02 4.0593e-03 1.6845e+00 4.58440e-01 0.0312 1.6845e+00 9\n",
" 60 1.6717e+01 6.3406e-02 3.9324e-03 1.6614e+00 2.30973e-01 0.5000 1.6614e+00 9\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 61 1.3984e+01 6.3880e-02 2.2781e-03 1.3897e+00 1.58437e+00 0.0078 1.3897e+00 8\n",
" 62 1.3939e+01 6.3811e-02 2.2664e-03 1.3853e+00 1.58385e-01 0.5000 1.3853e+00 7\n",
" 63 1.2394e+01 6.3660e-02 1.2087e-03 1.2318e+00 1.06356e-01 1.0000 1.2318e+00 7\n",
" 64 2.8017e+00 6.4400e-02 8.8039e-04 2.7285e-01 8.40767e-01 0.0039 2.7285e-01 4\n",
" 65 2.7979e+00 6.4386e-02 8.7717e-04 2.7248e-01 1.00490e-01 0.2500 2.7248e-01 5\n",
" 66 2.7057e+00 6.4326e-02 6.6559e-04 2.6347e-01 3.74989e-02 1.0000 2.6347e-01 5\n",
" 67 9.0319e-01 6.4479e-02 4.1500e-05 8.3830e-02 3.90287e-01 0.0039 8.3830e-02 4\n",
" 68 9.0183e-01 6.4475e-02 4.1344e-05 8.3694e-02 5.02827e-02 0.2500 8.3694e-02 4\n",
" 69 7.9576e-01 6.4458e-02 3.3958e-05 7.3097e-02 1.60704e-02 1.0000 7.3097e-02 3\n",
" 70 3.7931e-01 6.4501e-02 2.1824e-05 3.1459e-02 1.31533e-01 0.0156 3.1459e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 71 3.7880e-01 6.4499e-02 2.1489e-05 3.1409e-02 8.89083e-02 0.0078 3.1409e-02 1\n",
" 72 3.7875e-01 6.4499e-02 2.1321e-05 3.1404e-02 1.11903e-02 0.5000 3.1404e-02 1\n",
" 73 3.4772e-01 6.4498e-02 1.0728e-05 2.8311e-02 8.21009e-03 1.0000 2.8311e-02 1\n",
" 74 3.2471e-01 6.4495e-02 3.4409e-06 2.6018e-02 8.09044e-02 0.0312 2.6018e-02 1\n",
" 75 3.1709e-01 6.4494e-02 3.3378e-06 2.5256e-02 2.93565e-02 0.0020 2.5256e-02 1\n",
" 76 3.1709e-01 6.4494e-02 3.3378e-06 2.5256e-02 4.98787e-03 1.0000 2.5256e-02 1\n",
" 77 2.0623e-01 6.4500e-02 5.3182e-07 1.4173e-02 5.04536e-02 0.1250 1.4173e-02 1\n",
" 78 1.9005e-01 6.4499e-02 4.5604e-07 1.2555e-02 8.04394e-02 0.0156 1.2555e-02 1\n",
" 79 1.8979e-01 6.4498e-02 4.5091e-07 1.2529e-02 5.30162e-02 0.0156 1.2529e-02 1\n",
" 80 1.8873e-01 6.4498e-02 4.4362e-07 1.2423e-02 3.59884e-02 0.0020 1.2423e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 81 1.8873e-01 6.4498e-02 4.4362e-07 1.2423e-02 5.72219e-03 0.5000 1.2423e-02 1\n",
" 82 1.8063e-01 6.4498e-02 2.3913e-07 1.1613e-02 4.58877e-03 0.5000 1.1613e-02 1\n",
" 83 1.6998e-01 6.4498e-02 2.0365e-07 1.0548e-02 3.29995e-03 1.0000 1.0548e-02 1\n",
" END 1.2728e-01 6.4502e-02 1.2086e-07 6.2776e-03 ---- ---- 6.2776e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.9813e+01 7.1145e-02 1.9657e+00 8.4677e-03 1.26760e+00 0.0312 ---- 2\n",
" 2 1.9606e+01 7.0883e-02 1.9043e+00 4.9233e-02 1.36817e+00 0.0156 5.4807e-01 3\n",
" 3 1.9546e+01 7.0754e-02 1.8745e+00 7.3044e-02 1.42246e+00 0.0078 5.3949e-01 3\n",
" 4 1.9513e+01 7.0684e-02 1.8599e+00 8.4399e-02 9.53118e-01 0.0312 5.3526e-01 3\n",
" 5 1.9263e+01 7.0462e-02 1.8018e+00 1.1753e-01 1.51182e+00 0.0078 5.1852e-01 4\n",
" 6 1.9260e+01 7.0394e-02 1.7877e+00 1.3132e-01 8.72439e-01 0.0312 5.1444e-01 4\n",
" 7 1.8994e+01 7.0195e-02 1.7318e+00 1.6058e-01 1.58460e+00 0.0039 4.9836e-01 4\n",
" 8 1.8965e+01 7.0161e-02 1.7251e+00 1.6447e-01 8.63511e-01 0.0312 4.9640e-01 4\n",
" 9 1.8875e+01 6.9975e-02 1.6712e+00 2.0939e-01 1.92510e+00 0.0020 4.8088e-01 5\n",
" 10 1.8857e+01 6.9954e-02 1.6679e+00 2.1083e-01 1.08378e+00 0.0078 4.7993e-01 5\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.8802e+01 6.9897e-02 1.6549e+00 2.1832e-01 3.20250e-01 1.0000 4.7619e-01 5\n",
" 12 8.6144e+00 6.7861e-02 1.0649e-02 8.4401e-01 5.55236e+00 0.0020 8.4401e-01 19\n",
" 13 8.6144e+00 6.7861e-02 1.0649e-02 8.4401e-01 1.93460e+00 0.0039 8.4401e-01 25\n",
" 14 8.6022e+00 6.7812e-02 1.0609e-02 8.4283e-01 2.54994e-01 0.1250 8.4283e-01 8\n",
" 15 8.1259e+00 6.7639e-02 9.3151e-03 7.9651e-01 1.23271e-01 0.5000 7.9651e-01 8\n",
" 16 7.1898e+00 6.7539e-02 4.6569e-03 7.0757e-01 8.04652e-02 0.5000 7.0757e-01 6\n",
" 17 5.5212e+00 6.7579e-02 2.3279e-03 5.4304e-01 6.48289e-02 1.0000 5.4304e-01 6\n",
" 18 4.3875e+00 6.7748e-02 1.1857e-04 4.3186e-01 1.25382e+00 0.0020 4.3186e-01 6\n",
" 19 4.3835e+00 6.7733e-02 1.1845e-04 4.3146e-01 1.52706e-01 0.1250 4.3146e-01 6\n",
" 20 4.1799e+00 6.7638e-02 1.1508e-04 4.1111e-01 7.40626e-02 0.2500 4.1111e-01 7\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.7174e+00 6.7614e-02 8.9126e-05 3.6489e-01 3.59349e-02 1.0000 3.6489e-01 7\n",
" 22 5.1597e-01 6.7885e-02 1.2149e-04 4.4687e-02 2.79420e-01 0.0039 4.4687e-02 4\n",
" 23 5.1564e-01 6.7883e-02 1.2104e-04 4.4655e-02 3.38652e-02 0.2500 4.4655e-02 4\n",
" 24 4.5569e-01 6.7873e-02 9.0843e-05 3.8691e-02 1.18097e-02 1.0000 3.8691e-02 4\n",
" END 1.4481e-01 6.7900e-02 1.2351e-05 7.6783e-03 ---- ---- 7.6783e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.9127e+01 7.5510e-02 1.8935e+00 1.1699e-02 1.30908e+00 0.0625 ---- 2\n",
" 2 1.8735e+01 7.4738e-02 1.7753e+00 9.0732e-02 3.23583e+00 0.0020 5.8841e-01 7\n",
" 3 1.8726e+01 7.4702e-02 1.7718e+00 9.3272e-02 2.47837e+00 0.0039 5.8722e-01 7\n",
" 4 1.8715e+01 7.4643e-02 1.7649e+00 9.9144e-02 1.42813e+00 0.0156 5.8485e-01 6\n",
" 5 1.8607e+01 7.4469e-02 1.7373e+00 1.1589e-01 1.39929e+00 0.0078 5.7552e-01 6\n",
" 6 1.8554e+01 7.4378e-02 1.7238e+00 1.2419e-01 8.36010e-01 0.1250 5.7092e-01 6\n",
" 7 1.8195e+01 7.3200e-02 1.5087e+00 3.0358e-01 4.40672e+00 0.0020 4.9843e-01 9\n",
" 8 1.8195e+01 7.3200e-02 1.5087e+00 3.0358e-01 3.22268e+00 0.0020 4.9843e-01 9\n",
" 9 1.8188e+01 7.3153e-02 1.5057e+00 3.0580e-01 8.64888e-01 0.0312 4.9742e-01 8\n",
" 10 1.8163e+01 7.2810e-02 1.4587e+00 3.5035e-01 3.93241e-01 0.2500 4.8149e-01 8\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.7096e+01 7.1250e-02 1.0948e+00 6.0761e-01 1.38023e+00 0.0078 6.0761e-01 16\n",
" 12 1.7042e+01 7.1148e-02 1.0863e+00 6.1082e-01 2.60881e-01 0.5000 6.1082e-01 14\n",
" 13 1.3293e+01 6.9321e-02 5.4453e-01 7.7785e-01 1.33205e-01 1.0000 7.7785e-01 19\n",
" 14 7.9333e+00 6.8655e-02 1.9317e-03 7.8454e-01 1.66550e+00 0.0039 7.8454e-01 25\n",
" 15 7.9265e+00 6.8609e-02 1.9251e-03 7.8386e-01 2.09822e-01 0.1250 7.8386e-01 7\n",
" 16 7.5087e+00 6.8470e-02 1.6857e-03 7.4234e-01 9.47226e-02 0.5000 7.4234e-01 8\n",
" 17 6.7086e+00 6.8489e-02 8.5909e-04 6.6315e-01 8.47709e-02 1.0000 6.6315e-01 8\n",
" 18 6.6057e+00 6.8718e-02 3.0430e-04 6.5339e-01 1.59711e+00 0.0039 6.5339e-01 8\n",
" 19 6.6052e+00 6.8673e-02 3.0348e-04 6.5335e-01 1.99812e-01 0.1250 6.5335e-01 8\n",
" 20 6.3266e+00 6.8532e-02 2.8698e-04 6.2552e-01 8.20686e-02 0.5000 6.2552e-01 7\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 5.9856e+00 6.8528e-02 1.5996e-04 5.9154e-01 7.43385e-02 0.5000 5.9154e-01 7\n",
" 22 4.6928e+00 6.8611e-02 1.2421e-04 4.6230e-01 6.25866e-02 1.0000 4.6230e-01 7\n",
" 23 3.7366e+00 6.8825e-02 1.2740e-04 3.6665e-01 1.10328e+00 0.0039 3.6665e-01 7\n",
" 24 3.7360e+00 6.8798e-02 1.2735e-04 3.6659e-01 1.31827e-01 0.1250 3.6659e-01 7\n",
" 25 3.5368e+00 6.8726e-02 1.1632e-04 3.4669e-01 6.93678e-02 0.2500 3.4669e-01 6\n",
" 26 3.1360e+00 6.8708e-02 9.1347e-05 3.0664e-01 3.23578e-02 1.0000 3.0664e-01 4\n",
" 27 4.7635e-01 6.8946e-02 6.7286e-05 4.0674e-02 2.64790e-01 0.0020 4.0674e-02 3\n",
" 28 4.7628e-01 6.8944e-02 6.7155e-05 4.0666e-02 3.80543e-02 0.0625 4.0666e-02 3\n",
" 29 4.7325e-01 6.8939e-02 6.2931e-05 4.0368e-02 2.99801e-02 0.1250 4.0368e-02 3\n",
" 30 4.6037e-01 6.8935e-02 5.4935e-05 3.9089e-02 1.93306e-02 0.5000 3.9089e-02 3\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 4.2328e-01 6.8933e-02 2.6670e-05 3.5408e-02 1.59211e-02 1.0000 3.5408e-02 3\n",
" 32 4.1967e-01 6.8934e-02 3.8216e-06 3.5069e-02 2.22682e-01 0.0020 3.5069e-02 3\n",
" 33 4.1957e-01 6.8933e-02 3.8151e-06 3.5060e-02 3.12787e-02 0.0625 3.5060e-02 3\n",
" 34 4.1550e-01 6.8930e-02 3.6173e-06 3.4653e-02 2.44789e-02 0.2500 3.4653e-02 3\n",
" 35 4.1266e-01 6.8924e-02 3.4011e-06 3.4370e-02 7.69027e-03 1.0000 3.4370e-02 3\n",
" 36 2.2758e-01 6.8937e-02 4.0275e-06 1.5860e-02 5.49867e-02 0.0625 1.5860e-02 1\n",
" 37 2.2567e-01 6.8937e-02 3.8031e-06 1.5669e-02 4.44054e-02 0.1250 1.5669e-02 1\n",
" 38 2.2443e-01 6.8936e-02 3.8695e-06 1.5545e-02 6.30722e-02 0.0156 1.5545e-02 1\n",
" 39 2.2399e-01 6.8936e-02 3.8077e-06 1.5502e-02 4.68177e-02 0.0625 1.5502e-02 1\n",
" 40 2.2382e-01 6.8935e-02 3.5824e-06 1.5485e-02 4.08582e-02 0.0625 1.5485e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 41 2.2165e-01 6.8935e-02 3.4847e-06 1.5268e-02 3.76971e-02 0.1250 1.5268e-02 1\n",
" 42 2.2072e-01 6.8935e-02 3.1718e-06 1.5175e-02 5.66176e-02 0.0156 1.5175e-02 1\n",
" 43 2.2030e-01 6.8934e-02 3.1323e-06 1.5134e-02 4.04000e-02 0.0625 1.5134e-02 1\n",
" 44 2.2029e-01 6.8934e-02 3.0569e-06 1.5133e-02 3.68730e-02 0.0625 1.5133e-02 1\n",
" 45 2.1797e-01 6.8934e-02 2.8867e-06 1.4901e-02 3.25792e-02 0.1250 1.4901e-02 1\n",
" 46 2.1690e-01 6.8934e-02 2.9054e-06 1.4793e-02 5.13720e-02 0.0156 1.4793e-02 1\n",
" 47 2.1651e-01 6.8933e-02 2.8605e-06 1.4755e-02 3.69680e-02 0.0625 1.4755e-02 1\n",
" 48 2.1648e-01 6.8933e-02 2.6944e-06 1.4752e-02 3.24864e-02 0.0625 1.4752e-02 1\n",
" 49 2.1432e-01 6.8933e-02 2.6166e-06 1.4536e-02 2.99916e-02 0.1250 1.4536e-02 1\n",
" 50 2.1405e-01 6.8933e-02 2.3635e-06 1.4510e-02 4.89646e-02 0.0156 1.4510e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.9438e+01 7.6427e-02 1.9354e+00 8.0431e-04 1.17962e+00 0.0625 ---- 2\n",
" 2 1.8738e+01 7.5660e-02 1.8145e+00 5.1691e-02 1.48945e+00 0.0312 5.9293e-01 6\n",
" 3 1.8550e+01 7.5317e-02 1.7578e+00 8.9624e-02 2.16866e+00 0.0039 5.7406e-01 6\n",
" 4 1.8528e+01 7.5262e-02 1.7510e+00 9.4325e-02 1.42736e+00 0.0156 5.7175e-01 6\n",
" 5 1.8414e+01 7.5097e-02 1.7236e+00 1.1028e-01 1.43412e+00 0.0078 5.6264e-01 6\n",
" 6 1.8358e+01 7.5012e-02 1.7102e+00 1.1812e-01 8.04615e-01 0.1250 5.5815e-01 6\n",
" 7 1.8157e+01 7.3904e-02 1.4967e+00 3.1160e-01 5.93111e+00 0.0020 4.8735e-01 10\n",
" 8 1.8157e+01 7.3904e-02 1.4967e+00 3.1160e-01 5.72053e+00 0.0020 4.8735e-01 10\n",
" 9 1.8157e+01 7.3904e-02 1.4967e+00 3.1160e-01 4.26726e+00 0.0020 4.8736e-01 9\n",
" 10 1.8157e+01 7.3904e-02 1.4967e+00 3.1160e-01 1.39142e+00 0.0156 4.8738e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.8156e+01 7.3678e-02 1.4733e+00 3.3493e-01 8.42566e-01 0.0312 4.7949e-01 25\n",
" 12 1.8071e+01 7.3382e-02 1.4273e+00 3.7245e-01 3.14733e-01 1.0000 4.6420e-01 25\n",
" 13 1.1485e+01 6.9734e-02 1.0937e-02 1.1306e+00 6.10982e+00 0.0020 1.1306e+00 25\n",
" 14 1.1485e+01 6.9734e-02 1.0937e-02 1.1306e+00 2.41223e+00 0.0039 1.1306e+00 10\n",
" 15 1.1465e+01 6.9660e-02 1.0897e-02 1.1286e+00 3.61858e-01 0.1250 1.1286e+00 8\n",
" 16 1.0781e+01 6.9355e-02 9.6043e-03 1.0616e+00 2.27034e-01 0.2500 1.0616e+00 9\n",
" 17 9.9840e+00 6.9088e-02 7.2652e-03 9.8423e-01 1.02576e-01 1.0000 9.8423e-01 7\n",
" 18 5.6277e+00 6.9517e-02 1.7996e-04 5.5564e-01 1.31161e+00 0.0039 5.5564e-01 7\n",
" 19 5.6247e+00 6.9486e-02 1.7985e-04 5.5534e-01 1.48611e-01 0.2500 5.5534e-01 7\n",
" 20 5.5399e+00 6.9348e-02 1.6545e-04 5.4689e-01 5.60942e-02 1.0000 5.4689e-01 7\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 7.7666e-01 6.9732e-02 1.4763e-04 7.0545e-02 2.99543e-01 0.0078 7.0545e-02 2\n",
" 22 7.7321e-01 6.9728e-02 1.4649e-04 7.0202e-02 3.38181e-02 0.2500 7.0202e-02 3\n",
" 23 7.6934e-01 6.9721e-02 1.0977e-04 6.9853e-02 1.77848e-02 1.0000 6.9853e-02 2\n",
" 24 2.0301e-01 6.9789e-02 2.2427e-05 1.3300e-02 1.21045e-01 0.1250 1.3300e-02 1\n",
" 25 1.9813e-01 6.9777e-02 2.0037e-05 1.2815e-02 2.03143e-01 0.0312 1.2815e-02 1\n",
" 26 1.9675e-01 6.9766e-02 1.9647e-05 1.2679e-02 5.95219e-02 0.0020 1.2679e-02 1\n",
" 27 1.9675e-01 6.9766e-02 1.9647e-05 1.2679e-02 9.62172e-03 1.0000 1.2679e-02 1\n",
" END 8.1016e-02 6.9771e-02 4.5772e-06 1.1200e-03 ---- ---- 2.8884e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.9216e+01 7.7433e-02 1.9127e+00 1.1200e-03 1.35201e+00 0.0625 ---- 2\n",
" 2 1.8914e+01 7.6641e-02 1.7933e+00 9.0448e-02 3.51425e+00 0.0020 6.0498e-01 7\n",
" 3 1.8911e+01 7.6600e-02 1.7898e+00 9.3660e-02 2.67609e+00 0.0020 6.0375e-01 8\n",
" 4 1.8894e+01 7.6568e-02 1.7863e+00 9.5486e-02 2.06879e+00 0.0039 6.0253e-01 8\n",
" 5 1.8863e+01 7.6514e-02 1.7793e+00 9.9337e-02 1.24697e+00 0.0312 6.0012e-01 8\n",
" 6 1.8671e+01 7.6175e-02 1.7237e+00 1.3577e-01 2.53808e+00 0.0020 5.8103e-01 9\n",
" 7 1.8653e+01 7.6145e-02 1.7203e+00 1.3733e-01 1.40642e+00 0.0078 5.7986e-01 9\n",
" 8 1.8591e+01 7.6053e-02 1.7069e+00 1.4462e-01 3.72876e-01 1.0000 5.7524e-01 9\n",
" 9 5.0137e+00 7.0621e-02 1.4713e-02 4.7960e-01 5.44380e+00 0.0020 4.7960e-01 25\n",
" 10 5.0137e+00 7.0621e-02 1.4713e-02 4.7960e-01 1.64922e+00 0.0039 4.7960e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 5.0096e+00 7.0590e-02 1.4656e-02 4.7925e-01 1.97380e-01 0.2500 4.7925e-01 25\n",
" 12 4.7738e+00 7.0415e-02 1.1002e-02 4.5934e-01 8.08183e-02 1.0000 4.5934e-01 25\n",
" 13 2.4517e+00 7.0517e-02 3.2028e-04 2.3780e-01 7.08280e-01 0.0039 2.3780e-01 25\n",
" 14 2.4483e+00 7.0508e-02 3.1901e-04 2.3746e-01 8.62704e-02 0.2500 2.3746e-01 25\n",
" 15 2.1545e+00 7.0487e-02 2.3916e-04 2.0816e-01 3.99988e-02 1.0000 2.0816e-01 25\n",
" 16 3.6500e-01 7.0612e-02 4.6324e-05 2.9393e-02 2.45409e-01 0.0078 2.9393e-02 25\n",
" 17 3.6472e-01 7.0610e-02 4.5965e-05 2.9365e-02 3.09869e-02 0.5000 2.9365e-02 25\n",
" 18 3.0260e-01 7.0607e-02 2.4228e-05 2.3175e-02 1.83377e-02 1.0000 2.3175e-02 25\n",
" END 1.5134e-01 7.0611e-02 9.5403e-06 8.0631e-03 ---- ---- 8.0631e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.8709e+01 7.8586e-02 1.8581e+00 4.8696e-03 1.82484e+00 0.0156 ---- 25\n",
" 2 1.8601e+01 7.8364e-02 1.8291e+00 2.3188e-02 2.06263e+00 0.0039 6.5132e-01 25\n",
" 3 1.8570e+01 7.8304e-02 1.8220e+00 2.7148e-02 1.35191e+00 0.0312 6.4870e-01 25\n",
" 4 1.8377e+01 7.7908e-02 1.7651e+00 6.4882e-02 2.67569e+00 0.0020 6.2800e-01 25\n",
" 5 1.8364e+01 7.7873e-02 1.7616e+00 6.7014e-02 1.96948e+00 0.0039 6.2673e-01 25\n",
" 6 1.8340e+01 7.7817e-02 1.7547e+00 7.1480e-02 1.12387e+00 0.0312 6.2422e-01 25\n",
" 7 1.8163e+01 7.7457e-02 1.6999e+00 1.0863e-01 2.62487e+00 0.0020 6.0434e-01 25\n",
" 8 1.8148e+01 7.7425e-02 1.6966e+00 1.1050e-01 1.45288e+00 0.0078 6.0312e-01 25\n",
" 9 1.8102e+01 7.7326e-02 1.6833e+00 1.1912e-01 3.72678e-01 1.0000 5.9831e-01 25\n",
" 10 3.5338e+00 7.1574e-02 1.5051e-02 3.3117e-01 5.26080e+00 0.0020 3.3117e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.5338e+00 7.1574e-02 1.5051e-02 3.3117e-01 1.58829e+00 0.0020 3.3117e-01 25\n",
" 12 3.5313e+00 7.1559e-02 1.5022e-02 3.3095e-01 2.08815e-01 0.1250 3.3095e-01 25\n",
" 13 3.3919e+00 7.1463e-02 1.3148e-02 3.1889e-01 1.29838e-01 0.2500 3.1889e-01 25\n",
" 14 3.0217e+00 7.1413e-02 9.8469e-03 2.8518e-01 5.58615e-02 1.0000 2.8518e-01 25\n",
" 15 6.3508e-01 7.1542e-02 2.8490e-04 5.6069e-02 3.72677e-01 0.0039 5.6069e-02 25\n",
" 16 6.3416e-01 7.1540e-02 2.8379e-04 5.5978e-02 5.18784e-02 0.5000 5.5978e-02 25\n",
" 17 6.0647e-01 7.1526e-02 1.4854e-04 5.3346e-02 3.45336e-02 1.0000 5.3346e-02 25\n",
" 18 5.8207e-01 7.1527e-02 2.7117e-05 5.1028e-02 4.55017e-01 0.0020 5.1028e-02 25\n",
" 19 5.8190e-01 7.1525e-02 2.7067e-05 5.1011e-02 6.36898e-02 0.1250 5.1011e-02 25\n",
" 20 5.6572e-01 7.1511e-02 2.4926e-05 4.9396e-02 3.82249e-02 0.5000 4.9396e-02 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 5.3801e-01 7.1506e-02 2.0368e-05 4.6630e-02 2.93405e-02 0.5000 4.6630e-02 25\n",
" 22 4.4667e-01 7.1508e-02 1.2557e-05 3.7504e-02 2.35033e-02 0.5000 3.7504e-02 25\n",
" 23 3.5770e-01 7.1510e-02 1.0687e-05 2.8608e-02 1.99279e-02 0.5000 2.8608e-02 25\n",
" 24 2.8798e-01 7.1511e-02 6.1950e-06 2.1640e-02 1.69078e-02 0.5000 2.1640e-02 25\n",
" 25 2.3684e-01 7.1510e-02 6.0151e-06 1.6527e-02 1.51136e-02 0.5000 1.6527e-02 25\n",
" 26 2.0075e-01 7.1509e-02 3.3493e-06 1.2920e-02 1.33407e-02 0.5000 1.2920e-02 25\n",
" 27 1.7551e-01 7.1507e-02 3.4329e-06 1.0397e-02 1.26129e-02 0.5000 1.0397e-02 25\n",
" END 1.5797e-01 7.1504e-02 1.9460e-06 8.6442e-03 ---- ---- 8.6442e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.8894e+01 7.9760e-02 1.8809e+00 5.0928e-04 1.20307e+00 0.1250 ---- 25\n",
" 2 1.7702e+01 7.8160e-02 1.6462e+00 1.1620e-01 4.00244e+00 0.0020 5.9001e-01 25\n",
" 3 1.7695e+01 7.8124e-02 1.6429e+00 1.1874e-01 3.15897e+00 0.0039 5.8882e-01 25\n",
" 4 1.7688e+01 7.8065e-02 1.6365e+00 1.2449e-01 1.92941e+00 0.0156 5.8645e-01 25\n",
" 5 1.7622e+01 7.7895e-02 1.6110e+00 1.4345e-01 1.41868e+00 0.0078 5.7712e-01 25\n",
" 6 1.7566e+01 7.7812e-02 1.5984e+00 1.5045e-01 8.19992e-01 0.0625 5.7252e-01 25\n",
" 7 1.7282e+01 7.7246e-02 1.4985e+00 2.2197e-01 3.30825e+00 0.0020 5.3616e-01 25\n",
" 8 1.7282e+01 7.7246e-02 1.4985e+00 2.2197e-01 2.39594e+00 0.0020 5.3617e-01 25\n",
" 9 1.7271e+01 7.7209e-02 1.4956e+00 2.2380e-01 6.59739e-01 0.0312 5.3510e-01 25\n",
" 10 1.7047e+01 7.6904e-02 1.4489e+00 2.4815e-01 3.06239e-01 1.0000 5.1806e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 6.5889e+00 7.2534e-02 1.1778e-02 6.3986e-01 6.08331e+00 0.0020 6.3986e-01 25\n",
" 12 6.5889e+00 7.2534e-02 1.1778e-02 6.3986e-01 2.29501e+00 0.0020 6.3986e-01 25\n",
" 13 6.5843e+00 7.2499e-02 1.1755e-02 6.3942e-01 3.36317e-01 0.0625 6.3942e-01 25\n",
" 14 6.4154e+00 7.2336e-02 1.1027e-02 6.2328e-01 2.38101e-01 0.1250 6.2328e-01 23\n",
" 15 6.2284e+00 7.2153e-02 9.6560e-03 6.0596e-01 1.09748e-01 0.5000 6.0596e-01 14\n",
" 16 5.9831e+00 7.2089e-02 4.8048e-03 5.8629e-01 9.20999e-02 0.5000 5.8629e-01 13\n",
" 17 4.7953e+00 7.2140e-02 2.3731e-03 4.6994e-01 6.69009e-02 0.5000 4.6994e-01 12\n",
" 18 3.6198e+00 7.2212e-02 1.2053e-03 3.5355e-01 6.13280e-02 0.5000 3.5355e-01 13\n",
" 19 2.7555e+00 7.2263e-02 6.0134e-04 2.6772e-01 4.98942e-02 0.5000 2.6772e-01 13\n",
" 20 2.1668e+00 7.2295e-02 3.1154e-04 2.0914e-01 4.81264e-02 0.5000 2.0914e-01 13\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.7862e+00 7.2311e-02 1.6051e-04 1.7123e-01 4.21188e-02 0.5000 1.7123e-01 13\n",
" 22 1.5486e+00 7.2318e-02 8.5942e-05 1.4754e-01 4.12742e-02 0.5000 1.4754e-01 13\n",
" 23 1.4084e+00 7.2317e-02 4.9349e-05 1.3356e-01 3.90268e-02 0.5000 1.3356e-01 13\n",
" 24 1.3292e+00 7.2312e-02 2.9251e-05 1.2566e-01 3.74227e-02 0.5000 1.2566e-01 9\n",
" 25 1.2970e+00 7.2306e-02 1.9863e-05 1.2245e-01 3.54697e-02 0.2500 1.2245e-01 5\n",
" 26 1.1873e+00 7.2302e-02 1.5807e-05 1.1148e-01 1.07267e-02 1.0000 1.1148e-01 5\n",
" 27 2.6764e-01 7.2382e-02 1.3402e-05 1.9512e-02 5.37020e-02 0.0625 1.9512e-02 1\n",
" 28 2.6390e-01 7.2382e-02 1.2481e-05 1.9139e-02 4.23109e-02 0.1250 1.9139e-02 1\n",
" 29 2.6237e-01 7.2381e-02 1.0789e-05 1.8988e-02 7.12877e-02 0.0078 1.8988e-02 1\n",
" 30 2.6198e-01 7.2380e-02 1.0704e-05 1.8950e-02 9.84700e-03 1.0000 1.8950e-02 1\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.4121e-01 7.2379e-02 5.4065e-06 1.6878e-02 1.36805e-01 0.0039 1.6878e-02 1\n",
" 32 2.4105e-01 7.2378e-02 5.3868e-06 1.6861e-02 1.79634e-02 0.0020 1.6861e-02 1\n",
" 33 2.4105e-01 7.2378e-02 5.3868e-06 1.6861e-02 2.31180e-03 1.0000 1.6861e-02 1\n",
" END 1.3779e-01 7.2383e-02 3.4837e-07 6.5406e-03 ---- ---- 6.5406e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.9358e+01 8.0827e-02 1.9264e+00 1.3220e-03 1.25378e+00 0.0625 ---- 3\n",
" 2 1.8796e+01 7.9981e-02 1.8061e+00 6.5575e-02 2.25270e+00 0.0078 6.4665e-01 7\n",
" 3 1.8779e+01 7.9861e-02 1.7920e+00 7.7999e-02 1.21341e+00 0.0312 6.4147e-01 7\n",
" 4 1.8748e+01 7.9493e-02 1.7360e+00 1.3088e-01 2.71796e+00 0.0020 6.2104e-01 7\n",
" 5 1.8744e+01 7.9453e-02 1.7326e+00 1.3390e-01 1.86101e+00 0.0039 6.1978e-01 7\n",
" 6 1.8743e+01 7.9392e-02 1.7258e+00 1.4059e-01 1.00892e+00 0.0312 6.1729e-01 25\n",
" 7 1.8714e+01 7.9054e-02 1.6719e+00 1.9162e-01 2.77597e+00 0.0020 5.9765e-01 25\n",
" 8 1.8708e+01 7.9016e-02 1.6686e+00 1.9431e-01 1.48444e+00 0.0039 5.9643e-01 25\n",
" 9 1.8673e+01 7.8963e-02 1.6621e+00 1.9727e-01 4.20353e-01 0.2500 5.9406e-01 25\n",
" 10 1.7416e+01 7.6806e-02 1.2471e+00 4.8676e-01 1.49024e+00 0.0078 4.8676e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.7356e+01 7.6686e-02 1.2374e+00 4.9051e-01 2.59781e-01 1.0000 4.9051e-01 25\n",
" 12 1.6812e+01 7.2773e-02 1.1627e-02 1.6623e+00 2.43169e+00 0.0078 1.6623e+00 25\n",
" 13 1.6808e+01 7.2618e-02 1.1549e-02 1.6620e+00 3.23950e-01 0.2500 1.6620e+00 10\n",
" 14 1.6460e+01 7.2150e-02 8.6568e-03 1.6301e+00 1.60469e-01 1.0000 1.6301e+00 9\n",
" 15 2.7036e+00 7.3362e-02 1.4471e-03 2.6158e-01 9.65813e-01 0.0039 2.6158e-01 11\n",
" 16 2.7027e+00 7.3347e-02 1.4416e-03 2.6150e-01 1.26257e-01 0.2500 2.6150e-01 7\n",
" 17 2.5556e+00 7.3280e-02 1.0847e-03 2.4715e-01 5.22590e-02 1.0000 2.4715e-01 6\n",
" 18 4.7316e-01 7.3399e-02 9.4187e-05 3.9881e-02 3.42122e-01 0.0078 3.9881e-02 5\n",
" 19 4.7291e-01 7.3396e-02 9.3455e-05 3.9858e-02 4.47886e-02 0.5000 3.9858e-02 5\n",
" 20 4.2088e-01 7.3378e-02 3.9570e-05 3.4711e-02 2.73900e-02 1.0000 3.4711e-02 4\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 3.5654e-01 7.3375e-02 1.4981e-05 2.8301e-02 3.48044e-01 0.0078 2.8301e-02 1\n",
" 22 3.5224e-01 7.3370e-02 1.4914e-05 2.7872e-02 4.42299e-02 0.0020 2.7872e-02 1\n",
" 23 3.5224e-01 7.3370e-02 1.4914e-05 2.7872e-02 5.05811e-03 1.0000 2.7872e-02 1\n",
" 24 2.5167e-01 7.3381e-02 2.2788e-06 1.7826e-02 2.83883e-02 0.5000 1.7826e-02 1\n",
" 25 1.9603e-01 7.3387e-02 1.1969e-05 1.2253e-02 2.33005e-02 0.5000 1.2253e-02 1\n",
" 26 1.8714e-01 7.3387e-02 6.2074e-06 1.1369e-02 2.09000e-02 0.0020 1.1369e-02 1\n",
" 27 1.8714e-01 7.3387e-02 6.2074e-06 1.1369e-02 2.72813e-03 1.0000 1.1369e-02 1\n",
" END 1.4148e-01 7.3379e-02 4.4796e-07 6.8100e-03 ---- ---- 6.8100e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.8746e+01 8.2221e-02 1.8627e+00 3.6922e-03 1.39692e+00 0.0312 ---- 3\n",
" 2 1.8454e+01 8.1758e-02 1.8045e+00 3.2661e-02 1.74415e+00 0.0156 6.9139e-01 5\n",
" 3 1.8434e+01 8.1521e-02 1.7763e+00 5.8921e-02 2.39452e+00 0.0020 6.8033e-01 5\n",
" 4 1.8417e+01 8.1485e-02 1.7729e+00 6.0683e-02 1.80575e+00 0.0039 6.7895e-01 5\n",
" 5 1.8383e+01 8.1425e-02 1.7659e+00 6.4266e-02 1.18036e+00 0.0312 6.7623e-01 5\n",
" 6 1.8156e+01 8.1022e-02 1.7108e+00 9.6696e-02 2.32582e+00 0.0020 6.5465e-01 6\n",
" 7 1.8140e+01 8.0989e-02 1.7074e+00 9.8489e-02 1.62907e+00 0.0078 6.5333e-01 6\n",
" 8 1.8137e+01 8.0880e-02 1.6941e+00 1.1151e-01 7.99018e-01 0.0625 6.4812e-01 6\n",
" 9 1.7636e+01 8.0182e-02 1.5883e+00 1.6730e-01 2.23189e+00 0.0020 6.0689e-01 25\n",
" 10 1.7621e+01 8.0146e-02 1.5852e+00 1.6890e-01 6.01899e-01 0.0625 6.0568e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.7450e+01 7.9477e-02 1.4862e+00 2.5088e-01 5.81337e-01 0.0625 5.6718e-01 25\n",
" 12 1.7125e+01 7.8894e-02 1.3933e+00 3.1126e-01 5.01223e-01 0.0625 5.3118e-01 25\n",
" 13 1.6695e+01 7.8385e-02 1.3063e+00 3.5539e-01 4.76444e-01 0.0625 4.9751e-01 25\n",
" 14 1.6187e+01 7.7939e-02 1.2247e+00 3.8620e-01 4.16851e-01 0.1250 4.6601e-01 25\n",
" 15 1.5971e+01 7.7178e-02 1.0717e+00 5.1761e-01 9.55294e-01 0.0156 5.1761e-01 25\n",
" 16 1.5919e+01 7.7029e-02 1.0550e+00 5.2924e-01 5.44488e-01 0.0312 5.2924e-01 25\n",
" 17 1.5690e+01 7.6848e-02 1.0220e+00 5.3928e-01 2.25174e-01 1.0000 5.3928e-01 25\n",
" 18 1.4347e+00 7.4545e-02 4.9606e-03 1.3106e-01 3.96237e+00 0.0020 1.3106e-01 25\n",
" 19 1.4347e+00 7.4545e-02 4.9606e-03 1.3106e-01 1.02347e+00 0.0020 1.3106e-01 25\n",
" 20 1.4347e+00 7.4536e-02 4.9510e-03 1.3107e-01 1.29740e-01 0.1250 1.3107e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.4144e+00 7.4482e-02 4.3336e-03 1.2966e-01 8.18697e-02 0.2500 1.2966e-01 25\n",
" 22 1.2831e+00 7.4455e-02 3.2490e-03 1.1761e-01 3.37495e-02 1.0000 1.1761e-01 24\n",
" 23 2.0392e-01 7.4503e-02 5.0191e-05 1.2892e-02 1.99811e-01 0.0039 1.2892e-02 25\n",
" 24 2.0368e-01 7.4502e-02 4.9995e-05 1.2868e-02 3.24873e-02 0.5000 1.2868e-02 24\n",
" 25 1.9188e-01 7.4487e-02 2.8230e-05 1.1711e-02 1.91032e-02 1.0000 1.1711e-02 24\n",
" END 1.7283e-01 7.4463e-02 8.8407e-06 9.8281e-03 ---- ---- 9.8281e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.9300e+01 8.3547e-02 1.9162e+00 5.4558e-03 1.87064e+00 0.0156 ---- 25\n",
" 2 1.9216e+01 8.3295e-02 1.8863e+00 2.6976e-02 2.28386e+00 0.0039 7.1742e-01 25\n",
" 3 1.9180e+01 8.3223e-02 1.8789e+00 3.0776e-02 1.40830e+00 0.0312 7.1453e-01 25\n",
" 4 1.8931e+01 8.2779e-02 1.8202e+00 6.4592e-02 3.14872e+00 0.0020 6.9175e-01 25\n",
" 5 1.8921e+01 8.2738e-02 1.8166e+00 6.7235e-02 2.30391e+00 0.0039 6.9034e-01 25\n",
" 6 1.8906e+01 8.2671e-02 1.8095e+00 7.2849e-02 1.14941e+00 0.0312 6.8756e-01 25\n",
" 7 1.8745e+01 8.2268e-02 1.7530e+00 1.1325e-01 2.80158e+00 0.0020 6.6563e-01 25\n",
" 8 1.8737e+01 8.2228e-02 1.7496e+00 1.1590e-01 1.47411e+00 0.0078 6.6429e-01 25\n",
" 9 1.8709e+01 8.2116e-02 1.7359e+00 1.2676e-01 3.44952e-01 1.0000 6.5900e-01 25\n",
" 10 2.1311e+00 7.5733e-02 1.4474e-02 1.9107e-01 4.00212e+00 0.0020 1.9107e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.1311e+00 7.5733e-02 1.4474e-02 1.9107e-01 1.08291e+00 0.0039 1.9107e-01 25\n",
" 12 2.1305e+00 7.5720e-02 1.4417e-02 1.9106e-01 1.28914e-01 0.2500 1.9106e-01 25\n",
" 13 1.9902e+00 7.5655e-02 1.0799e-02 1.8066e-01 5.41932e-02 1.0000 1.8066e-01 25\n",
" 14 5.8389e-01 7.5684e-02 4.4624e-04 5.0374e-02 3.01997e-01 0.0156 5.0374e-02 25\n",
" 15 5.8384e-01 7.5680e-02 4.3929e-04 5.0377e-02 2.21487e-01 0.0312 5.0377e-02 25\n",
" 16 5.8049e-01 7.5677e-02 4.2569e-04 5.0056e-02 1.23304e-01 0.1250 5.0056e-02 25\n",
" 17 5.4983e-01 7.5673e-02 3.7507e-04 4.7040e-02 1.04298e-01 0.1250 4.7040e-02 25\n",
" 18 5.2259e-01 7.5670e-02 3.3159e-04 4.4361e-02 1.28725e-01 0.0625 4.4361e-02 25\n",
" 19 5.1858e-01 7.5667e-02 3.1125e-04 4.3980e-02 8.28471e-02 0.1250 4.3980e-02 25\n",
" 20 5.0072e-01 7.5665e-02 2.7413e-04 4.2231e-02 1.38439e-01 0.0156 4.2231e-02 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 4.9777e-01 7.5664e-02 2.6987e-04 4.1941e-02 8.99612e-02 0.0625 4.1941e-02 25\n",
" 22 4.8929e-01 7.5662e-02 2.5332e-04 4.1110e-02 7.85206e-02 0.1250 4.1110e-02 25\n",
" 23 4.8336e-01 7.5660e-02 2.2271e-04 4.0547e-02 1.55171e-01 0.0156 4.0547e-02 25\n",
" 24 4.8261e-01 7.5658e-02 2.1925e-04 4.0476e-02 9.29467e-02 0.0625 4.0476e-02 25\n",
" 25 4.8100e-01 7.5657e-02 2.0578e-04 4.0329e-02 8.73106e-02 0.0625 4.0329e-02 25\n",
" 26 4.7654e-01 7.5655e-02 1.9308e-04 3.9895e-02 8.21669e-02 0.0625 3.9895e-02 25\n",
" 27 4.7374e-01 7.5654e-02 1.8122e-04 3.9627e-02 8.45630e-02 0.0312 3.9627e-02 25\n",
" 28 4.6797e-01 7.5653e-02 1.7558e-04 3.9056e-02 3.14113e-02 1.0000 3.9056e-02 25\n",
" 29 1.2935e-01 7.5661e-02 5.2218e-05 5.3170e-03 7.10915e-01 0.0020 1.5949e-02 25\n",
" 30 1.2935e-01 7.5661e-02 5.2218e-05 5.3170e-03 1.96599e-01 0.0020 1.3380e-02 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 1.2935e-01 7.5661e-02 5.2218e-05 5.3170e-03 2.69010e-02 0.0625 1.0269e-02 25\n",
" 32 1.2933e-01 7.5659e-02 4.8937e-05 5.3182e-03 2.06573e-02 0.0625 1.0193e-02 25\n",
" 33 1.2778e-01 7.5657e-02 4.5869e-05 5.1664e-03 1.61544e-02 0.1250 1.0119e-02 25\n",
" END 1.2519e-01 7.5655e-02 4.0114e-05 4.9129e-03 ---- ---- 9.9757e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.0074e+01 8.5114e-02 1.9978e+00 1.1112e-03 1.30973e+00 0.1250 ---- 25\n",
" 2 1.9484e+01 8.3303e-02 1.7483e+00 1.9173e-01 6.31150e+00 0.0020 6.5453e-01 25\n",
" 3 1.9484e+01 8.3303e-02 1.7483e+00 1.9173e-01 6.30912e+00 0.0020 6.5453e-01 25\n",
" 4 1.9484e+01 8.3303e-02 1.7483e+00 1.9173e-01 6.28546e+00 0.0020 6.5453e-01 25\n",
" 5 1.9484e+01 8.3303e-02 1.7483e+00 1.9173e-01 6.05892e+00 0.0020 6.5453e-01 25\n",
" 6 1.9484e+01 8.3303e-02 1.7483e+00 1.9173e-01 4.49539e+00 0.0020 6.5453e-01 25\n",
" 7 1.9481e+01 8.3246e-02 1.7449e+00 1.9486e-01 1.20250e+00 0.0312 6.5320e-01 25\n",
" 8 1.9462e+01 8.2797e-02 1.6904e+00 2.4755e-01 4.97415e-01 0.5000 6.3229e-01 25\n",
" 9 1.7472e+01 7.8787e-02 8.4779e-01 8.9157e-01 1.65370e+00 0.0156 8.9157e-01 25\n",
" 10 1.7464e+01 7.8618e-02 8.3455e-01 9.0397e-01 1.15084e+00 0.0312 9.0397e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.7444e+01 7.8417e-02 8.0848e-01 9.2811e-01 5.47938e-01 0.1250 9.2811e-01 25\n",
" 12 1.6976e+01 7.8029e-02 7.0749e-01 9.8232e-01 4.71999e-01 0.1250 9.8232e-01 25\n",
" 13 1.6160e+01 7.7727e-02 6.1909e-01 9.8915e-01 6.77618e-01 0.0312 9.8915e-01 25\n",
" 14 1.5961e+01 7.7625e-02 5.9975e-01 9.8864e-01 2.39105e-01 0.5000 9.8864e-01 25\n",
" 15 1.2073e+01 7.7058e-02 3.0014e-01 8.9946e-01 1.65001e+00 0.0078 8.9946e-01 25\n",
" 16 1.2054e+01 7.6986e-02 2.9779e-01 8.9986e-01 2.04963e-01 0.5000 8.9986e-01 25\n",
" 17 1.0195e+01 7.6524e-02 1.4899e-01 8.6282e-01 9.66096e-02 1.0000 8.6282e-01 19\n",
" 18 6.0030e+00 7.6622e-02 1.5624e-04 5.9248e-01 1.47173e+00 0.0020 5.9248e-01 25\n",
" 19 5.9976e+00 7.6602e-02 1.5589e-04 5.9195e-01 2.09174e-01 0.1250 5.9195e-01 25\n",
" 20 5.7692e+00 7.6475e-02 1.4420e-04 5.6913e-01 1.06667e-01 0.5000 5.6913e-01 20\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 5.3924e+00 7.6459e-02 1.0748e-04 5.3149e-01 8.29165e-02 0.5000 5.3149e-01 18\n",
" 22 4.0889e+00 7.6525e-02 7.5394e-05 4.0116e-01 6.95050e-02 1.0000 4.0116e-01 18\n",
" 23 3.3597e+00 7.6661e-02 6.7706e-05 3.2823e-01 1.25951e+00 0.0020 3.2823e-01 25\n",
" 24 3.3576e+00 7.6647e-02 6.7631e-05 3.2803e-01 1.68066e-01 0.1250 3.2803e-01 23\n",
" 25 3.2537e+00 7.6563e-02 6.3846e-05 3.1765e-01 8.79980e-02 0.2500 3.1765e-01 18\n",
" 26 2.8628e+00 7.6548e-02 5.1864e-05 2.7857e-01 2.86438e-02 1.0000 2.7857e-01 16\n",
" 27 2.3594e-01 7.6740e-02 3.9072e-05 1.5881e-02 2.58826e-01 0.0020 1.5881e-02 5\n",
" 28 2.3590e-01 7.6740e-02 3.8996e-05 1.5878e-02 3.58452e-02 0.0625 1.5878e-02 5\n",
" 29 2.3210e-01 7.6737e-02 3.6552e-05 1.5500e-02 2.90692e-02 0.0312 1.5500e-02 1\n",
" 30 2.3158e-01 7.6736e-02 3.5408e-05 1.5449e-02 2.62083e-02 0.2500 1.5449e-02 4\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.1730e-01 7.6733e-02 2.6330e-05 1.4030e-02 9.82913e-03 1.0000 1.4030e-02 4\n",
" END 1.2817e-01 7.6738e-02 5.6896e-06 5.1375e-03 ---- ---- 5.1375e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.9859e+01 8.6438e-02 1.9723e+00 4.9967e-03 1.41724e+00 0.0625 ---- 4\n",
" 2 1.9583e+01 8.5462e-02 1.8491e+00 1.0068e-01 3.66134e+00 0.0020 7.2107e-01 25\n",
" 3 1.9570e+01 8.5419e-02 1.8455e+00 1.0299e-01 2.77150e+00 0.0039 7.1961e-01 25\n",
" 4 1.9549e+01 8.5351e-02 1.8383e+00 1.0809e-01 1.48207e+00 0.0312 7.1672e-01 25\n",
" 5 1.9493e+01 8.4929e-02 1.7809e+00 1.5990e-01 3.19466e+00 0.0020 6.9387e-01 25\n",
" 6 1.9484e+01 8.4887e-02 1.7774e+00 1.6250e-01 2.26198e+00 0.0039 6.9247e-01 25\n",
" 7 1.9466e+01 8.4823e-02 1.7704e+00 1.6768e-01 1.03343e+00 0.0312 6.8970e-01 25\n",
" 8 1.9177e+01 8.4443e-02 1.7151e+00 1.9407e-01 1.69292e+00 0.0078 6.6776e-01 25\n",
" 9 1.9145e+01 8.4333e-02 1.7017e+00 2.0437e-01 3.29425e-01 1.0000 6.6244e-01 25\n",
" 10 3.9230e+00 7.8175e-02 1.3804e-02 3.7068e-01 5.00844e+00 0.0020 3.7068e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 3.9230e+00 7.8175e-02 1.3804e-02 3.7068e-01 1.48348e+00 0.0020 3.7068e-01 25\n",
" 12 3.9202e+00 7.8160e-02 1.3777e-02 3.7043e-01 1.95075e-01 0.1250 3.7043e-01 25\n",
" 13 3.7584e+00 7.8062e-02 1.2053e-02 3.5598e-01 1.22970e-01 0.2500 3.5598e-01 25\n",
" 14 3.3611e+00 7.8012e-02 9.0204e-03 3.1929e-01 4.93300e-02 1.0000 3.1929e-01 25\n",
" 15 6.6557e-01 7.8161e-02 3.5163e-04 5.8389e-02 3.61574e-01 0.0039 5.8389e-02 25\n",
" 16 6.6497e-01 7.8159e-02 3.5026e-04 5.8331e-02 5.01241e-02 0.2500 5.8331e-02 25\n",
" 17 6.0332e-01 7.8148e-02 2.6358e-04 5.2254e-02 2.00496e-02 1.0000 5.2254e-02 25\n",
" END 1.3436e-01 7.8161e-02 1.6022e-05 5.6036e-03 ---- ---- 9.2811e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 1.9844e+01 8.8504e-02 1.9702e+00 5.3265e-03 1.59315e+00 0.0312 ---- 25\n",
" 2 1.9668e+01 8.7984e-02 1.9087e+00 4.9369e-02 2.79311e+00 0.0039 7.7691e-01 25\n",
" 3 1.9663e+01 8.7908e-02 1.9012e+00 5.6351e-02 1.55139e+00 0.0156 7.7379e-01 25\n",
" 4 1.9569e+01 8.7665e-02 1.8715e+00 7.6620e-02 1.72114e+00 0.0078 7.6144e-01 25\n",
" 5 1.9519e+01 8.7542e-02 1.8569e+00 8.6308e-02 9.98104e-01 0.0625 7.5536e-01 25\n",
" 6 1.8991e+01 8.6679e-02 1.7409e+00 1.4956e-01 4.28452e+00 0.0020 7.0724e-01 25\n",
" 7 1.8991e+01 8.6679e-02 1.7409e+00 1.4956e-01 4.11053e+00 0.0020 7.0724e-01 25\n",
" 8 1.8991e+01 8.6679e-02 1.7409e+00 1.4956e-01 2.94864e+00 0.0020 7.0724e-01 25\n",
" 9 1.8979e+01 8.6635e-02 1.7375e+00 1.5175e-01 7.60225e-01 0.0312 7.0583e-01 25\n",
" 10 1.8694e+01 8.6214e-02 1.6832e+00 1.7755e-01 3.12997e-01 1.0000 6.8335e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.8990e+00 7.9859e-02 1.3749e-02 2.6816e-01 5.10426e+00 0.0020 2.6816e-01 25\n",
" 12 2.8990e+00 7.9859e-02 1.3749e-02 2.6816e-01 1.54353e+00 0.0020 2.6816e-01 25\n",
" 13 2.8990e+00 7.9859e-02 1.3749e-02 2.6816e-01 2.19536e-01 0.0625 2.6816e-01 25\n",
" 14 2.8339e+00 7.9792e-02 1.2890e-02 2.6252e-01 1.67043e-01 0.1250 2.6252e-01 25\n",
" 15 2.7549e+00 7.9718e-02 1.1276e-02 2.5624e-01 9.69714e-02 0.5000 2.5624e-01 25\n",
" 16 2.6972e+00 7.9677e-02 5.5777e-03 2.5618e-01 7.65177e-02 0.5000 2.5618e-01 25\n",
" 17 2.2038e+00 7.9682e-02 2.7640e-03 2.0965e-01 6.38002e-02 0.5000 2.0965e-01 25\n",
" 18 1.7992e+00 7.9688e-02 1.3692e-03 1.7058e-01 5.99658e-02 0.5000 1.7058e-01 25\n",
" 19 1.5322e+00 7.9686e-02 6.8769e-04 1.4457e-01 5.65679e-02 0.5000 1.4457e-01 25\n",
" 20 1.3766e+00 7.9678e-02 3.5034e-04 1.2934e-01 5.59028e-02 0.5000 1.2934e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.3131e+00 7.9665e-02 1.8755e-04 1.2315e-01 5.62742e-02 0.2500 1.2315e-01 25\n",
" 22 1.1602e+00 7.9656e-02 1.4447e-04 1.0791e-01 1.42214e-02 1.0000 1.0791e-01 25\n",
" 23 8.5487e-02 7.9717e-02 1.2688e-05 5.6427e-04 8.17409e-02 0.0020 1.4000e-02 25\n",
" 24 8.5484e-02 7.9717e-02 1.2663e-05 5.6409e-04 1.41162e-02 0.1250 1.6218e-02 25\n",
" 25 8.5275e-02 7.9713e-02 1.1079e-05 5.4512e-04 1.13311e-02 0.5000 1.6216e-02 25\n",
" 26 8.4420e-02 7.9697e-02 5.7405e-06 4.6660e-04 1.03176e-02 0.5000 1.6205e-02 25\n",
" 27 8.4269e-02 7.9681e-02 3.2610e-06 4.5558e-04 1.05808e-02 0.2500 1.6182e-02 25\n",
" 28 8.3836e-02 7.9673e-02 2.5089e-06 4.1380e-04 8.17537e-03 1.0000 1.6168e-02 25\n",
" 29 8.0005e-02 7.9642e-02 2.2286e-06 3.4119e-05 4.16014e-02 0.0039 1.3850e-02 25\n",
" 30 8.0004e-02 7.9642e-02 2.2206e-06 3.4074e-05 8.08400e-03 0.1250 1.6127e-02 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 7.9992e-02 7.9638e-02 1.9664e-06 3.3443e-05 7.74082e-03 0.5000 1.6123e-02 25\n",
" 32 7.9945e-02 7.9622e-02 1.3764e-06 3.0935e-05 7.54026e-03 0.5000 1.6106e-02 25\n",
" 33 7.9931e-02 7.9607e-02 1.0950e-06 3.1389e-05 7.49160e-03 0.2500 1.6088e-02 25\n",
" 34 7.9913e-02 7.9599e-02 9.0230e-07 3.0514e-05 7.14270e-03 1.0000 1.6078e-02 25\n",
" 35 7.9800e-02 7.9568e-02 1.4019e-06 2.1736e-05 3.36310e-02 0.0078 1.3774e-02 25\n",
" 36 7.9798e-02 7.9568e-02 1.3925e-06 2.1615e-05 6.82284e-03 1.0000 1.6043e-02 25\n",
" 37 7.9791e-02 7.9538e-02 1.2174e-06 2.4154e-05 3.34275e-02 0.0039 1.3763e-02 25\n",
" 38 7.9791e-02 7.9538e-02 1.2130e-06 2.4158e-05 6.68327e-03 0.2500 1.6016e-02 25\n",
" 39 7.9787e-02 7.9530e-02 9.8371e-07 2.4762e-05 6.44503e-03 0.5000 1.6005e-02 25\n",
" 40 7.9743e-02 7.9515e-02 7.6821e-07 2.2034e-05 6.20154e-03 0.5000 1.0246e-02 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" END 7.9652e-02 7.9501e-02 6.6131e-07 1.4509e-05 ---- ---- 9.1755e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.0575e+01 9.0182e-02 2.0485e+00 1.0877e-05 1.22458e+00 0.1250 ---- 25\n",
" 2 1.9189e+01 8.8155e-02 1.7927e+00 1.1736e-01 3.19091e+00 0.0078 7.1923e-01 25\n",
" 3 1.9163e+01 8.8030e-02 1.7787e+00 1.2881e-01 1.67882e+00 0.0312 7.1350e-01 25\n",
" 4 1.9144e+01 8.7627e-02 1.7231e+00 1.8246e-01 2.87865e+00 0.0020 6.9082e-01 25\n",
" 5 1.9114e+01 8.7586e-02 1.7198e+00 1.8289e-01 2.03514e+00 0.0039 6.8942e-01 25\n",
" 6 1.9067e+01 8.7526e-02 1.7131e+00 1.8483e-01 9.85361e-01 0.0625 6.8666e-01 25\n",
" 7 1.8742e+01 8.6832e-02 1.6060e+00 2.5949e-01 4.61499e+00 0.0020 6.4300e-01 25\n",
" 8 1.8742e+01 8.6832e-02 1.6060e+00 2.5949e-01 3.35532e+00 0.0020 6.4300e-01 25\n",
" 9 1.8730e+01 8.6790e-02 1.6029e+00 2.6147e-01 8.51925e-01 0.0312 6.4171e-01 25\n",
" 10 1.8465e+01 8.6427e-02 1.5528e+00 2.8508e-01 3.66530e-01 1.0000 6.2129e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.2932e+01 8.1123e-02 1.1191e-02 1.2739e+00 6.31524e+00 0.0020 1.2739e+00 25\n",
" 12 1.2932e+01 8.1123e-02 1.1191e-02 1.2739e+00 2.66344e+00 0.0039 1.2739e+00 25\n",
" 13 1.2913e+01 8.1046e-02 1.1149e-02 1.2720e+00 4.23947e-01 0.1250 1.2720e+00 25\n",
" 14 1.2119e+01 8.0730e-02 9.7696e-03 1.1941e+00 2.85087e-01 0.5000 1.1941e+00 25\n",
" 15 1.1694e+01 8.0305e-02 4.9052e-03 1.1565e+00 1.49043e-01 0.5000 1.1565e+00 17\n",
" 16 8.9001e+00 8.0374e-02 2.4244e-03 8.7955e-01 1.29972e-01 0.5000 8.7955e-01 18\n",
" 17 8.8333e+00 8.0274e-02 1.2110e-03 8.7409e-01 1.42887e-01 0.2500 8.7409e-01 19\n",
" 18 8.2061e+00 8.0164e-02 9.1270e-04 8.1168e-01 6.83406e-02 1.0000 8.1168e-01 13\n",
" 19 6.5165e-01 8.0820e-02 6.4543e-04 5.6437e-02 5.41614e-01 0.0020 5.6437e-02 25\n",
" 20 6.5165e-01 8.0820e-02 6.4543e-04 5.6437e-02 8.35174e-02 0.0625 5.6437e-02 18\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 6.4505e-01 8.0801e-02 6.0512e-04 5.5820e-02 6.08787e-02 0.1250 5.5820e-02 17\n",
" 22 6.3052e-01 8.0782e-02 5.2843e-04 5.4445e-02 3.61706e-02 0.5000 5.4445e-02 16\n",
" 23 5.0504e-01 8.0774e-02 2.4917e-04 4.2178e-02 3.13344e-02 0.2500 4.2178e-02 16\n",
" 24 4.5558e-01 8.0764e-02 1.8683e-04 3.7295e-02 1.91842e-02 1.0000 3.7295e-02 16\n",
" END 1.0370e-01 8.0761e-02 2.2325e-05 2.2720e-03 ---- ---- 6.8769e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.0706e+01 9.1753e-02 2.0608e+00 6.5086e-04 1.40417e+00 0.0625 ---- 25\n",
" 2 1.9923e+01 9.0645e-02 1.9321e+00 5.1160e-02 3.28947e+00 0.0039 7.9650e-01 25\n",
" 3 1.9919e+01 9.0539e-02 1.9245e+00 5.8329e-02 1.53625e+00 0.0156 7.9325e-01 25\n",
" 4 1.9917e+01 9.0290e-02 1.8945e+00 8.8206e-02 2.10074e+00 0.0020 7.8059e-01 25\n",
" 5 1.9901e+01 9.0250e-02 1.8908e+00 9.0282e-02 1.39700e+00 0.0078 7.7901e-01 25\n",
" 6 1.9884e+01 9.0123e-02 1.8760e+00 1.0341e-01 1.00035e+00 0.0156 7.7277e-01 25\n",
" 7 1.9797e+01 8.9894e-02 1.8467e+00 1.2404e-01 1.58089e+00 0.0039 7.6045e-01 25\n",
" 8 1.9783e+01 8.9825e-02 1.8395e+00 1.2990e-01 6.22082e-01 0.1250 7.5739e-01 25\n",
" 9 1.9048e+01 8.8254e-02 1.6097e+00 2.8627e-01 4.07925e+00 0.0020 6.6121e-01 25\n",
" 10 1.9048e+01 8.8254e-02 1.6097e+00 2.8627e-01 1.24961e+00 0.0156 6.6126e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.9039e+01 8.7958e-02 1.5845e+00 3.1058e-01 7.06420e-01 0.0312 6.5056e-01 25\n",
" 12 1.8938e+01 8.7555e-02 1.5350e+00 3.5006e-01 2.68501e-01 1.0000 6.2979e-01 25\n",
" 13 1.3106e+00 8.2351e-02 1.0480e-02 1.1235e-01 3.27287e+00 0.0020 1.1235e-01 25\n",
" 14 1.3106e+00 8.2351e-02 1.0480e-02 1.1235e-01 9.87785e-01 0.0020 1.1235e-01 25\n",
" 15 1.3105e+00 8.2344e-02 1.0459e-02 1.1235e-01 1.28599e-01 0.1250 1.1235e-01 25\n",
" 16 1.2898e+00 8.2296e-02 9.1503e-03 1.1160e-01 7.80609e-02 0.5000 1.1160e-01 25\n",
" 17 1.2460e+00 8.2254e-02 4.5402e-03 1.1184e-01 5.86975e-02 0.5000 1.1184e-01 25\n",
" 18 1.2445e+00 8.2229e-02 2.2550e-03 1.1397e-01 6.08886e-02 0.2500 1.1397e-01 25\n",
" 19 1.1898e+00 8.2203e-02 1.6904e-03 1.0907e-01 2.41220e-02 1.0000 1.0907e-01 25\n",
" 20 1.9391e-01 8.2255e-02 5.5758e-05 1.1110e-02 2.51580e-01 0.0020 1.2648e-02 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.9391e-01 8.2255e-02 5.5758e-05 1.1110e-02 3.96708e-02 0.0625 1.1110e-02 25\n",
" 22 1.9266e-01 8.2249e-02 5.2248e-05 1.0989e-02 3.16413e-02 0.1250 1.0989e-02 25\n",
" 23 1.9014e-01 8.2242e-02 4.5655e-05 1.0745e-02 2.10131e-02 0.5000 1.0745e-02 25\n",
" END 1.6636e-01 8.2225e-02 2.2901e-05 8.3903e-03 ---- ---- 8.3903e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.0786e+01 9.3757e-02 2.0649e+00 4.2995e-03 1.61017e+00 0.0312 ---- 25\n",
" 2 2.0483e+01 9.3158e-02 2.0004e+00 3.8590e-02 3.48396e+00 0.0020 8.5344e-01 25\n",
" 3 2.0464e+01 9.3098e-02 1.9965e+00 4.0647e-02 2.39920e+00 0.0039 8.5169e-01 25\n",
" 4 2.0428e+01 9.3007e-02 1.9887e+00 4.4804e-02 1.20633e+00 0.0625 8.4824e-01 25\n",
" 5 2.0144e+01 9.1971e-02 1.8644e+00 1.4077e-01 5.63187e+00 0.0020 7.9410e-01 25\n",
" 6 2.0144e+01 9.1971e-02 1.8644e+00 1.4077e-01 5.42093e+00 0.0020 7.9411e-01 25\n",
" 7 2.0144e+01 9.1971e-02 1.8644e+00 1.4077e-01 3.99241e+00 0.0020 7.9411e-01 25\n",
" 8 2.0144e+01 9.1971e-02 1.8644e+00 1.4077e-01 1.27816e+00 0.0156 7.9416e-01 25\n",
" 9 2.0100e+01 9.1650e-02 1.8353e+00 1.6551e-01 6.57117e-01 0.0625 7.8134e-01 25\n",
" 10 1.9808e+01 9.0748e-02 1.7206e+00 2.5108e-01 7.67478e-01 0.0312 7.3161e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.9645e+01 9.0304e-02 1.6669e+00 2.8859e-01 3.43901e-01 1.0000 7.0827e-01 25\n",
" 12 1.6387e+01 8.4059e-02 1.4866e-02 1.6154e+00 7.93808e+00 0.0020 1.6154e+00 25\n",
" 13 1.6378e+01 8.3930e-02 1.4840e-02 1.6146e+00 3.58435e+00 0.0039 1.6146e+00 25\n",
" 14 1.6359e+01 8.3779e-02 1.4785e-02 1.6128e+00 6.51292e-01 0.0625 1.6128e+00 22\n",
" 15 1.5952e+01 8.3303e-02 1.3863e-02 1.5731e+00 4.68213e-01 0.1250 1.5731e+00 17\n",
" 16 1.5758e+01 8.2722e-02 1.2128e-02 1.5554e+00 1.82458e-01 0.2500 1.5554e+00 15\n",
" 17 1.4154e+01 8.2617e-02 9.0774e-03 1.3981e+00 1.39778e-01 1.0000 1.3981e+00 12\n",
" 18 1.2377e+00 8.3806e-02 2.2617e-03 1.1313e-01 8.64401e-01 0.0020 1.1313e-01 25\n",
" 19 1.2377e+00 8.3806e-02 2.2617e-03 1.1313e-01 1.33295e-01 0.0625 1.1313e-01 11\n",
" 20 1.2270e+00 8.3768e-02 2.1208e-03 1.1220e-01 9.21423e-02 0.1250 1.1220e-01 10\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 1.1972e+00 8.3735e-02 1.8533e-03 1.0949e-01 4.98834e-02 1.0000 1.0949e-01 9\n",
" 22 1.1401e+00 8.3732e-02 1.6369e-04 1.0547e-01 7.47634e-01 0.0020 1.0547e-01 25\n",
" 23 1.1401e+00 8.3732e-02 1.6369e-04 1.0547e-01 1.08815e-01 0.0625 1.0547e-01 10\n",
" 24 1.1192e+00 8.3702e-02 1.5420e-04 1.0339e-01 7.98366e-02 0.1250 1.0339e-01 10\n",
" 25 1.0905e+00 8.3671e-02 1.3620e-04 1.0055e-01 4.34498e-02 0.5000 1.0055e-01 9\n",
" 26 9.2934e-01 8.3667e-02 6.7163e-05 8.4500e-02 5.15577e-02 0.2500 8.4500e-02 9\n",
" 27 8.8302e-01 8.3649e-02 5.5921e-05 7.9881e-02 2.57625e-02 1.0000 7.9881e-02 9\n",
" 28 2.4630e-01 8.3677e-02 1.4627e-05 1.6248e-02 2.86514e-01 0.0020 1.9468e-02 16\n",
" 29 2.4630e-01 8.3677e-02 1.4627e-05 1.6248e-02 4.73930e-02 0.0625 1.6248e-02 9\n",
" 30 2.4614e-01 8.3669e-02 1.3709e-05 1.6233e-02 3.66535e-02 0.1250 1.6233e-02 9\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 2.4458e-01 8.3660e-02 1.1963e-05 1.6080e-02 2.22558e-02 0.5000 1.6080e-02 8\n",
" 32 2.1162e-01 8.3645e-02 6.7695e-06 1.2791e-02 2.47726e-02 0.2500 1.2791e-02 8\n",
" 33 2.0419e-01 8.3635e-02 5.2384e-06 1.2050e-02 1.63663e-02 1.0000 1.2050e-02 8\n",
" END 1.2110e-01 8.3606e-02 7.3510e-06 3.7423e-03 ---- ---- 8.3650e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.0729e+01 9.5312e-02 2.0613e+00 2.0355e-03 1.42404e+00 0.0625 ---- 25\n",
" 2 1.9977e+01 9.4134e-02 1.9326e+00 5.5747e-02 4.36122e+00 0.0020 8.4286e-01 25\n",
" 3 1.9977e+01 9.4134e-02 1.9326e+00 5.5747e-02 4.35946e+00 0.0020 8.4286e-01 25\n",
" 4 1.9977e+01 9.4134e-02 1.9326e+00 5.5747e-02 4.34195e+00 0.0020 8.4286e-01 25\n",
" 5 1.9977e+01 9.4134e-02 1.9326e+00 5.5747e-02 4.17476e+00 0.0020 8.4286e-01 25\n",
" 6 1.9975e+01 9.4065e-02 1.9288e+00 5.9329e-02 2.13176e+00 0.0039 8.4112e-01 25\n",
" 7 1.9962e+01 9.3976e-02 1.9213e+00 6.5496e-02 4.89565e-01 0.2500 8.3775e-01 25\n",
" 8 1.9653e+01 9.0503e-02 1.4419e+00 5.1439e-01 2.25323e+00 0.0039 6.2572e-01 25\n",
" 9 1.9620e+01 9.0386e-02 1.4363e+00 5.1668e-01 4.02352e-01 0.2500 6.2317e-01 25\n",
" 10 1.8543e+01 8.7859e-02 1.0779e+00 7.6764e-01 1.40042e-01 1.0000 7.6764e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 2.1163e+00 8.5438e-02 5.3233e-03 1.9776e-01 1.30782e+00 0.0020 1.9776e-01 25\n",
" 12 2.1163e+00 8.5438e-02 5.3233e-03 1.9776e-01 1.84653e-01 0.0625 1.9776e-01 25\n",
" 13 2.1056e+00 8.5368e-02 4.9887e-03 1.9704e-01 1.20220e-01 0.1250 1.9704e-01 25\n",
" 14 2.0661e+00 8.5313e-02 4.3642e-03 1.9371e-01 6.31520e-02 1.0000 1.9371e-01 25\n",
" 15 1.8301e+00 8.5372e-02 1.0446e-04 1.7437e-01 1.13591e+00 0.0020 1.7437e-01 25\n",
" 16 1.8301e+00 8.5372e-02 1.0446e-04 1.7437e-01 1.59977e-01 0.0625 1.7437e-01 25\n",
" 17 1.8027e+00 8.5318e-02 9.8731e-05 1.7164e-01 1.15193e-01 0.1250 1.7164e-01 25\n",
" 18 1.7720e+00 8.5266e-02 8.8227e-05 1.6858e-01 6.42703e-02 0.5000 1.6858e-01 25\n",
" 19 1.5367e+00 8.5277e-02 5.7384e-05 1.4508e-01 6.69750e-02 0.2500 1.4508e-01 25\n",
" 20 1.5075e+00 8.5248e-02 4.3517e-05 1.4218e-01 3.64625e-02 1.0000 1.4218e-01 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 21 5.0693e-01 8.5338e-02 4.3990e-05 4.2115e-02 5.10116e-01 0.0020 4.2115e-02 25\n",
" 22 5.0693e-01 8.5338e-02 4.3990e-05 4.2115e-02 7.31315e-02 0.0312 4.2115e-02 25\n",
" 23 5.0094e-01 8.5328e-02 4.2621e-05 4.1518e-02 6.15263e-02 0.0625 4.1518e-02 25\n",
" 24 4.9496e-01 8.5315e-02 3.9903e-05 4.0925e-02 4.41523e-02 0.1250 4.0925e-02 25\n",
" 25 4.7987e-01 8.5304e-02 3.4557e-05 3.9422e-02 2.55549e-02 1.0000 3.9422e-02 25\n",
" 26 3.7544e-01 8.5294e-02 2.2863e-05 2.8992e-02 4.03642e-01 0.0020 3.0428e-02 25\n",
" 27 3.7544e-01 8.5294e-02 2.2863e-05 2.8992e-02 5.95919e-02 0.0625 2.8992e-02 25\n",
" 28 3.7427e-01 8.5281e-02 2.1553e-05 2.8877e-02 4.46707e-02 0.1250 2.8877e-02 25\n",
" 29 3.7199e-01 8.5268e-02 1.9055e-05 2.8653e-02 2.64100e-02 0.5000 2.8653e-02 25\n",
" 30 3.1360e-01 8.5258e-02 1.0267e-05 2.2824e-02 3.02377e-02 0.2500 2.2824e-02 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 31 3.0582e-01 8.5246e-02 9.1264e-06 2.2048e-02 1.95617e-02 1.0000 2.2048e-02 25\n",
" 32 1.4539e-01 8.5231e-02 5.4601e-06 6.0106e-03 2.01368e-01 0.0020 1.4205e-02 17\n",
" END 1.4539e-01 8.5231e-02 5.4601e-06 6.0106e-03 ---- ---- 9.2580e-03 -----\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 1 2.0411e+01 9.7549e-02 2.0311e+00 2.3399e-04 1.56684e+00 0.0312 ---- 25\n",
" 2 1.9879e+01 9.6896e-02 1.9676e+00 1.0621e-02 3.34242e+00 0.0020 9.0684e-01 25\n",
" 3 1.9843e+01 9.6830e-02 1.9638e+00 1.0803e-02 2.23352e+00 0.0039 9.0497e-01 25\n",
" 4 1.9818e+01 9.6733e-02 1.9561e+00 1.6020e-02 1.25086e+00 0.0625 9.0130e-01 25\n",
" 5 1.9235e+01 9.5598e-02 1.8339e+00 8.0008e-02 4.39080e+00 0.0020 8.4369e-01 25\n",
" 6 1.9235e+01 9.5598e-02 1.8339e+00 8.0008e-02 4.21798e+00 0.0020 8.4370e-01 25\n",
" 7 1.9235e+01 9.5598e-02 1.8339e+00 8.0008e-02 3.05196e+00 0.0020 8.4370e-01 25\n",
" 8 1.9228e+01 9.5538e-02 1.8304e+00 8.2932e-02 7.70400e-01 0.0312 8.4200e-01 25\n",
" 9 1.9011e+01 9.4995e-02 1.7732e+00 1.1840e-01 3.80867e-01 1.0000 8.1508e-01 25\n",
" 10 1.5702e+01 8.7055e-02 1.4431e-02 1.5471e+00 7.36781e+00 0.0020 1.5471e+00 25\n",
"iter merit cost ||gaps|| ||Constraint|| ||(dx,du)|| step KKT criteria QP iters \n",
" 11 1.5699e+01 8.6935e-02 1.4406e-02 1.5468e+00 3.03264e+00 0.0039 1.5468e+00 25\n",
" 12 1.5670e+01 8.6817e-02 1.4353e-02 1.5440e+00 4.64512e-01 0.1250 1.5440e+00 18\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[121], line 12\u001b[0m\n\u001b[1;32m 10\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 11\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---> 12\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 13\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 15\u001b[0m \u001b[38;5;66;03m# solution=mpc.getSolution()\u001b[39;00m\n",
"Cell \u001b[0;32mIn[82], line 268\u001b[0m, in \u001b[0;36mGo2MPC.updateAndSolve\u001b[0;34m(self, t, quat, q, v, omega, dq)\u001b[0m\n\u001b[1;32m 266\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 267\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--> 268\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 269\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 270\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:38\u001b[0m, in \u001b[0;36mResidualFrictionCone.calc\u001b[0;34m(self, data, x, u)\u001b[0m\n\u001b[1;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[0;32m---> 38\u001b[0m F \u001b[38;5;241m=\u001b[39m \u001b[43mdata\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mshared\u001b[49m\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",
"\n",
"for i in range(500):\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(20):\n",
" robot.setCommands(q, dq, kp, kv, tau)\n",
" robot.step()\n",
" time.sleep(0.0005)\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",
" # print(getForceInWorld(robot, force_in_body, force_sensor_site))"
]
},
{
"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": 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