{ "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": 21, "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)\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": 22, "metadata": {}, "outputs": [], "source": [ "model = Go2Model()" ] }, { "cell_type": "code", "execution_count": 23, "metadata": {}, "outputs": [], "source": [ "model.update(np.zeros(12), np.zeros(12), np.eye(4), np.zeros(6))" ] }, { "cell_type": "code", "execution_count": 25, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "{'FR_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , 0.142 , 0. ,\n", " -0.426 , -0.213 , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 1. , 0. , 0.426 , 0. , 0.1934, 0.426 ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 1. , -0.142 , -0.1934, 0. , -0.0955,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 0. , 1. , 0. , 0. , 1. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n", " 1. , 1. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ]]),\n", " 'FL_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , -0.142 , 0. ,\n", " 0. , 0. , 0. , -0.426 , -0.213 , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 1. , 0. , 0.426 , 0. , 0.1934, 0. ,\n", " 0. , 0. , 0.426 , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 1. , 0.142 , -0.1934, 0. , 0. ,\n", " 0. , 0. , 0.0955, 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 0. , 1. , 0. , 0. , 0. ,\n", " 0. , 0. , 1. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n", " 0. , 0. , 0. , 1. , 1. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ]]),\n", " 'RR_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , 0.142 , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , -0.426 ,\n", " -0.213 , 0. , 0. , 0. ],\n", " [ 0. , 1. , 0. , 0.426 , 0. , -0.1934, 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0.426 , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 1. , -0.142 , 0.1934, 0. , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , -0.0955, 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 0. , 1. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n", " 0. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 1. ,\n", " 1. , 0. , 0. , 0. ],\n", " [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ]]),\n", " 'RL_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , -0.142 , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , -0.426 , -0.213 ],\n", " [ 0. , 1. , 0. , 0.426 , 0. , -0.1934, 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0.426 , 0. , 0. ],\n", " [ 0. , 0. , 1. , 0.142 , 0.1934, 0. , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0.0955, 0. , 0. ],\n", " [ 0. , 0. , 0. , 1. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 1. , 0. , 0. ],\n", " [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 1. , 1. ],\n", " [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n", " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", " 0. , 0. , 0. , 0. ]])}" ] }, "execution_count": 25, "metadata": {}, "output_type": "execute_result" } ], "source": [ "model.getInfo()['J']" ] }, { "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 }