Skip to content

Instantly share code, notes, and snippets.

@sjchoi86

sjchoi86/ik_mano_simple.ipynb

Created August 24, 2025 16:50
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save sjchoi86/926a002be315f748f721be022dc9ad86 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save sjchoi86/926a002be315f748f721be022dc9ad86 to your computer and use it in GitHub Desktop.
Download ZIP
Uploaded from jupyterlab
Display the source blob
Display the rendered blob
Raw
ik_mano_simple.ipynb
{
"cells": [
{
"cell_type": "markdown",
"id": "514d80c7-4d41-40d9-9777-d7184e7aba8e",
"metadata": {},
"source": [
"### Inverse Kinematics for `MANO` hand model"
]
},
{
"cell_type": "code",
"execution_count": 1,
"id": "19eed835-d03a-4a2c-b6a7-281e1b5b6991",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\u001b[32mJupyter Notebook GUI initialized: Retina, inline, qt\u001b[0m\n",
"\u001b[32mFollowing packages are imported:\u001b[0m\n",
"\u001b[32m [mujoco_parser, transforms, ik, torch_chain, utils, bvh_parser, gp, objaverse_tool, leapmotion_tool, smpl_model, gpt_helper, detection]\u001b[0m\n",
"MuJoCo version: (3, 3, 0)\n"
]
}
],
"source": [
"%run ../../package/init_jupyter.py"
]
},
{
"cell_type": "code",
"execution_count": 2,
"id": "1817141f-f295-4437-80b6-ccbead3cb4e9",
"metadata": {
"scrolled": true
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[merge_mjcfs] Merging [3] MJCF files:\n",
" - [0] [../../asset/floor/floor_white_gray.xml]\n",
" - [1] [../../asset/mano_hand/right_hand_simple.xml]\n",
" - [2] [../../asset/ycb/010_potted_meat_can_decomposed/model.xml]\n",
"[merge_mjcfs] Saved merged XML to:[xml/scene_manus_right.xml]\n",
"\n",
"-----------------------------------------------------------------------------\n",
"name:[scene] dt:[0.002] HZ:[500]\n",
" n_qpos:[34] n_qvel:[32] n_qacc:[32] n_ctrl:[0]\n",
" integrator:[IMPLICITFAST]\n",
"\n",
"n_body:[25]\n",
" [0/25] [world] mass:[0.00]kg\n",
" [1/25] [rw_base] mass:[0.00]kg\n",
" [2/25] [rw_offset] mass:[0.30]kg\n",
" [3/25] [rh_index1] mass:[0.01]kg\n",
" [4/25] [rh_index2] mass:[0.00]kg\n",
" [5/25] [rh_index3] mass:[0.00]kg\n",
" [6/25] [rh_middle1] mass:[0.01]kg\n",
" [7/25] [rh_middle2] mass:[0.00]kg\n",
" [8/25] [rh_middle3] mass:[0.00]kg\n",
" [9/25] [rh_ring1] mass:[0.01]kg\n",
" [10/25] [rh_ring1_y] mass:[0.01]kg\n",
" [11/25] [rh_ring1_z] mass:[0.01]kg\n",
" [12/25] [rh_ring2] mass:[0.01]kg\n",
" [13/25] [rh_ring2_y] mass:[0.01]kg\n",
" [14/25] [rh_ring2_z] mass:[0.01]kg\n",
" [15/25] [rh_ring3] mass:[0.00]kg\n",
" [16/25] [rh_ring3_y] mass:[0.00]kg\n",
" [17/25] [rh_ring3_z] mass:[0.00]kg\n",
" [18/25] [rh_pinky1] mass:[0.00]kg\n",
" [19/25] [rh_pinky2] mass:[0.00]kg\n",
" [20/25] [rh_pinky3] mass:[0.00]kg\n",
" [21/25] [rh_thumb1] mass:[0.01]kg\n",
" [22/25] [rh_thumb2] mass:[0.01]kg\n",
" [23/25] [rh_thumb3] mass:[0.01]kg\n",
" [24/25] [object:010_potted_meat_can] mass:[0.76]kg\n",
"body_total_mass:[1.18]kg\n",
"\n",
"n_geom:[63]\n",
"geom_names:['floor', None, None, 'rindex1-rindex2', None, 'rindex2-rindex3', None, 'rindex3-rindex4', None, 'rmiddle1-rmiddle2', None, 'rmiddle2-rmiddle3', None, 'rmiddle3-rmiddle4', None, 'rring1-rring2', None, 'rring2-rring3', None, 'rring3-rring4', None, 'rpinky1-rpinky2', None, 'rpinky2-rpinky3', None, 'rpinky3-rpinky4', None, 'rthumb1-rthumb2', None, 'rthumb2-rthumb3', None, 'rthumb3-rthumb4', None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None]\n",
"\n",
"n_mesh:[47]\n",
"mesh_names:['right_wrist', 'right_index1', 'right_index2', 'right_index3', 'right_middle1', 'right_middle2', 'right_middle3', 'right_pinky1', 'right_pinky2', 'right_pinky3', 'right_ring1', 'right_ring2', 'right_ring3', 'right_thumb1', 'right_thumb2', 'right_thumb3', '010_potted_meat_can', '010_potted_meat_can_collision_0', '010_potted_meat_can_collision_1', '010_potted_meat_can_collision_2', '010_potted_meat_can_collision_3', '010_potted_meat_can_collision_4', '010_potted_meat_can_collision_5', '010_potted_meat_can_collision_6', '010_potted_meat_can_collision_7', '010_potted_meat_can_collision_8', '010_potted_meat_can_collision_9', '010_potted_meat_can_collision_10', '010_potted_meat_can_collision_11', '010_potted_meat_can_collision_12', '010_potted_meat_can_collision_13', '010_potted_meat_can_collision_14', '010_potted_meat_can_collision_15', '010_potted_meat_can_collision_16', '010_potted_meat_can_collision_17', '010_potted_meat_can_collision_18', '010_potted_meat_can_collision_19', '010_potted_meat_can_collision_20', '010_potted_meat_can_collision_21', '010_potted_meat_can_collision_22', '010_potted_meat_can_collision_23', '010_potted_meat_can_collision_24', '010_potted_meat_can_collision_25', '010_potted_meat_can_collision_26', '010_potted_meat_can_collision_27', '010_potted_meat_can_collision_28', '010_potted_meat_can_collision_29']\n",
"\n",
"n_joint:[22]\n",
" [0/22] [floating_base_joint] axis:[0. 0. 1.]\n",
" [1/22] [index1_y] axis:[-0.06 1. 0.01]\n",
" [2/22] [index1_z] axis:[0.23 0. 0.97]\n",
" [3/22] [index2_z] axis:[0.07 0. 1. ]\n",
" [4/22] [index3_z] axis:[-0.01 0. 1. ]\n",
" [5/22] [middle1_y] axis:[-0.02 1. -0. ]\n",
" [6/22] [middle1_z] axis:[-0.04 0. 1. ]\n",
" [7/22] [middle2_z] axis:[-0.18 0. 0.98]\n",
" [8/22] [middle3_z] axis:[-0.17 0. 0.99]\n",
" [9/22] [ring1_y] axis:[-0.04 1. -0.01]\n",
" [10/22] [ring1_z] axis:[-0.31 0. 0.95]\n",
" [11/22] [ring2_z] axis:[-0.18 0. 0.98]\n",
" [12/22] [ring3_z] axis:[-0.31 0. 0.95]\n",
" [13/22] [pinky1_y] axis:[-0.1 0.99 -0.06]\n",
" [14/22] [pinky1_z] axis:[-0.53 0. 0.85]\n",
" [15/22] [pinky2_z] axis:[-0.59 0. 0.81]\n",
" [16/22] [pinky3_z] axis:[-0.55 0. 0.84]\n",
" [17/22] [thumb1_y] axis:[0.46 0.51 0.73]\n",
" [18/22] [thumb1_z] axis:[ 0.63 -0.76 0.13]\n",
" [19/22] [thumb2_z] axis:[ 0.43 -0.82 0.39]\n",
" [20/22] [thumb3_z] axis:[ 0.44 -0.82 0.37]\n",
" [21/22] [None] axis:[0. 0. 1.]\n",
"\n",
"n_dof:[32] (=number of rows of Jacobian)\n",
" [0/32] [None] attached joint:[floating_base_joint] body:[rw_base]\n",
" [1/32] [None] attached joint:[floating_base_joint] body:[rw_base]\n",
" [2/32] [None] attached joint:[floating_base_joint] body:[rw_base]\n",
" [3/32] [None] attached joint:[floating_base_joint] body:[rw_base]\n",
" [4/32] [None] attached joint:[floating_base_joint] body:[rw_base]\n",
" [5/32] [None] attached joint:[floating_base_joint] body:[rw_base]\n",
" [6/32] [None] attached joint:[index1_y] body:[rh_index1]\n",
" [7/32] [None] attached joint:[index1_z] body:[rh_index1]\n",
" [8/32] [None] attached joint:[index2_z] body:[rh_index2]\n",
" [9/32] [None] attached joint:[index3_z] body:[rh_index3]\n",
" [10/32] [None] attached joint:[middle1_y] body:[rh_middle1]\n",
" [11/32] [None] attached joint:[middle1_z] body:[rh_middle1]\n",
" [12/32] [None] attached joint:[middle2_z] body:[rh_middle2]\n",
" [13/32] [None] attached joint:[middle3_z] body:[rh_middle3]\n",
" [14/32] [None] attached joint:[ring1_y] body:[rh_ring1_y]\n",
" [15/32] [None] attached joint:[ring1_z] body:[rh_ring1_z]\n",
" [16/32] [None] attached joint:[ring2_z] body:[rh_ring2_z]\n",
" [17/32] [None] attached joint:[ring3_z] body:[rh_ring3_z]\n",
" [18/32] [None] attached joint:[pinky1_y] body:[rh_pinky1]\n",
" [19/32] [None] attached joint:[pinky1_z] body:[rh_pinky1]\n",
" [20/32] [None] attached joint:[pinky2_z] body:[rh_pinky2]\n",
" [21/32] [None] attached joint:[pinky3_z] body:[rh_pinky3]\n",
" [22/32] [None] attached joint:[thumb1_y] body:[rh_thumb1]\n",
" [23/32] [None] attached joint:[thumb1_z] body:[rh_thumb1]\n",
" [24/32] [None] attached joint:[thumb2_z] body:[rh_thumb2]\n",
" [25/32] [None] attached joint:[thumb3_z] body:[rh_thumb3]\n",
" [26/32] [None] attached joint:[None] body:[object:010_potted_meat_can]\n",
" [27/32] [None] attached joint:[None] body:[object:010_potted_meat_can]\n",
" [28/32] [None] attached joint:[None] body:[object:010_potted_meat_can]\n",
" [29/32] [None] attached joint:[None] body:[object:010_potted_meat_can]\n",
" [30/32] [None] attached joint:[None] body:[object:010_potted_meat_can]\n",
" [31/32] [None] attached joint:[None] body:[object:010_potted_meat_can]\n",
"\n",
"Free joint information. n_free_joint:[2]\n",
" [0/2] [floating_base_joint] body_name_attached:[rw_base]\n",
" [1/2] [None] body_name_attached:[object:010_potted_meat_can]\n",
"\n",
"Revolute joint information. n_rev_joint:[20]\n",
" [0/20] [index1_y] range:[-0.500]~[0.500]\n",
" [1/20] [index1_z] range:[-0.300]~[1.500]\n",
" [2/20] [index2_z] range:[0.000]~[2.000]\n",
" [3/20] [index3_z] range:[0.000]~[1.000]\n",
" [4/20] [middle1_y] range:[-0.500]~[0.500]\n",
" [5/20] [middle1_z] range:[-0.300]~[1.500]\n",
" [6/20] [middle2_z] range:[0.000]~[2.000]\n",
" [7/20] [middle3_z] range:[0.000]~[1.000]\n",
" [8/20] [ring1_y] range:[-0.500]~[0.500]\n",
" [9/20] [ring1_z] range:[-0.300]~[1.500]\n",
" [10/20] [ring2_z] range:[0.000]~[2.000]\n",
" [11/20] [ring3_z] range:[0.000]~[1.000]\n",
" [12/20] [pinky1_y] range:[-0.500]~[0.500]\n",
" [13/20] [pinky1_z] range:[-0.300]~[1.500]\n",
" [14/20] [pinky2_z] range:[0.000]~[2.000]\n",
" [15/20] [pinky3_z] range:[0.000]~[1.000]\n",
" [16/20] [thumb1_y] range:[0.000]~[1.630]\n",
" [17/20] [thumb1_z] range:[-0.920]~[1.020]\n",
" [18/20] [thumb2_z] range:[0.000]~[2.000]\n",
" [19/20] [thumb3_z] range:[0.000]~[1.000]\n",
"\n",
"Prismatic joint information. n_pri_joint:[0]\n",
"\n",
"Control information. n_ctrl:[0]\n",
"\n",
"Camera information. n_cam:[0]\n",
"\n",
"n_sensor:[5]\n",
"sensor_names:['rh_thumb_tip_sensor', 'rh_index_tip_sensor', 'rh_middle_tip_sensor', 'rh_ring_tip_sensor', 'rh_pinky_tip_sensor']\n",
"n_site:[6]\n",
"site_names:['rh_palm_site', 'rh_index_tip_site', 'rh_middle_tip_site', 'rh_ring_tip_site', 'rh_pinky_tip_site', 'rh_thumb_tip_site']\n",
"-----------------------------------------------------------------------------\n",
"env:[scene] reset\n"
]
}
],
"source": [
"xml_path = merge_mjcfs(\n",
" included_mjcf_files=[\n",
" '../../asset/floor/floor_white_gray.xml',\n",
" '../../asset/mano_hand/right_hand_simple.xml',\n",
" '../../asset/ycb/010_potted_meat_can_decomposed/model.xml',\n",
" ],\n",
" output_xml_path = 'xml/scene_manus_right.xml',\n",
")\n",
"env = MuJoCoParserClass(rel_xml_path=xml_path,verbose=True)"
]
},
{
"cell_type": "code",
"execution_count": 41,
"id": "3cf5b2da-6389-442b-a6e8-db53f027715e",
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"['rh_palm_site',\n",
" 'rh_index_tip_site',\n",
" 'rh_middle_tip_site',\n",
" 'rh_ring_tip_site',\n",
" 'rh_pinky_tip_site',\n",
" 'rh_thumb_tip_site']"
]
},
"execution_count": 41,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"env.site_names"
]
},
{
"cell_type": "code",
"execution_count": 64,
"id": "fe1fa8d8-d589-4975-afc5-75de8da415e5",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"env:[scene] reset\n",
"env:[scene] initalize viewer\n",
"\u001b[32mDone.\u001b[0m\n"
]
}
],
"source": [
"# Configuration\n",
"hand_name = 'rw_base' # hand name\n",
"obj_name = 'object:010_potted_meat_can' # object name\n",
"finger_tip_names = [\n",
" 'rh_thumb_tip_site',\n",
" 'rh_index_tip_site',\n",
" 'rh_middle_tip_site',\n",
" 'rh_ring_tip_site',\n",
" 'rh_pinky_tip_site',\n",
"] # finger tip site names\n",
"contact_points_raw = np.array([\n",
" [-0.01789237, -0.05275698, 0.07574528],\n",
" [-0.08266491, -0.03900955, 0.07749628],\n",
" [-0.08018656, -0.01344122, 0.07378599],\n",
" [-0.05262063, -0.00137704, 0.07402063],\n",
" [-2.22321746e-02, -5.53653161e-05, 7.30036543e-02]\n",
"])\n",
"# Initialize\n",
"env.reset()\n",
"cfgs = {'azimuth':147,'distance':0.91,'elevation':-18.38,'lookat':[-0.05,0.01,0.18]}\n",
"env.init_viewer(transparent=True,geomgroup_3=False,**cfgs)\n",
"env.set_pR_base_body(hand_name,p=(0,0,0.4)) # set hand position\n",
"env.set_pR_base_body(obj_name,p=(0,0,0.1),R=rpy2r([0,0,0.2*np.pi])) # set object position\n",
"# Loop\n",
"paused = True\n",
"while env.is_viewer_alive():\n",
" # Update\n",
" if env.is_key_pressed_once(key=glfw.KEY_SPACE): # space pressed\n",
" paused = not paused\n",
" T_obj = env.get_T_body(body_name=obj_name)\n",
" contact_points = tf_view_to(p=contact_points_raw,T_view_to=T_obj)\n",
" # Solve IK\n",
" if not paused:\n",
" ik_info = init_ik_info()\n",
" for t_idx,finger_tip_name in enumerate(finger_tip_names):\n",
" add_ik_info(ik_info,site_name=finger_tip_name,p_trgt=contact_points[t_idx])\n",
" max_ik_tick = 1\n",
" for ik_tick in range(max_ik_tick): # ik loop\n",
" dq,ik_err_stack = get_dq_from_ik_info(\n",
" env = env,\n",
" ik_info = ik_info,\n",
" stepsize = 0.1, #1.0,\n",
" eps = 1e-3,\n",
" th = np.radians(10.0),\n",
" )\n",
" qpos = env.get_qpos() \n",
" mujoco.mj_integratePos(env.model,qpos,dq,1)\n",
" env.forward(q=qpos)\n",
" qpos_coupled = env.get_qpos_force_coupled(\n",
" max_iter = 5,\n",
" joint_names = env.rev_joint_names,\n",
" )\n",
" env.forward(q=qpos_coupled,joint_names=env.rev_joint_names)\n",
" ik_err = np.linalg.norm(ik_err_stack)\n",
" else:\n",
" ik_err = np.inf\n",
" # Render\n",
" env.viewer_text_overlay('Status','%s'%('Paused' if paused else 'Running'))\n",
" env.viewer_text_overlay('IK Error','%.2f'%(ik_err))\n",
" env.plot_global_coordinate_axes()\n",
" # env.plot_contact_info()\n",
" joint_colors = get_colors(env.n_rev_joint,alpha=0.25)\n",
" env.plot_joint_axis(rate=0.25,axis_colors=joint_colors)\n",
" # Plot hand\n",
" env.plot_body_T(hand_name,axis_len=0.05,axis_width=0.002)\n",
" colors = get_colors(n_color=5,cmap_name='rainbow_r',alpha=0.5)\n",
" for t_idx,finger_tip_name in enumerate(finger_tip_names):\n",
" env.plot_sphere(env.get_p_site(finger_tip_name),0.01,colors[t_idx])\n",
" # Plot object\n",
" env.plot_body_T(obj_name,label='Object',axis_len=0.05,axis_width=0.002)\n",
" # Plot object contact points\n",
" for p_idx,p in enumerate(contact_points): env.plot_sphere(p,0.01,colors[p_idx])\n",
" env.render()\n",
"print_green(\"Done.\")"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "e4e89aa9-9d66-4487-af31-3b92b6ca8b1c",
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3 (ipykernel)",
"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.10.13"
}
},
"nbformat": 4,
"nbformat_minor": 5
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment