2024-03-24 10:42:44 +08:00
|
|
|
{
|
|
|
|
|
"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",
|
2024-03-29 01:32:00 +08:00
|
|
|
"execution_count": 3,
|
2024-03-24 10:42:44 +08:00
|
|
|
"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",
|
2024-03-29 01:32:00 +08:00
|
|
|
" J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)\n",
|
2024-03-24 10:42:44 +08:00
|
|
|
" 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",
|
2024-03-29 01:32:00 +08:00
|
|
|
"execution_count": 4,
|
2024-03-24 10:42:44 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"model = Go2Model()"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-03-29 01:32:00 +08:00
|
|
|
"execution_count": 5,
|
2024-03-24 10:42:44 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"model.update(np.zeros(12), np.zeros(12), np.eye(4), np.zeros(6))"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-03-29 01:32:00 +08:00
|
|
|
"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,
|
2024-03-24 10:42:44 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
2024-03-29 01:32:00 +08:00
|
|
|
"<matplotlib.image.AxesImage at 0x7f87343e1f10>"
|
2024-03-24 10:42:44 +08:00
|
|
|
]
|
|
|
|
|
},
|
2024-03-29 01:32:00 +08:00
|
|
|
"execution_count": 126,
|
2024-03-24 10:42:44 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
2024-03-29 01:32:00 +08:00
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"image/png": "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
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"<Figure size 432x288 with 1 Axes>"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"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": [
|
|
|
|
|
"<matplotlib.image.AxesImage at 0x7f87342cbd30>"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 117,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"image/png": "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
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"<Figure size 432x288 with 1 Axes>"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"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": [
|
|
|
|
|
"<matplotlib.image.AxesImage at 0x7f87343b4af0>"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 136,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"image/png": "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",
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"<Figure size 432x288 with 1 Axes>"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"metadata": {
|
|
|
|
|
"needs_background": "light"
|
|
|
|
|
},
|
|
|
|
|
"output_type": "display_data"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"plt.imshow(Jp)"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 138,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"<matplotlib.image.AxesImage at 0x7f873605d820>"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 138,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"image/png": "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",
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"<Figure size 432x288 with 1 Axes>"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"metadata": {
|
|
|
|
|
"needs_background": "light"
|
|
|
|
|
},
|
|
|
|
|
"output_type": "display_data"
|
2024-03-24 10:42:44 +08:00
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
2024-03-29 01:32:00 +08:00
|
|
|
"plt.imshow(J[0:3,dq_reordering_idx])"
|
2024-03-24 10:42:44 +08:00
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"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
|
|
|
|
|
}
|
