408 lines
13 KiB
Plaintext
408 lines
13 KiB
Plaintext
|
{
|
||
|
"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
|
||
|
}
|