373 lines
150 KiB
Plaintext
373 lines
150 KiB
Plaintext
|
{
|
|||
|
|
"cells": [
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 65,
|
|||
|
|
"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_reordered.urdf'\n",
|
|||
|
|
"robot = pin.RobotWrapper.BuildFromURDF(\n",
|
|||
|
|
"urdf_path, urdf_root_path, pin.JointModelFreeFlyer())"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 66,
|
|||
|
|
"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": 67,
|
|||
|
|
"metadata": {},
|
|||
|
|
"outputs": [],
|
|||
|
|
"source": [
|
|||
|
|
"ee_frame_names = ['FL_foot', 'FR_foot', 'RL_foot', 'RR_foot']\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])"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 68,
|
|||
|
|
"metadata": {},
|
|||
|
|
"outputs": [],
|
|||
|
|
"source": [
|
|||
|
|
"q0 = np.array([0.0, 0.0, 0.35, 0.0, 0.0, 0.0, 1.0] \n",
|
|||
|
|
" +4*[0.0, 0.77832842, -1.56065452]\n",
|
|||
|
|
" )\n",
|
|||
|
|
"x0 = np.concatenate([q0, np.zeros(rmodel.nv)])"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 69,
|
|||
|
|
"metadata": {},
|
|||
|
|
"outputs": [],
|
|||
|
|
"source": [
|
|||
|
|
"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 "
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 70,
|
|||
|
|
"metadata": {},
|
|||
|
|
"outputs": [],
|
|||
|
|
"source": [
|
|||
|
|
"comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4\n",
|
|||
|
|
"comRef[2] = pinocchio.centerOfMass(rmodel, rdata, q0)[2].item() "
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 71,
|
|||
|
|
"metadata": {},
|
|||
|
|
"outputs": [],
|
|||
|
|
"source": [
|
|||
|
|
"supportFeetIds = [lfFootId, rfFootId, lhFootId, rhFootId]\n",
|
|||
|
|
"supportFeePos = [lfFootPos0, rfFootPos0, lhFootPos0, rhFootPos0]"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 72,
|
|||
|
|
"metadata": {},
|
|||
|
|
"outputs": [],
|
|||
|
|
"source": [
|
|||
|
|
"state = crocoddyl.StateMultibody(rmodel)\n",
|
|||
|
|
"actuation = crocoddyl.ActuationModelFloatingBase(state)\n",
|
|||
|
|
"nu = actuation.nu"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"cell_type": "code",
|
|||
|
|
"execution_count": 73,
|
|||
|
|
"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": 74,
|
|||
|
|
"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)\n",
|
|||
|
|
" if t == N_ocp:\n",
|
|||
|
|
" costModel.addCost(\"comTrack\", com_track, 1e5)\n",
|
|||
|
|
" else:\n",
|
|||
|
|
" costModel.addCost(\"comTrack\", com_track, 1e5)\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": 75,
|
|||
|
|
"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": 76,
|
|||
|
|
"metadata": {},
|
|||
|
|
"outputs": [
|
|||
|
|
{
|
|||
|
|
"data": {
|
|||
|
|
"text/plain": [
|
|||
|
|
"True"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
"execution_count": 76,
|
|||
|
|
"metadata": {},
|
|||
|
|
"output_type": "execute_result"
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"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": 93,
|
|||
|
|
"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",
|
|||
|
|
"\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": 97,
|
|||
|
|
"metadata": {},
|
|||
|
|
"outputs": [
|
|||
|
|
{
|
|||
|
|
"data": {
|
|||
|
|
"image/png": "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
|
|||
|
|
"text/plain": [
|
|||
|
|
"<Figure size 640x480 with 12 Axes>"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
"metadata": {},
|
|||
|
|
"output_type": "display_data"
|
|||
|
|
},
|
|||
|
|
{
|
|||
|
|
"data": {
|
|||
|
|
"image/png": "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
|
|||
|
|
"text/plain": [
|
|||
|
|
"<Figure size 640x480 with 1 Axes>"
|
|||
|
|
]
|
|||
|
|
},
|
|||
|
|
"metadata": {},
|
|||
|
|
"output_type": "display_data"
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"source": [
|
|||
|
|
"import matplotlib.pyplot as plt\n",
|
|||
|
|
"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()"
|
|||
|
|
]
|
|||
|
|
}
|
|||
|
|
],
|
|||
|
|
"metadata": {
|
|||
|
|
"kernelspec": {
|
|||
|
|
"display_name": "base",
|
|||
|
|
"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.12.7"
|
|||
|
|
}
|
|||
|
|
},
|
|||
|
|
"nbformat": 4,
|
|||
|
|
"nbformat_minor": 2
|
|||
|
|
}
|