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

998 lines
315 KiB
Plaintext
Raw Normal View History

dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
{
"cells": [
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Without ARM"
]
},
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
{
"cell_type": "code",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"execution_count": 61,
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"metadata": {},
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"outputs": [
{
"ename": "ValueError",
"evalue": "Mesh dae/base.dae could not be found.",
"output_type": "error",
"traceback": [
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[0;31mValueError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[0;32mIn[61], line 7\u001b[0m\n\u001b[1;32m 5\u001b[0m urdf_root_path \u001b[38;5;241m=\u001b[39m \u001b[38;5;124m'\u001b[39m\u001b[38;5;124m/home/Go2py/Go2Py/assets\u001b[39m\u001b[38;5;124m'\u001b[39m\n\u001b[1;32m 6\u001b[0m urdf_path \u001b[38;5;241m=\u001b[39m \u001b[38;5;124m'\u001b[39m\u001b[38;5;124m/home/Go2py/Go2Py/assets/urdf/go2_reordered.urdf\u001b[39m\u001b[38;5;124m'\u001b[39m\n\u001b[0;32m----> 7\u001b[0m robot \u001b[38;5;241m=\u001b[39m \u001b[43mpin\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mRobotWrapper\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mBuildFromURDF\u001b[49m\u001b[43m(\u001b[49m\n\u001b[1;32m 8\u001b[0m \u001b[43murdf_path\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43murdf_root_path\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mpin\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mJointModelFreeFlyer\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\u001b[43m)\u001b[49m\n",
"File \u001b[0;32m/opt/conda/lib/python3.12/site-packages/pinocchio/robot_wrapper.py:21\u001b[0m, in \u001b[0;36mRobotWrapper.BuildFromURDF\u001b[0;34m(filename, package_dirs, root_joint, verbose, meshLoader)\u001b[0m\n\u001b[1;32m 16\u001b[0m \u001b[38;5;129m@staticmethod\u001b[39m\n\u001b[1;32m 17\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21mBuildFromURDF\u001b[39m(\n\u001b[1;32m 18\u001b[0m filename, package_dirs\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mNone\u001b[39;00m, root_joint\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mNone\u001b[39;00m, verbose\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mFalse\u001b[39;00m, meshLoader\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mNone\u001b[39;00m\n\u001b[1;32m 19\u001b[0m ):\n\u001b[1;32m 20\u001b[0m robot \u001b[38;5;241m=\u001b[39m RobotWrapper()\n\u001b[0;32m---> 21\u001b[0m \u001b[43mrobot\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minitFromURDF\u001b[49m\u001b[43m(\u001b[49m\u001b[43mfilename\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mpackage_dirs\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mroot_joint\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mverbose\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mmeshLoader\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 22\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m robot\n",
"File \u001b[0;32m/opt/conda/lib/python3.12/site-packages/pinocchio/robot_wrapper.py:32\u001b[0m, in \u001b[0;36mRobotWrapper.initFromURDF\u001b[0;34m(self, filename, package_dirs, root_joint, verbose, meshLoader)\u001b[0m\n\u001b[1;32m 24\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21minitFromURDF\u001b[39m(\n\u001b[1;32m 25\u001b[0m \u001b[38;5;28mself\u001b[39m,\n\u001b[1;32m 26\u001b[0m filename,\n\u001b[0;32m (...)\u001b[0m\n\u001b[1;32m 30\u001b[0m meshLoader\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mNone\u001b[39;00m,\n\u001b[1;32m 31\u001b[0m ):\n\u001b[0;32m---> 32\u001b[0m model, collision_model, visual_model \u001b[38;5;241m=\u001b[39m \u001b[43mbuildModelsFromUrdf\u001b[49m\u001b[43m(\u001b[49m\n\u001b[1;32m 33\u001b[0m \u001b[43m \u001b[49m\u001b[43mfilename\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mpackage_dirs\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mroot_joint\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mverbose\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mmeshLoader\u001b[49m\n\u001b[1;32m 34\u001b[0m \u001b[43m \u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 35\u001b[0m RobotWrapper\u001b[38;5;241m.\u001b[39m\u001b[38;5;21m__init__\u001b[39m(\n\u001b[1;32m 36\u001b[0m \u001b[38;5;28mself\u001b[39m,\n\u001b[1;32m 37\u001b[0m model\u001b[38;5;241m=\u001b[39mmodel,\n\u001b[1;32m 38\u001b[0m collision_model\u001b[38;5;241m=\u001b[39mcollision_model,\n\u001b[1;32m 39\u001b[0m visual_model\u001b[38;5;241m=\u001b[39mvisual_model,\n\u001b[1;32m 40\u001b[0m )\n",
"File \u001b[0;32m/opt/conda/lib/python3.12/site-packages/pinocchio/shortcuts.py:63\u001b[0m, in \u001b[0;36mbuildModelsFromUrdf\u001b[0;34m(filename, package_dirs, root_joint, verbose, meshLoader, geometry_types)\u001b[0m\n\u001b[1;32m 61\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m geometry_type \u001b[38;5;129;01min\u001b[39;00m geometry_types:\n\u001b[1;32m 62\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m meshLoader \u001b[38;5;129;01mis\u001b[39;00m \u001b[38;5;28;01mNone\u001b[39;00m \u001b[38;5;129;01mor\u001b[39;00m (\u001b[38;5;129;01mnot\u001b[39;00m WITH_HPP_FCL \u001b[38;5;129;01mand\u001b[39;00m \u001b[38;5;129;01mnot\u001b[39;00m WITH_HPP_FCL_BINDINGS):\n\u001b[0;32m---> 63\u001b[0m geom_model \u001b[38;5;241m=\u001b[39m \u001b[43mpin\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mbuildGeomFromUrdf\u001b[49m\u001b[43m(\u001b[49m\n\u001b[1;32m 64\u001b[0m \u001b[43m \u001b[49m\u001b[43mmodel\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mfilename\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mgeometry_type\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mpackage_dirs\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mpackage_dirs\u001b[49m\n\u001b[1;32m 65\u001b[0m \u001b[43m \u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 66\u001b[0m \u001b[38;5;28;01melse\u001b[39;00m:\n\u001b[1;32m 67\u001b[0m geom_model \u001b[38;5;241m=\u001b[39m pin\u001b[38;5;241m.\u001b[39mbuildGeomFromUrdf(\n\u001b[1;32m 68\u001b[0m model,\n\u001b[1;32m 69\u001b[0m filename,\n\u001b[0;32m (...)\u001b[0m\n\u001b[1;32m 72\u001b[0m mesh_loader\u001b[38;5;241m=\u001b[39mmeshLoader,\n\u001b[1;32m 73\u001b[0m )\n",
"\u001b[0;31mValueError\u001b[0m: Mesh dae/base.dae could not be found."
]
}
],
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"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",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"execution_count": 62,
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"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",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"execution_count": 28,
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"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",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"execution_count": 29,
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"metadata": {},
"outputs": [],
"source": [
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"q0 = np.array([0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 1.0] \n",
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
" +4*[0.0, 0.77832842, -1.56065452]\n",
" )\n",
"x0 = np.concatenate([q0, np.zeros(rmodel.nv)])"
]
},
{
"cell_type": "code",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"execution_count": 30,
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"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 "
]
},
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
{
"cell_type": "code",
"execution_count": 34,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"The desired CoM position is: [0.00060515 0. 0.27853327]\n"
]
}
],
"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": 35,
"metadata": {},
"outputs": [],
"source": [
"supportFeetIds = [lfFootId, rfFootId, lhFootId, rhFootId]\n",
"supportFeePos = [lfFootPos0, rfFootPos0, lhFootPos0, rhFootPos0]"
]
},
{
"cell_type": "code",
"execution_count": 36,
"metadata": {},
"outputs": [],
"source": [
"state = crocoddyl.StateMultibody(rmodel)\n",
"actuation = crocoddyl.ActuationModelFloatingBase(state)\n",
"nu = actuation.nu"
]
},
{
"cell_type": "code",
"execution_count": 37,
"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": 38,
"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": 39,
"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": 40,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"True"
]
},
"execution_count": 40,
"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": 41,
"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": 42,
"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()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Animate The Motion"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"from Go2Py.sim.mujoco import Go2Sim\n",
"robot_sim = Go2Sim()"
]
},
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
{
"cell_type": "code",
"execution_count": 70,
"metadata": {},
"outputs": [],
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"source": [
"import mujoco\n",
"import time\n",
"for i in range(len(xs)):\n",
" q0 = xs[i][:19]\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",
" robot_sim.reset(q0)\n",
" time.sleep(0.01)"
]
},
{
"cell_type": "code",
"execution_count": 71,
"metadata": {},
"outputs": [],
"source": [
"from Go2Py.robot.model import Go2Model\n",
"model = Go2Model()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## With Arm"
]
},
{
"cell_type": "code",
"execution_count": 63,
"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": 64,
"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": 65,
"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": 66,
"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": 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 \n",
"efPos0 = rdata.oMf[efId].translation"
]
},
{
"cell_type": "code",
"execution_count": 70,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"The desired CoM position is: [0.00060515 0. 0.29477433]\n"
]
}
],
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"source": [
"comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4\n",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"comRef[2] = pinocchio.centerOfMass(rmodel, rdata, q0)[2].item() \n",
"print(f'The desired CoM position is: {comRef}')"
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
]
},
{
"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",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
" com_track = crocoddyl.CostModelResidual(state, com_activation, com_residual) # What does it correspond to exactly?\n",
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
" if t == N_ocp:\n",
" costModel.addCost(\"comTrack\", com_track, 1e5)\n",
" else:\n",
" costModel.addCost(\"comTrack\", com_track, 1e5)\n",
"\n",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
" # 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",
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
" 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",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"execution_count": 77,
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"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",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"xs, us = solver.xs, solver.us\n",
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"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",
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"execution_count": 78,
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"metadata": {},
"outputs": [
{
"data": {
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"image/png": "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
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"text/plain": [
"<Figure size 640x480 with 12 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
"image/png": "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
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
"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()"
]
D1 arm assets added and ocp example updated.
2024-12-04 00:06:54 +08:00
},
{
"cell_type": "code",
"execution_count": 111,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Joint FL_hip_joint has id 4\n",
"Joint FL_thigh_joint has id 5\n",
"Joint FL_calf_joint has id 6\n",
"Joint FR_hip_joint has id 1\n",
"Joint FR_thigh_joint has id 2\n",
"Joint FR_calf_joint has id 3\n",
"Joint RL_hip_joint has id 10\n",
"Joint RL_thigh_joint has id 11\n",
"Joint RL_calf_joint has id 12\n",
"Joint RR_hip_joint has id 7\n",
"Joint RR_thigh_joint has id 8\n",
"Joint RR_calf_joint has id 9\n",
"Joint Joint1 has id 13\n",
"Joint Joint2 has id 14\n",
"Joint Joint3 has id 15\n",
"Joint Joint4 has id 16\n",
"Joint Joint5 has id 17\n",
"Joint Joint6 has id 18\n",
"Joint Joint7_1 has id 19\n",
"Joint Joint7_2 has id 20\n"
]
}
],
"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": 112,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Joint FL_HAA_joint has id 2\n",
"Joint FL_HFE_joint has id 3\n",
"Joint FL_KFE_joint has id 4\n",
"Joint FR_HAA_joint has id 5\n",
"Joint FR_HFE_joint has id 6\n",
"Joint FR_KFE_joint has id 7\n",
"Joint HL_HAA_joint has id 8\n",
"Joint HL_HFE_joint has id 9\n",
"Joint HL_KFE_joint has id 10\n",
"Joint HR_HAA_joint has id 11\n",
"Joint HR_HFE_joint has id 12\n",
"Joint HR_KFE_joint has id 13\n",
"Joint Joint1 has id 14\n",
"Joint Joint2 has id 15\n",
"Joint Joint3 has id 16\n",
"Joint Joint4 has id 17\n",
"Joint Joint5 has id 18\n",
"Joint Joint6 has id 19\n",
"Joint Joint7_1 has id 20\n",
"Joint Joint7_2 has id 21\n"
]
}
],
"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": 116,
"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": 118,
"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)"
]
dockerfile updated and basic ocp example added
2024-12-03 13:41:25 +08:00
}
],
"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
}