{ "cells": [ { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import pinocchio as pin" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "dir(pin)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import os\n", "from Go2Py import ASSETS_PATH\n", "urdf_path = os.path.join(ASSETS_PATH, 'urdf/go2.urdf')\n", "urdf_root_path = os.path.join(ASSETS_PATH, 'urdf')" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "robot = pin.RobotWrapper.BuildFromURDF(urdf_path, urdf_root_path, pin.JointModelFreeFlyer())" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "robot_model = robot.model\n", "robot_data = robot.data" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "robot_mass = sum([I.mass for I in robot_model.inertias])\n", "base_frame_name = robot_model.frames[2].name" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "for frame in robot.model.frames:\n", " print(frame.name)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "for name in robot.model.names:\n", " print(name)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "robot_model.getJointId(\"RR_calf_joint\")" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "unitree_joints = [\n", " 'FR_hip_joint',\n", " 'FR_thigh_joint',\n", " 'FR_calf_joint',\n", " 'FL_hip_joint',\n", " 'FL_thigh_joint',\n", " 'FL_calf_joint',\n", " 'RR_hip_joint',\n", " 'RR_thigh_joint',\n", " 'RR_calf_joint',\n", " 'RL_hip_joint',\n", " 'RL_thigh_joint',\n", " 'RL_calf_joint',\n", " ]\n", "pin_joints = [name for name in robot.model.names[2:]]" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "pin_unitree_id_translator = [robot.model.getJointId(joint_name) for joint_name in unitree_joints]" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "pin_unitree_id_translator" ] }, { "cell_type": "code", "execution_count": 3, "metadata": {}, "outputs": [], "source": [ "import os\n", "from Go2Py import ASSETS_PATH\n", "import pinocchio as pin\n", "import numpy as np\n", "urdf_path = os.path.join(ASSETS_PATH, 'urdf/go2.urdf')\n", "urdf_root_path = os.path.join(ASSETS_PATH, 'urdf')\n", "\n", "\n", "class Go2Model:\n", " def __init__(self):\n", " self.robot = pin.RobotWrapper.BuildFromURDF(urdf_path, urdf_root_path, pin.JointModelFreeFlyer())\n", " # Standing joint configuration in Unitree Joint order\n", " self.ef_frames = ['FR_foot', 'FL_foot', 'RR_foot', 'RL_foot']\n", " self.dq_reordering_idx = np.array([0, 1, 2, 3, 4, 5,\\\n", " 9, 10, 11, 6, 7, 8, 15, 16, 17, 12, 13, 14])\n", " self.q_reordering_idx = np.array([9, 10, 11, 6, 7, 8, 15, 16, 17, 12, 13, 14])-6\n", " self.ef_J_ = {}\n", " \n", " def update(self, q, dq, T, v):\n", " q_ = np.hstack([pin.SE3ToXYZQUATtuple(pin.SE3(T)), q[self.q_reordering_idx]])\n", " dq_ = np.hstack([v, dq[self.q_reordering_idx]])\n", " self.robot.computeJointJacobians(q_)\n", " self.robot.framesForwardKinematics(q_)\n", " self.robot.centroidalMomentum(q_,dq_)\n", " self.nle_ = self.robot.nle(q_, dq_)[self.dq_reordering_idx]\n", " self.g_ = self.robot.gravity(q_)[self.dq_reordering_idx]\n", " self.M_ = self.robot.mass(q_)[[self.dq_reordering_idx],[self.dq_reordering_idx]]\n", " self.Minv_ = pin.computeMinverse(self.robot.model, self.robot.data, q_)[[self.dq_reordering_idx],[self.dq_reordering_idx]]\n", " for ef_frame in self.ef_frames:\n", " J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)\n", " self.ef_J_[ef_frame]=J[:, self.dq_reordering_idx]\n", "\n", " def getInfo(self):\n", " return {\n", " 'M':self.M_,\n", " 'Minv':self.Minv_,\n", " 'nle':self.nle_,\n", " 'g':self.g_,\n", " 'J':self.ef_J_,\n", " }" ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [], "source": [ "model = Go2Model()" ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, "outputs": [], "source": [ "model.update(np.zeros(12), np.zeros(12), np.eye(4), np.zeros(6))" ] }, { "cell_type": "code", "execution_count": 8, "metadata": {}, "outputs": [], "source": [ "from Go2Py.sim.mujoco import Go2Sim\n", "robot = Go2Sim()" ] }, { "cell_type": "code", "execution_count": 11, "metadata": {}, "outputs": [], "source": [ "robot.step()\n", "mujoco_params = robot.getDynamicsParams()" ] }, { "cell_type": "code", "execution_count": 67, "metadata": {}, "outputs": [], "source": [ "states = robot.getJointStates()\n", "q = states['q']\n", "dq = states['dq']\n", "trans, quat = robot.getPose()\n", "quat = np.hstack([quat[1:], quat[0]])\n", "q = np.hstack([trans, quat, q])" ] }, { "cell_type": "code", "execution_count": 102, "metadata": {}, "outputs": [], "source": [ "dq_reordering_idx = np.array([0, 1, 2, 3, 4, 5,\\\n", " 9, 10, 11, 6, 7, 8, 15, 16, 17, 12, 13, 14])" ] }, { "cell_type": "code", "execution_count": 125, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "-0.5194361885075903" ] }, "execution_count": 125, "metadata": {}, "output_type": "execute_result" } ], "source": [ "M1 = model.robot.mass(q)[dq_reordering_idx,:]\n", "M1=M1[:,dq_reordering_idx]\n", "# M2 = robot.getDynamicsParams()['M']\n", "(M1-M2).sum()\n", "# M1.shape" ] }, { "cell_type": "code", "execution_count": 126, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "" ] }, "execution_count": 126, "metadata": {}, "output_type": "execute_result" }, { "data": { "image/png": "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", "text/plain": [ "
" ] }, "metadata": { "needs_background": "light" }, "output_type": "display_data" } ], "source": [ "import matplotlib.pyplot as plt\n", "plt.imshow(M1[6:,6:])" ] }, { "cell_type": "code", "execution_count": 117, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "" ] }, "execution_count": 117, "metadata": {}, "output_type": "execute_result" }, { "data": { "image/png": "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", "text/plain": [ "
" ] }, "metadata": { "needs_background": "light" }, "output_type": "display_data" } ], "source": [ "plt.imshow(M2[6:,6:])" ] }, { "cell_type": "code", "execution_count": 129, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "15.206408000000003" ] }, "execution_count": 129, "metadata": {}, "output_type": "execute_result" } ], "source": [ "robot.model.body_mass.sum()" ] }, { "cell_type": "code", "execution_count": 135, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "15.019000000000004" ] }, "execution_count": 135, "metadata": {}, "output_type": "execute_result" } ], "source": [ "s = model.robot.model\n", "sum([i.mass for i in s.inertias])" ] }, { "cell_type": "code", "execution_count": 97, "metadata": {}, "outputs": [], "source": [ "Jp, Jr = robot.getSiteJacobian('FL_foot')\n", "model.robot.computeJointJacobians(q).T\n", "J = model.robot.getFrameJacobian(model.robot.model.getFrameId('FL_foot'), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)" ] }, { "cell_type": "code", "execution_count": 143, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "(0.009002122197148882, 0.13167880433018087)" ] }, "execution_count": 143, "metadata": {}, "output_type": "execute_result" } ], "source": [ "(Jp.T-J[0:3,:].T).sum(),(Jr.T-J[3:,:].T).sum()" ] }, { "cell_type": "code", "execution_count": 136, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "" ] }, "execution_count": 136, "metadata": {}, "output_type": "execute_result" }, { "data": { "image/png": "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", "text/plain": [ "
" ] }, "metadata": { "needs_background": "light" }, "output_type": "display_data" } ], "source": [ "plt.imshow(Jp)" ] }, { "cell_type": "code", "execution_count": 138, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "" ] }, "execution_count": 138, "metadata": {}, "output_type": "execute_result" }, { "data": { "image/png": "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", "text/plain": [ "
" ] }, "metadata": { "needs_background": "light" }, "output_type": "display_data" } ], "source": [ "plt.imshow(J[0:3,dq_reordering_idx])" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "q = np.hstack([pin.SE3ToXYZQUATtuple(pin.SE3(np.eye(4))), np.zeros(12)])\n", "v = np.zeros(18)\n", "robot.computeJointJacobians(q)\n", "robot.framesForwardKinematics(q)\n", "robot.updateGeometryPlacements(q)\n", "nle = robot.nle(q, v)\n", "g = robot.gravity(q)\n", "robot.centroidalMomentum(q,v)\n", "pin.computeMinverse(q)\n" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "robot.frameJacobian(q, robot.model.getFrameId('FL_Foot'))" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "robot.model.frames[2]" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "robot.getFrameJacobian(robot.model.getFrameId('FL_Foot'), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[0:6,0:6]" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "pin.se3ToXYZQUATtuple(pin.SE3(np.eye(4)))" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "pin.SE3(np.eye(4))" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "pin.rnea(robot.model, robot.data, q, v, v).shape" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "M = pin.crba(robot.model, robot.data, q)" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "M.T-M" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "kernelspec": { "display_name": "Python 3", "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.10" } }, "nbformat": 4, "nbformat_minor": 2 }