Skip to content

Instantly share code, notes, and snippets.

@sjchoi86

sjchoi86/coupled_ik_allex.ipynb

Created September 4, 2025 15:10
Show Gist options

  • Select an option

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

Select an option

Learn more about clone URLs
Save sjchoi86/f5b0374f3d6e09752c27a494b6ae3f35 to your computer and use it in GitHub Desktop.
Download ZIP
Uploaded from jupyterlab
Display the source blob
Display the rendered blob
Raw
coupled_ik_allex.ipynb
{
"cells": [
{
"cell_type": "markdown",
"id": "2b73281b-31b5-42b1-a033-07a943ea6985",
"metadata": {},
"source": [
"### Coupled IK `EXPLAINED!`"
]
},
{
"cell_type": "code",
"execution_count": 1,
"id": "e4027888-e58a-4e2e-b9bf-e171a057c27f",
"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": "5c0a6e93-0417-4106-823b-68b1975593d5",
"metadata": {
"scrolled": true
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[merge_mjcfs] Merging [4] MJCF files:\n",
" - [0] [../../asset/floor/floor_white_gray.xml]\n",
" - [1] [../../asset/allex/allex_contact_sensor.xml]\n",
" - [2] [object/table/base_table.xml]\n",
" - [3] [object/daruma/daruma/daruma.xml]\n",
"[merge_mjcfs] Saved merged XML to:[xml/scene_allex.xml]\n",
"\n",
"-----------------------------------------------------------------------------\n",
"name:[scene] dt:[0.002] HZ:[500]\n",
" n_qpos:[67] n_qvel:[66] n_qacc:[66] n_ctrl:[48]\n",
" integrator:[IMPLICITFAST]\n",
"\n",
"n_body:[69]\n",
" [0/69] [world] mass:[0.00]kg\n",
" [1/69] [Waist_Base] mass:[3.00]kg\n",
" [2/69] [Waist_Yaw_link] mass:[2.00]kg\n",
" [3/69] [Waist_Pitch_Lower_link] mass:[2.00]kg\n",
" [4/69] [Waist_Pitch_Upper_link] mass:[4.40]kg\n",
" [5/69] [Neck_Yaw_link] mass:[0.50]kg\n",
" [6/69] [Head_Tilt_link] mass:[0.50]kg\n",
" [7/69] [Camera_Body] mass:[0.00]kg\n",
" [8/69] [L_Shoulder_Pitch_link] mass:[0.92]kg\n",
" [9/69] [L_Shoulder_Roll_link] mass:[2.50]kg\n",
" [10/69] [L_Upperarm_link] mass:[2.65]kg\n",
" [11/69] [L_Elbow_link] mass:[0.46]kg\n",
" [12/69] [L_Forearm_link] mass:[2.19]kg\n",
" [13/69] [L_Wrist_Roll_link] mass:[0.04]kg\n",
" [14/69] [L_Wrist_Pitch_link] mass:[0.06]kg\n",
" [15/69] [Left_Hand_Palm] mass:[0.55]kg\n",
" [16/69] [Left_Hand_base] mass:[0.00]kg\n",
" [17/69] [L_Hand_Thumb_Yaw] mass:[0.03]kg\n",
" [18/69] [L_Hand_Thumb_Proximal] mass:[0.03]kg\n",
" [19/69] [L_Hand_Thumb_Middle] mass:[0.01]kg\n",
" [20/69] [L_Hand_Thumb_Distal] mass:[0.01]kg\n",
" [21/69] [L_Hand_Index_Roll] mass:[0.01]kg\n",
" [22/69] [L_Hand_Index_Proximal] mass:[0.02]kg\n",
" [23/69] [L_Hand_Index_Middle] mass:[0.01]kg\n",
" [24/69] [L_Hand_Index_Distal] mass:[0.01]kg\n",
" [25/69] [L_Hand_Middle_Roll] mass:[0.01]kg\n",
" [26/69] [L_Hand_Middle_Proximal] mass:[0.02]kg\n",
" [27/69] [L_Hand_Middle_Middle] mass:[0.01]kg\n",
" [28/69] [L_Hand_Middle_Distal] mass:[0.01]kg\n",
" [29/69] [L_Hand_Ring_Roll] mass:[0.01]kg\n",
" [30/69] [L_Hand_Ring_Proximal] mass:[0.02]kg\n",
" [31/69] [L_Hand_Ring_Middle] mass:[0.01]kg\n",
" [32/69] [L_Hand_Ring_Distal] mass:[0.01]kg\n",
" [33/69] [L_Hand_Little_Roll] mass:[0.01]kg\n",
" [34/69] [L_Hand_Little_Proximal] mass:[0.02]kg\n",
" [35/69] [L_Hand_Little_Middle] mass:[0.01]kg\n",
" [36/69] [L_Hand_Little_Distal] mass:[0.01]kg\n",
" [37/69] [R_Shoulder_Pitch_link] mass:[0.92]kg\n",
" [38/69] [R_Shoulder_Roll_link] mass:[2.50]kg\n",
" [39/69] [R_Upperarm_link] mass:[2.65]kg\n",
" [40/69] [R_Elbow_link] mass:[0.46]kg\n",
" [41/69] [R_Forearm_link] mass:[2.19]kg\n",
" [42/69] [R_Wrist_Roll_link] mass:[0.04]kg\n",
" [43/69] [R_Wrist_Pitch_link] mass:[0.06]kg\n",
" [44/69] [Right_Hand_Palm] mass:[0.55]kg\n",
" [45/69] [Right_Hand_base] mass:[0.00]kg\n",
" [46/69] [R_Hand_Thumb_Yaw] mass:[0.03]kg\n",
" [47/69] [R_Hand_Thumb_Proximal] mass:[0.03]kg\n",
" [48/69] [R_Hand_Thumb_Middle] mass:[0.01]kg\n",
" [49/69] [R_Hand_Thumb_Distal] mass:[0.01]kg\n",
" [50/69] [R_Hand_Index_Roll] mass:[0.01]kg\n",
" [51/69] [R_Hand_Index_Proximal] mass:[0.02]kg\n",
" [52/69] [R_Hand_Index_Middle] mass:[0.01]kg\n",
" [53/69] [R_Hand_Index_Distal] mass:[0.01]kg\n",
" [54/69] [R_Hand_Middle_Roll] mass:[0.01]kg\n",
" [55/69] [R_Hand_Middle_Proximal] mass:[0.02]kg\n",
" [56/69] [R_Hand_Middle_Middle] mass:[0.01]kg\n",
" [57/69] [R_Hand_Middle_Distal] mass:[0.01]kg\n",
" [58/69] [R_Hand_Ring_Roll] mass:[0.01]kg\n",
" [59/69] [R_Hand_Ring_Proximal] mass:[0.02]kg\n",
" [60/69] [R_Hand_Ring_Middle] mass:[0.01]kg\n",
" [61/69] [R_Hand_Ring_Distal] mass:[0.01]kg\n",
" [62/69] [R_Hand_Little_Roll] mass:[0.01]kg\n",
" [63/69] [R_Hand_Little_Proximal] mass:[0.02]kg\n",
" [64/69] [R_Hand_Little_Middle] mass:[0.01]kg\n",
" [65/69] [R_Hand_Little_Distal] mass:[0.01]kg\n",
" [66/69] [Waist_Pitch_Back_link] mass:[1.00]kg\n",
" [67/69] [base_table] mass:[900.00]kg\n",
" [68/69] [daruma] mass:[0.98]kg\n",
"body_total_mass:[933.62]kg\n",
"\n",
"n_geom:[294]\n",
"geom_names:['floor', 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, 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, 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, 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, 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, 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, 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, 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, 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, None, None, None, None, None, 'base_table', None, None, None, None, None, None, None, None]\n",
"\n",
"n_mesh:[97]\n",
"mesh_names:['LIMS4-ALLEX_Hip_Base', 'LIMS4-ALLEX_Hip_Dummy', 'LIMS4-ALLEX_Waist_Front_Link', 'LIMS4-ALLEX_Waist_Back_Link', 'LIMS4-ALLEX_Chest_Frame', 'LIMS4-ALLEX_Neck_Frame', 'LIMS4-ALLEX_Head_Back_Frame', 'LIMS4-ALLEX_Head_Front_Frame', 'ALLEX_Left_Shoulder_Pitch_Frame', 'ALLEX_Left_Shoulder_Yaw_Frame', 'ALLEX_Left_Shoulder_Yaw_Frame_Collision_1', 'ALLEX_Left_Shoulder_Yaw_Frame_Collision_2', 'ALLEX_Left_Upperarm_Frame', 'ALLEX_Left_Upperarm_Cover_Collision1', 'ALLEX_Left_Upperarm_Cover_Collision2', 'ALLEX_Left_Upperarm_Cover_Collision3', 'ALLEX_Left_Elbow_Frame', 'ALLEX_Left_Forearm_Frame', 'ALLEX_Left_Forearm_Base_Cover', 'ALLEX_Left_Forearm_Middle_Frame', 'ALLEX_Left_Forearm_Lower_Cover', 'ALLEX_Left_Forearm_Cover', 'ALLEX_Left_Wrist_Roll_Frame', 'ALLEX_Left_Wrist_Pitch_Frame', 'ALLEX_Left_Hand_Palm', 'ALLEX_Left_Hand_Palm_Thumb', 'ALLEX_Left_Hand_Palm_Pad', 'ALLEX_Left_Hand_Palm_Pad1', 'ALLEX_Left_Hand_Palm_Pad2', 'ALLEX_Left_Hand_Palm_Thumb1', 'ALLEX_Left_Hand_Palm_Thumb2', 'ALLEX_Left_Hand_Palm_Thumb3', 'ALLEX_Left_Hand_Thumb_Pitch_Pivot', 'ALLEX_Left_Hand_Thumb_Proximal_Frame', 'ALLEX_Left_Hand_Thumb_Proximal_Cover', 'ALLEX_Left_Hand_Thumb_Proximal_Pad', 'ALLEX_Left_Hand_Thumb_Proximal_Link1', 'ALLEX_Left_Hand_Thumb_Proximal_Link2', 'ALLEX_Left_Hand_Thumb_Middle_Frame', 'ALLEX_Left_Hand_Thumb_Middle_Pad', 'ALLEX_Left_Hand_Thumb_Middle_Link', 'ALLEX_Left_Hand_Thumb_Distal_Frame', 'ALLEX_Left_Hand_Thumb_Distal_Pad', 'ALLEX_Right_Shoulder_Pitch_Frame', 'ALLEX_Right_Shoulder_Yaw_Frame', 'ALLEX_Right_Shoulder_Yaw_Frame_Collision_1', 'ALLEX_Right_Shoulder_Yaw_Frame_Collision_2', 'ALLEX_Right_Upperarm_Frame', 'ALLEX_Right_Upperarm_Cover_Collision1', 'ALLEX_Right_Upperarm_Cover_Collision2', 'ALLEX_Right_Upperarm_Cover_Collision3', 'ALLEX_Right_Elbow_Frame', 'ALLEX_Right_Forearm_Frame', 'ALLEX_Right_Forearm_Base_Cover', 'ALLEX_Right_Forearm_Lower_Cover', 'ALLEX_Right_Forearm_Middle_Frame', 'ALLEX_Right_Forearm_Cover', 'ALLEX_Right_Wrist_Roll_Frame', 'ALLEX_Right_Wrist_Pitch_Frame', 'ALLEX_Right_Hand_Palm', 'ALLEX_Right_Hand_Palm_Thumb', 'ALLEX_Right_Hand_Palm_Pad', 'ALLEX_Right_Hand_Palm_Pad1', 'ALLEX_Right_Hand_Palm_Pad2', 'ALLEX_Right_Hand_Palm_Thumb1', 'ALLEX_Right_Hand_Palm_Thumb2', 'ALLEX_Right_Hand_Palm_Thumb3', 'ALLEX_Right_Hand_Thumb_Pitch_Pivot', 'ALLEX_Right_Hand_Thumb_Proximal_Frame', 'ALLEX_Right_Hand_Thumb_Proximal_Cover', 'ALLEX_Right_Hand_Thumb_Proximal_Pad', 'ALLEX_Right_Hand_Thumb_Proximal_Link1', 'ALLEX_Right_Hand_Thumb_Proximal_Link2', 'ALLEX_Right_Hand_Thumb_Middle_Frame', 'ALLEX_Right_Hand_Thumb_Middle_Pad', 'ALLEX_Right_Hand_Thumb_Middle_Link', 'ALLEX_Right_Hand_Thumb_Distal_Frame', 'ALLEX_Right_Hand_Thumb_Distal_Pad', 'ALLEX_Hand_Proximal_Roll', 'ALLEX_Hand_Proximal', 'ALLEX_Hand_Proximal_Frame1', 'ALLEX_Hand_Proximal_Frame2', 'ALLEX_Hand_Proximal_Cover', 'ALLEX_Hand_Proximal_Pad', 'ALLEX_Hand_Middle_Frame1', 'ALLEX_Hand_Middle_Frame2', 'ALLEX_Hand_Middle_Pad', 'ALLEX_Hand_Distal_Frame1', 'ALLEX_Hand_Distal_Pad', 'daruma', 'daruma_collision_0', 'daruma_collision_1', 'daruma_collision_2', 'daruma_collision_3', 'daruma_collision_4', 'daruma_collision_5', 'daruma_collision_6']\n",
"\n",
"n_joint:[61]\n",
" [0/61] [Waist_Yaw_Joint] axis:[0. 0. 1.]\n",
" [1/61] [Waist_Pitch_Lower_Joint] axis:[0. 1. 0.]\n",
" [2/61] [Waist_Pitch_Upper_Joint] axis:[0. 1. 0.]\n",
" [3/61] [Head_Pan_Joint] axis:[0. 0. 1.]\n",
" [4/61] [Head_Tilt_Joint] axis:[0. 1. 0.]\n",
" [5/61] [L_Shoudler_Pitch_Joint] axis:[0. 1. 0.]\n",
" [6/61] [L_Shoudler_Roll_Joint] axis:[1. 0. 0.]\n",
" [7/61] [L_Shoudler_Yaw_Joint] axis:[0. 0. 1.]\n",
" [8/61] [L_Elbow_Joint] axis:[0. 1. 0.]\n",
" [9/61] [L_Wrist_Yaw_Joint] axis:[0. 0. 1.]\n",
" [10/61] [L_Wrist_Roll_Joint] axis:[1. 0. 0.]\n",
" [11/61] [L_Wrist_Pitch_Joint] axis:[0. 1. 0.]\n",
" [12/61] [L_Thumb_Yaw_Joint] axis:[ 0. 0. -1.]\n",
" [13/61] [L_Thumb_CMC_Joint] axis:[-1. 0. 0.]\n",
" [14/61] [L_Thumb_MCP_Joint] axis:[-1. 0. 0.]\n",
" [15/61] [L_Thumb_IP_Joint] axis:[-1. 0. 0.]\n",
" [16/61] [L_Index_Roll_Joint] axis:[1. 0. 0.]\n",
" [17/61] [L_Index_MCP_Joint] axis:[ 0. -1. 0.]\n",
" [18/61] [L_Index_PIP_Joint] axis:[ 0. -1. 0.]\n",
" [19/61] [L_Index_DIP_Joint] axis:[ 0. -1. 0.]\n",
" [20/61] [L_Middle_Roll_Joint] axis:[1. 0. 0.]\n",
" [21/61] [L_Middle_MCP_Joint] axis:[ 0. -1. 0.]\n",
" [22/61] [L_Middle_PIP_Joint] axis:[ 0. -1. 0.]\n",
" [23/61] [L_Middle_DIP_Joint] axis:[ 0. -1. 0.]\n",
" [24/61] [L_Ring_Roll_Joint] axis:[1. 0. 0.]\n",
" [25/61] [L_Ring_MCP_Joint] axis:[ 0. -1. 0.]\n",
" [26/61] [L_Ring_PIP_Joint] axis:[ 0. -1. 0.]\n",
" [27/61] [L_Ring_DIP_Joint] axis:[ 0. -1. 0.]\n",
" [28/61] [L_Little_Roll_Joint] axis:[1. 0. 0.]\n",
" [29/61] [L_Little_MCP_Joint] axis:[ 0. -1. 0.]\n",
" [30/61] [L_Little_PIP_Joint] axis:[ 0. -1. 0.]\n",
" [31/61] [L_Little_DIP_Joint] axis:[ 0. -1. 0.]\n",
" [32/61] [R_Shoudler_Pitch_Joint] axis:[0. 1. 0.]\n",
" [33/61] [R_Shoudler_Roll_Joint] axis:[1. 0. 0.]\n",
" [34/61] [R_Shoudler_Yaw_Joint] axis:[0. 0. 1.]\n",
" [35/61] [R_Elbow_Joint] axis:[0. 1. 0.]\n",
" [36/61] [R_Wrist_Yaw_Joint] axis:[0. 0. 1.]\n",
" [37/61] [R_Wrist_Roll_Joint] axis:[1. 0. 0.]\n",
" [38/61] [R_Wrist_Pitch_Joint] axis:[0. 1. 0.]\n",
" [39/61] [R_Thumb_Yaw_Joint] axis:[ 0. 0. -1.]\n",
" [40/61] [R_Thumb_CMC_Joint] axis:[1. 0. 0.]\n",
" [41/61] [R_Thumb_MCP_Joint] axis:[1. 0. 0.]\n",
" [42/61] [R_Thumb_IP_Joint] axis:[1. 0. 0.]\n",
" [43/61] [R_Index_Roll_Joint] axis:[1. 0. 0.]\n",
" [44/61] [R_Index_MCP_Joint] axis:[ 0. -1. 0.]\n",
" [45/61] [R_Index_PIP_Joint] axis:[ 0. -1. 0.]\n",
" [46/61] [R_Index_DIP_Joint] axis:[ 0. -1. 0.]\n",
" [47/61] [R_Middle_Roll_Joint] axis:[1. 0. 0.]\n",
" [48/61] [R_Middle_MCP_Joint] axis:[ 0. -1. 0.]\n",
" [49/61] [R_Middle_PIP_Joint] axis:[ 0. -1. 0.]\n",
" [50/61] [R_Middle_DIP_Joint] axis:[ 0. -1. 0.]\n",
" [51/61] [R_Ring_Roll_Joint] axis:[1. 0. 0.]\n",
" [52/61] [R_Ring_MCP_Joint] axis:[ 0. -1. 0.]\n",
" [53/61] [R_Ring_PIP_Joint] axis:[ 0. -1. 0.]\n",
" [54/61] [R_Ring_DIP_Joint] axis:[ 0. -1. 0.]\n",
" [55/61] [R_Little_Roll_Joint] axis:[1. 0. 0.]\n",
" [56/61] [R_Little_MCP_Joint] axis:[ 0. -1. 0.]\n",
" [57/61] [R_Little_PIP_Joint] axis:[ 0. -1. 0.]\n",
" [58/61] [R_Little_DIP_Joint] axis:[ 0. -1. 0.]\n",
" [59/61] [Waist_Pitch_Dummy_Joint] axis:[0. 1. 0.]\n",
" [60/61] [root_free] axis:[0. 0. 1.]\n",
"\n",
"n_dof:[66] (=number of rows of Jacobian)\n",
" [0/66] [None] attached joint:[Waist_Yaw_Joint] body:[Waist_Yaw_link]\n",
" [1/66] [None] attached joint:[Waist_Pitch_Lower_Joint] body:[Waist_Pitch_Lower_link]\n",
" [2/66] [None] attached joint:[Waist_Pitch_Upper_Joint] body:[Waist_Pitch_Upper_link]\n",
" [3/66] [None] attached joint:[Head_Pan_Joint] body:[Neck_Yaw_link]\n",
" [4/66] [None] attached joint:[Head_Tilt_Joint] body:[Head_Tilt_link]\n",
" [5/66] [None] attached joint:[L_Shoudler_Pitch_Joint] body:[L_Shoulder_Pitch_link]\n",
" [6/66] [None] attached joint:[L_Shoudler_Roll_Joint] body:[L_Shoulder_Roll_link]\n",
" [7/66] [None] attached joint:[L_Shoudler_Yaw_Joint] body:[L_Upperarm_link]\n",
" [8/66] [None] attached joint:[L_Elbow_Joint] body:[L_Elbow_link]\n",
" [9/66] [None] attached joint:[L_Wrist_Yaw_Joint] body:[L_Forearm_link]\n",
" [10/66] [None] attached joint:[L_Wrist_Roll_Joint] body:[L_Wrist_Roll_link]\n",
" [11/66] [None] attached joint:[L_Wrist_Pitch_Joint] body:[L_Wrist_Pitch_link]\n",
" [12/66] [None] attached joint:[L_Thumb_Yaw_Joint] body:[L_Hand_Thumb_Yaw]\n",
" [13/66] [None] attached joint:[L_Thumb_CMC_Joint] body:[L_Hand_Thumb_Proximal]\n",
" [14/66] [None] attached joint:[L_Thumb_MCP_Joint] body:[L_Hand_Thumb_Middle]\n",
" [15/66] [None] attached joint:[L_Thumb_IP_Joint] body:[L_Hand_Thumb_Distal]\n",
" [16/66] [None] attached joint:[L_Index_Roll_Joint] body:[L_Hand_Index_Roll]\n",
" [17/66] [None] attached joint:[L_Index_MCP_Joint] body:[L_Hand_Index_Proximal]\n",
" [18/66] [None] attached joint:[L_Index_PIP_Joint] body:[L_Hand_Index_Middle]\n",
" [19/66] [None] attached joint:[L_Index_DIP_Joint] body:[L_Hand_Index_Distal]\n",
" [20/66] [None] attached joint:[L_Middle_Roll_Joint] body:[L_Hand_Middle_Roll]\n",
" [21/66] [None] attached joint:[L_Middle_MCP_Joint] body:[L_Hand_Middle_Proximal]\n",
" [22/66] [None] attached joint:[L_Middle_PIP_Joint] body:[L_Hand_Middle_Middle]\n",
" [23/66] [None] attached joint:[L_Middle_DIP_Joint] body:[L_Hand_Middle_Distal]\n",
" [24/66] [None] attached joint:[L_Ring_Roll_Joint] body:[L_Hand_Ring_Roll]\n",
" [25/66] [None] attached joint:[L_Ring_MCP_Joint] body:[L_Hand_Ring_Proximal]\n",
" [26/66] [None] attached joint:[L_Ring_PIP_Joint] body:[L_Hand_Ring_Middle]\n",
" [27/66] [None] attached joint:[L_Ring_DIP_Joint] body:[L_Hand_Ring_Distal]\n",
" [28/66] [None] attached joint:[L_Little_Roll_Joint] body:[L_Hand_Little_Roll]\n",
" [29/66] [None] attached joint:[L_Little_MCP_Joint] body:[L_Hand_Little_Proximal]\n",
" [30/66] [None] attached joint:[L_Little_PIP_Joint] body:[L_Hand_Little_Middle]\n",
" [31/66] [None] attached joint:[L_Little_DIP_Joint] body:[L_Hand_Little_Distal]\n",
" [32/66] [None] attached joint:[R_Shoudler_Pitch_Joint] body:[R_Shoulder_Pitch_link]\n",
" [33/66] [None] attached joint:[R_Shoudler_Roll_Joint] body:[R_Shoulder_Roll_link]\n",
" [34/66] [None] attached joint:[R_Shoudler_Yaw_Joint] body:[R_Upperarm_link]\n",
" [35/66] [None] attached joint:[R_Elbow_Joint] body:[R_Elbow_link]\n",
" [36/66] [None] attached joint:[R_Wrist_Yaw_Joint] body:[R_Forearm_link]\n",
" [37/66] [None] attached joint:[R_Wrist_Roll_Joint] body:[R_Wrist_Roll_link]\n",
" [38/66] [None] attached joint:[R_Wrist_Pitch_Joint] body:[R_Wrist_Pitch_link]\n",
" [39/66] [None] attached joint:[R_Thumb_Yaw_Joint] body:[R_Hand_Thumb_Yaw]\n",
" [40/66] [None] attached joint:[R_Thumb_CMC_Joint] body:[R_Hand_Thumb_Proximal]\n",
" [41/66] [None] attached joint:[R_Thumb_MCP_Joint] body:[R_Hand_Thumb_Middle]\n",
" [42/66] [None] attached joint:[R_Thumb_IP_Joint] body:[R_Hand_Thumb_Distal]\n",
" [43/66] [None] attached joint:[R_Index_Roll_Joint] body:[R_Hand_Index_Roll]\n",
" [44/66] [None] attached joint:[R_Index_MCP_Joint] body:[R_Hand_Index_Proximal]\n",
" [45/66] [None] attached joint:[R_Index_PIP_Joint] body:[R_Hand_Index_Middle]\n",
" [46/66] [None] attached joint:[R_Index_DIP_Joint] body:[R_Hand_Index_Distal]\n",
" [47/66] [None] attached joint:[R_Middle_Roll_Joint] body:[R_Hand_Middle_Roll]\n",
" [48/66] [None] attached joint:[R_Middle_MCP_Joint] body:[R_Hand_Middle_Proximal]\n",
" [49/66] [None] attached joint:[R_Middle_PIP_Joint] body:[R_Hand_Middle_Middle]\n",
" [50/66] [None] attached joint:[R_Middle_DIP_Joint] body:[R_Hand_Middle_Distal]\n",
" [51/66] [None] attached joint:[R_Ring_Roll_Joint] body:[R_Hand_Ring_Roll]\n",
" [52/66] [None] attached joint:[R_Ring_MCP_Joint] body:[R_Hand_Ring_Proximal]\n",
" [53/66] [None] attached joint:[R_Ring_PIP_Joint] body:[R_Hand_Ring_Middle]\n",
" [54/66] [None] attached joint:[R_Ring_DIP_Joint] body:[R_Hand_Ring_Distal]\n",
" [55/66] [None] attached joint:[R_Little_Roll_Joint] body:[R_Hand_Little_Roll]\n",
" [56/66] [None] attached joint:[R_Little_MCP_Joint] body:[R_Hand_Little_Proximal]\n",
" [57/66] [None] attached joint:[R_Little_PIP_Joint] body:[R_Hand_Little_Middle]\n",
" [58/66] [None] attached joint:[R_Little_DIP_Joint] body:[R_Hand_Little_Distal]\n",
" [59/66] [None] attached joint:[Waist_Pitch_Dummy_Joint] body:[Waist_Pitch_Back_link]\n",
" [60/66] [None] attached joint:[root_free] body:[daruma]\n",
" [61/66] [None] attached joint:[root_free] body:[daruma]\n",
" [62/66] [None] attached joint:[root_free] body:[daruma]\n",
" [63/66] [None] attached joint:[root_free] body:[daruma]\n",
" [64/66] [None] attached joint:[root_free] body:[daruma]\n",
" [65/66] [None] attached joint:[root_free] body:[daruma]\n",
"\n",
"Free joint information. n_free_joint:[1]\n",
" [0/1] [root_free] body_name_attached:[daruma]\n",
"\n",
"Revolute joint information. n_rev_joint:[60]\n",
" [0/60] [Waist_Yaw_Joint] range:[-2.793]~[2.793]\n",
" [1/60] [Waist_Pitch_Lower_Joint] range:[-0.524]~[0.785]\n",
" [2/60] [Waist_Pitch_Upper_Joint] range:[-0.785]~[0.524]\n",
" [3/60] [Head_Pan_Joint] range:[-1.571]~[1.571]\n",
" [4/60] [Head_Tilt_Joint] range:[-0.785]~[0.785]\n",
" [5/60] [L_Shoudler_Pitch_Joint] range:[-3.316]~[1.745]\n",
" [6/60] [L_Shoudler_Roll_Joint] range:[-0.087]~[3.752]\n",
" [7/60] [L_Shoudler_Yaw_Joint] range:[-3.316]~[1.745]\n",
" [8/60] [L_Elbow_Joint] range:[-2.880]~[0.087]\n",
" [9/60] [L_Wrist_Yaw_Joint] range:[-4.800]~[0.611]\n",
" [10/60] [L_Wrist_Roll_Joint] range:[-0.873]~[0.873]\n",
" [11/60] [L_Wrist_Pitch_Joint] range:[-1.396]~[1.396]\n",
" [12/60] [L_Thumb_Yaw_Joint] range:[0.000]~[2.618]\n",
" [13/60] [L_Thumb_CMC_Joint] range:[0.000]~[1.571]\n",
" [14/60] [L_Thumb_MCP_Joint] range:[0.000]~[1.571]\n",
" [15/60] [L_Thumb_IP_Joint] range:[0.000]~[1.222]\n",
" [16/60] [L_Index_Roll_Joint] range:[-0.524]~[0.524]\n",
" [17/60] [L_Index_MCP_Joint] range:[-0.175]~[1.571]\n",
" [18/60] [L_Index_PIP_Joint] range:[0.000]~[1.745]\n",
" [19/60] [L_Index_DIP_Joint] range:[0.000]~[1.449]\n",
" [20/60] [L_Middle_Roll_Joint] range:[-0.524]~[0.524]\n",
" [21/60] [L_Middle_MCP_Joint] range:[-0.175]~[1.571]\n",
" [22/60] [L_Middle_PIP_Joint] range:[0.000]~[1.745]\n",
" [23/60] [L_Middle_DIP_Joint] range:[0.000]~[1.449]\n",
" [24/60] [L_Ring_Roll_Joint] range:[-0.524]~[0.524]\n",
" [25/60] [L_Ring_MCP_Joint] range:[-0.175]~[1.571]\n",
" [26/60] [L_Ring_PIP_Joint] range:[0.000]~[1.745]\n",
" [27/60] [L_Ring_DIP_Joint] range:[0.000]~[1.449]\n",
" [28/60] [L_Little_Roll_Joint] range:[-0.524]~[0.524]\n",
" [29/60] [L_Little_MCP_Joint] range:[-0.175]~[1.571]\n",
" [30/60] [L_Little_PIP_Joint] range:[0.000]~[1.745]\n",
" [31/60] [L_Little_DIP_Joint] range:[0.000]~[1.449]\n",
" [32/60] [R_Shoudler_Pitch_Joint] range:[-3.316]~[1.745]\n",
" [33/60] [R_Shoudler_Roll_Joint] range:[-3.752]~[0.087]\n",
" [34/60] [R_Shoudler_Yaw_Joint] range:[-1.745]~[3.316]\n",
" [35/60] [R_Elbow_Joint] range:[-2.880]~[0.087]\n",
" [36/60] [R_Wrist_Yaw_Joint] range:[-0.611]~[4.800]\n",
" [37/60] [R_Wrist_Roll_Joint] range:[-0.873]~[0.873]\n",
" [38/60] [R_Wrist_Pitch_Joint] range:[-1.396]~[1.396]\n",
" [39/60] [R_Thumb_Yaw_Joint] range:[-2.618]~[0.000]\n",
" [40/60] [R_Thumb_CMC_Joint] range:[0.000]~[1.571]\n",
" [41/60] [R_Thumb_MCP_Joint] range:[0.000]~[1.571]\n",
" [42/60] [R_Thumb_IP_Joint] range:[0.000]~[1.222]\n",
" [43/60] [R_Index_Roll_Joint] range:[-0.524]~[0.524]\n",
" [44/60] [R_Index_MCP_Joint] range:[-0.175]~[1.571]\n",
" [45/60] [R_Index_PIP_Joint] range:[0.000]~[1.745]\n",
" [46/60] [R_Index_DIP_Joint] range:[0.000]~[1.449]\n",
" [47/60] [R_Middle_Roll_Joint] range:[-0.524]~[0.524]\n",
" [48/60] [R_Middle_MCP_Joint] range:[-0.175]~[1.571]\n",
" [49/60] [R_Middle_PIP_Joint] range:[0.000]~[1.745]\n",
" [50/60] [R_Middle_DIP_Joint] range:[0.000]~[1.449]\n",
" [51/60] [R_Ring_Roll_Joint] range:[-0.524]~[0.524]\n",
" [52/60] [R_Ring_MCP_Joint] range:[-0.175]~[1.571]\n",
" [53/60] [R_Ring_PIP_Joint] range:[0.000]~[1.745]\n",
" [54/60] [R_Ring_DIP_Joint] range:[0.000]~[1.449]\n",
" [55/60] [R_Little_Roll_Joint] range:[-0.524]~[0.524]\n",
" [56/60] [R_Little_MCP_Joint] range:[-0.175]~[1.571]\n",
" [57/60] [R_Little_PIP_Joint] range:[0.000]~[1.745]\n",
" [58/60] [R_Little_DIP_Joint] range:[0.000]~[1.449]\n",
" [59/60] [Waist_Pitch_Dummy_Joint] range:[-0.524]~[0.785]\n",
"\n",
"Prismatic joint information. n_pri_joint:[0]\n",
"\n",
"Control information. n_ctrl:[48]\n",
" [0/48] [Waist_Yaw_Position] range:[-2.793]~[2.793] gear:[1.00] type:[JOINT]\n",
" [1/48] [Waist_Pitch_Lower_Position] range:[-0.524]~[0.785] gear:[1.00] type:[JOINT]\n",
" [2/48] [Head_Pan_Position] range:[-1.571]~[1.571] gear:[1.00] type:[JOINT]\n",
" [3/48] [Head_Tilt_Position] range:[-0.785]~[0.785] gear:[1.00] type:[JOINT]\n",
" [4/48] [L_Shoudler_Pitch_Position] range:[-3.316]~[1.745] gear:[1.00] type:[JOINT]\n",
" [5/48] [L_Shoudler_Roll_Position] range:[-0.087]~[3.752] gear:[1.00] type:[JOINT]\n",
" [6/48] [L_Shoudler_Yaw_Position] range:[-3.316]~[1.745] gear:[1.00] type:[JOINT]\n",
" [7/48] [L_Elbow_Position] range:[-2.880]~[0.087] gear:[1.00] type:[JOINT]\n",
" [8/48] [L_Wrist_Yaw_Position] range:[-4.800]~[0.611] gear:[1.00] type:[JOINT]\n",
" [9/48] [L_Wrist_Roll_Position] range:[-0.873]~[0.873] gear:[1.00] type:[JOINT]\n",
" [10/48] [L_Wrist_Pitch_Position] range:[-1.396]~[1.396] gear:[1.00] type:[JOINT]\n",
" [11/48] [R_Shoudler_Pitch_Position] range:[-3.316]~[1.745] gear:[1.00] type:[JOINT]\n",
" [12/48] [R_Shoudler_Roll_Position] range:[-3.752]~[0.087] gear:[1.00] type:[JOINT]\n",
" [13/48] [R_Shoudler_Yaw_Position] range:[-1.745]~[3.316] gear:[1.00] type:[JOINT]\n",
" [14/48] [R_Elbow_Position] range:[-2.880]~[0.087] gear:[1.00] type:[JOINT]\n",
" [15/48] [R_Wrist_Yaw_Position] range:[-0.611]~[4.800] gear:[1.00] type:[JOINT]\n",
" [16/48] [R_Wrist_Roll_Position] range:[-0.873]~[0.873] gear:[1.00] type:[JOINT]\n",
" [17/48] [R_Wrist_Pitch_Position] range:[-1.396]~[1.396] gear:[1.00] type:[JOINT]\n",
" [18/48] [L_Thumb_Yaw_Position] range:[0.000]~[2.618] gear:[1.00] type:[JOINT]\n",
" [19/48] [L_Thumb_CMC_Position] range:[0.000]~[1.571] gear:[1.00] type:[JOINT]\n",
" [20/48] [L_Thumb_MCP_Position] range:[0.000]~[1.571] gear:[1.00] type:[JOINT]\n",
" [21/48] [L_Index_Roll_Position] range:[-0.524]~[0.524] gear:[1.00] type:[JOINT]\n",
" [22/48] [L_Index_MCP_Position] range:[-0.175]~[1.571] gear:[1.00] type:[JOINT]\n",
" [23/48] [L_Index_PIP_Position] range:[0.000]~[1.745] gear:[1.00] type:[JOINT]\n",
" [24/48] [L_Middle_Roll_Position] range:[-0.524]~[0.524] gear:[1.00] type:[JOINT]\n",
" [25/48] [L_Middle_MCP_Position] range:[-0.175]~[1.571] gear:[1.00] type:[JOINT]\n",
" [26/48] [L_Middle_PIP_Position] range:[0.000]~[1.745] gear:[1.00] type:[JOINT]\n",
" [27/48] [L_Ring_Roll_Position] range:[-0.524]~[0.524] gear:[1.00] type:[JOINT]\n",
" [28/48] [L_Ring_MCP_Position] range:[-0.175]~[1.571] gear:[1.00] type:[JOINT]\n",
" [29/48] [L_Ring_PIP_Position] range:[0.000]~[1.745] gear:[1.00] type:[JOINT]\n",
" [30/48] [L_Little_Roll_Position] range:[-0.524]~[0.524] gear:[1.00] type:[JOINT]\n",
" [31/48] [L_Little_MCP_Position] range:[-0.175]~[1.571] gear:[1.00] type:[JOINT]\n",
" [32/48] [L_Little_PIP_Position] range:[0.000]~[1.745] gear:[1.00] type:[JOINT]\n",
" [33/48] [R_Thumb_Yaw_Position] range:[-2.618]~[0.000] gear:[1.00] type:[JOINT]\n",
" [34/48] [R_Thumb_CMC_Position] range:[0.000]~[1.571] gear:[1.00] type:[JOINT]\n",
" [35/48] [R_Thumb_MCP_Position] range:[0.000]~[1.571] gear:[1.00] type:[JOINT]\n",
" [36/48] [R_Index_Roll_Position] range:[-0.524]~[0.524] gear:[1.00] type:[JOINT]\n",
" [37/48] [R_Index_MCP_Position] range:[-0.175]~[1.571] gear:[1.00] type:[JOINT]\n",
" [38/48] [R_Index_PIP_Position] range:[0.000]~[1.745] gear:[1.00] type:[JOINT]\n",
" [39/48] [R_Middle_Roll_Position] range:[-0.524]~[0.524] gear:[1.00] type:[JOINT]\n",
" [40/48] [R_Middle_MCP_position] range:[-0.175]~[1.571] gear:[1.00] type:[JOINT]\n",
" [41/48] [R_Middle_PIP_Position] range:[0.000]~[1.745] gear:[1.00] type:[JOINT]\n",
" [42/48] [R_Ring_Roll_Position] range:[-0.524]~[0.524] gear:[1.00] type:[JOINT]\n",
" [43/48] [R_Ring_MCP_Position] range:[-0.175]~[1.571] gear:[1.00] type:[JOINT]\n",
" [44/48] [R_Ring_PIP_Position] range:[0.000]~[1.745] gear:[1.00] type:[JOINT]\n",
" [45/48] [R_Little_Roll_Position] range:[-0.524]~[0.524] gear:[1.00] type:[JOINT]\n",
" [46/48] [R_Little_MCP_Position] range:[-0.175]~[1.571] gear:[1.00] type:[JOINT]\n",
" [47/48] [R_Little_PIP_Position] range:[0.000]~[1.745] gear:[1.00] type:[JOINT]\n",
"\n",
"Camera information. n_cam:[1]\n",
" [0/1] [robotview] fov:[60.0]\n",
"\n",
"n_sensor:[20]\n",
"sensor_names:['touch_rpalm', 'touch_rthumb_mcp', 'touch_rthumb_ip', 'touch_rthumb_tip', 'touch_rindex_mcp', 'touch_rindex_pip', 'touch_rindex_dip', 'touch_rindex_tip', 'touch_rmiddle_mcp', 'touch_rmiddle_pip', 'touch_rmiddle_dip', 'touch_rmiddle_tip', 'touch_rring_mcp', 'touch_rring_pip', 'touch_rring_dip', 'touch_rring_tip', 'touch_rlittle_mcp', 'touch_rlittle_pip', 'touch_rlittle_dip', 'touch_rlittle_tip']\n",
"n_site:[21]\n",
"site_names:['rpalm', 'rgcp', 'rthumb_mcp', 'rthumb_ip', 'rthumb_tip', 'rindex_mcp', 'rindex_pip', 'rindex_dip', 'rindex_tip', 'rmiddle_mcp', 'rmiddle_pip', 'rmiddle_dip', 'rmiddle_tip', 'rring_mcp', 'rring_pip', 'rring_dip', 'rring_tip', 'rlittle_mcp', 'rlittle_pip', 'rlittle_dip', 'rlittle_tip']\n",
"-----------------------------------------------------------------------------\n",
"env:[scene] reset\n"
]
}
],
"source": [
"xml_path = merge_mjcfs(\n",
" included_mjcf_files=[\n",
" '../../asset/floor/floor_white_gray.xml',\n",
" '../../asset/allex/allex_contact_sensor.xml',\n",
" 'object/table/base_table.xml',\n",
" 'object/daruma/daruma/daruma.xml',\n",
" ],\n",
" output_xml_path = 'xml/scene_allex.xml',\n",
")\n",
"env = MuJoCoParserClass(rel_xml_path=xml_path,verbose=True)\n",
"env_ik = MuJoCoParserClass(rel_xml_path=xml_path,verbose=False) # env for IK"
]
},
{
"cell_type": "code",
"execution_count": 23,
"id": "9d6ce97e-3b17-4837-a8f5-bfd85d855193",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Helper functions ready\n"
]
}
],
"source": [
"def evaluate_poly(x, x0, coef):\n",
" x_bar = x-x0\n",
" return coef[0]+coef[1]*x_bar+coef[2]*x_bar**2+coef[3]*x_bar**3+coef[4]*x_bar**4\n",
"def evaluate_poly_prime(x, x0, coef):\n",
" x_bar = x-x0\n",
" return coef[1]+2*coef[2]*x_bar+3*coef[3]*x_bar**2+4*coef[4]*x_bar**3\n",
"print (\"Helper functions ready\")"
]
},
{
"cell_type": "code",
"execution_count": 4,
"id": "c980507b-22bf-486d-9fbe-1f6afdc20e62",
"metadata": {
"scrolled": true
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[0/12]\n",
" active joint2:[Waist_Pitch_Lower_Joint]\n",
" passive joint1:[Waist_Pitch_Upper_Joint]\n",
" coef:[ 0. -1. 0. 0. 0.]\n",
"[1/12]\n",
" active joint2:[Waist_Pitch_Lower_Joint]\n",
" passive joint1:[Waist_Pitch_Dummy_Joint]\n",
" coef:[0. 1. 0. 0. 0.]\n",
"[2/12]\n",
" active joint2:[L_Thumb_MCP_Joint]\n",
" passive joint1:[L_Thumb_IP_Joint]\n",
" coef:[-0. 0.67 0.02 0.12 -0.07]\n",
"[3/12]\n",
" active joint2:[L_Index_PIP_Joint]\n",
" passive joint1:[L_Index_DIP_Joint]\n",
" coef:[-0. 0.43 0.07 0.14 -0.05]\n",
"[4/12]\n",
" active joint2:[L_Middle_PIP_Joint]\n",
" passive joint1:[L_Middle_DIP_Joint]\n",
" coef:[-0. 0.43 0.07 0.14 -0.05]\n",
"[5/12]\n",
" active joint2:[L_Ring_PIP_Joint]\n",
" passive joint1:[L_Ring_DIP_Joint]\n",
" coef:[-0. 0.43 0.07 0.14 -0.05]\n",
"[6/12]\n",
" active joint2:[L_Little_PIP_Joint]\n",
" passive joint1:[L_Little_DIP_Joint]\n",
" coef:[-0. 0.43 0.07 0.14 -0.05]\n",
"[7/12]\n",
" active joint2:[R_Thumb_MCP_Joint]\n",
" passive joint1:[R_Thumb_IP_Joint]\n",
" coef:[-0. 0.67 0.02 0.12 -0.07]\n",
"[8/12]\n",
" active joint2:[R_Index_PIP_Joint]\n",
" passive joint1:[R_Index_DIP_Joint]\n",
" coef:[-0. 0.43 0.07 0.14 -0.05]\n",
"[9/12]\n",
" active joint2:[R_Middle_PIP_Joint]\n",
" passive joint1:[R_Middle_DIP_Joint]\n",
" coef:[-0. 0.43 0.07 0.14 -0.05]\n",
"[10/12]\n",
" active joint2:[R_Ring_PIP_Joint]\n",
" passive joint1:[R_Ring_DIP_Joint]\n",
" coef:[-0. 0.43 0.07 0.14 -0.05]\n",
"[11/12]\n",
" active joint2:[R_Little_PIP_Joint]\n",
" passive joint1:[R_Little_DIP_Joint]\n",
" coef:[-0. 0.43 0.07 0.14 -0.05]\n"
]
}
],
"source": [
"# Get equality constraints\n",
"n_eq = env.model.eq_data.shape[0]\n",
"for eq_idx in range(n_eq): # for each joint equality constraints\n",
" joint1 = env.model.joint(env.model.eq_obj1id[eq_idx]).name # passive joint name\n",
" joint2 = env.model.joint(env.model.eq_obj2id[eq_idx]).name # active joint name\n",
" coef = env.model.eq_data[eq_idx,:5]\n",
" y0 = env.model.joint(joint1).qpos0[0] # passive joint initial position\n",
" x0 = env.model.joint(joint2).qpos0[0] # active joint initial position\n",
" y = env.data.joint(joint1).qpos[0] # passive joint position\n",
" x = env.data.joint(joint2).qpos[0] # active joint position\n",
" # Apply equality constraints, i.e., y=f(x)\n",
" y_bar = evaluate_poly(x=x,x0=x0,coef=coef)\n",
" # Print\n",
" print (\"[%d/%d]\"%(eq_idx,n_eq))\n",
" print (\" active joint2:[%s]\"%(joint2))\n",
" print (\" passive joint1:[%s]\"%(joint1))\n",
" print (\" coef:%s\"%(coef))"
]
},
{
"cell_type": "code",
"execution_count": 5,
"id": "17c0f1cd-2692-425b-890c-d38b89d94d36",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"#rev joints:[60] #active joints:[48] #passive joints:[12]\n"
]
}
],
"source": [
"# Get the list of active joint names\n",
"active_joint_names = env.rev_joint_names.copy()\n",
"passive_joint_names = []\n",
"for eq_idx in range(n_eq): # for each joint equality constraints\n",
" joint1 = env.model.joint(env.model.eq_obj1id[eq_idx]).name # passive joint name\n",
" active_joint_names.remove(joint1)\n",
" passive_joint_names.append(joint1)\n",
"print (\"#rev joints:[%d] #active joints:[%d] #passive joints:[%d]\"%\n",
" (env.n_rev_joint,len(active_joint_names),len(passive_joint_names)))"
]
},
{
"cell_type": "code",
"execution_count": 6,
"id": "5cbbf096-c561-49d6-9f20-4ea66c93e4e9",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"env:[scene] reset\n",
"env:[scene] initalize viewer\n",
"\u001b[32mDone.\u001b[0m\n"
]
}
],
"source": [
"# FK with slider control of active joints only\n",
"env.reset()\n",
"cfgs = {'azimuth':129.18,'distance':1.65,'elevation':-24.0,'lookat':[0.59,-0.13,1.33],\n",
" 'geomgroup_2':False,'geomgroup_3':True}\n",
"env.init_viewer(transparent=True,width=0.6,height=1.0,**cfgs)\n",
"env.forward(\n",
" q=[-1.57,-1.57,-1.57,+1.57],\n",
" joint_names=['L_Elbow_Joint','L_Wrist_Yaw_Joint',\n",
" 'R_Elbow_Joint','R_Wrist_Yaw_Joint'])\n",
"env.set_pR_base_body('daruma',p=[1,0,0.91],R=rpy2r([1.57,0,0])) # object\n",
"sliders = MultiSliderQtClass(\n",
" n_slider = len(active_joint_names),\n",
" window_width = 500,\n",
" window_height = env.monitor_height,\n",
" slider_width = 300,\n",
" label_width = 150,\n",
" label_texts = active_joint_names,\n",
" slider_mins = env.joint_mins[get_idxs(env.joint_names,active_joint_names)],\n",
" slider_maxs = env.joint_maxs[get_idxs(env.joint_names,active_joint_names)],\n",
" slider_vals = env.get_qpos_joints(joint_names=active_joint_names),\n",
")\n",
"while env.is_viewer_alive():\n",
" rev_q = env.get_qpos_joints(joint_names=env.rev_joint_names)\n",
" rev_q[get_idxs(env.rev_joint_names,active_joint_names)] = sliders.get_values()\n",
" for eq_idx in range(n_eq): # for each joint equality constraints\n",
" passive_joint = env.model.joint(env.model.eq_obj1id[eq_idx]).name\n",
" active_joint = env.model.joint(env.model.eq_obj2id[eq_idx]).name\n",
" coef = env.model.eq_data[eq_idx,:5]\n",
" y0 = env.model.joint(passive_joint).qpos0[0]\n",
" x0 = env.model.joint(active_joint).qpos0[0]\n",
" x = env.data.joint(active_joint).qpos[0]\n",
" y = evaluate_poly(x=x,x0=x0,coef=coef)\n",
" rev_q[env.rev_joint_names.index(passive_joint)] = y\n",
" env.forward(q=rev_q,joint_names=env.rev_joint_names)\n",
" env.render()\n",
"sliders.close()\n",
"print_green (\"Done.\")"
]
},
{
"cell_type": "code",
"execution_count": 54,
"id": "773e5b7c-296f-42ef-be4e-20dd76d8d569",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"env:[scene] reset\n",
"env:[scene] initalize viewer\n",
"\u001b[32mDone.\u001b[0m\n"
]
}
],
"source": [
"# IK with slider control (assuming fixed base)\n",
"env.reset()\n",
"env.forward(q=env.model.qpos0)\n",
"cfgs = {'azimuth':129.18,'distance':1.65,'elevation':-24.0,'lookat':[0.59,-0.13,1.33],\n",
" 'geomgroup_2':False,'geomgroup_3':True}\n",
"env.init_viewer(transparent=True,width=0.6,height=1.0,**cfgs)\n",
"env.forward(\n",
" q=[-1.57,-1.57,-1.57,+1.57],\n",
" joint_names=['L_Elbow_Joint','L_Wrist_Yaw_Joint',\n",
" 'R_Elbow_Joint','R_Wrist_Yaw_Joint'])\n",
"env.set_pR_base_body('daruma',p=[1,0,0.91],R=rpy2r([1.57,0,0])) # object\n",
"T_rpalm0 = env.get_T_site(site_name='rpalm')\n",
"sliders = MultiSliderQtClass(\n",
" n_slider = 3,\n",
" window_width = 500,\n",
" window_height = int(0.2*env.monitor_height),\n",
" slider_width = 300,\n",
" label_width = 150,\n",
" label_texts = ['X','Y','Z'],\n",
" slider_mins = t2p(T_rpalm0)-np.ones(3),\n",
" slider_maxs = t2p(T_rpalm0)+np.ones(3),\n",
" slider_vals = t2p(T_rpalm0),\n",
")\n",
"while env.is_viewer_alive():\n",
" # Current and target pose of the right palm\n",
" T_rpalm = env.get_T_site('rpalm')\n",
" p_trgt = sliders.get_values()\n",
" R_trgt = t2r(T_rpalm0)\n",
"\n",
" # Compute Jacobian and task-space error\n",
" J_p,J_R,J_full = env.get_J_site(site_name='rpalm') # J_full:(6,66), env.n_qvel=66, env.model.nv=66\n",
" p_curr,R_curr = env.get_pR_site(site_name='rpalm')\n",
" p_err = (p_trgt-p_curr) # position error (3,)\n",
" R_err = np.linalg.solve(R_curr,R_trgt) # relative rotation (3x3)\n",
" w_err = R_curr @ r2w(R_err) # orientation error (3,)\n",
" err = np.concatenate((p_err,w_err)) # task error (6,)\n",
"\n",
" # Extract Jacobian columns for revoulte joints only \n",
" J_full_rev = J_full[:,env.get_jacobian_column_idxs(env.rev_joint_names)] # (6,60)\n",
"\n",
" # Modify Jacobian to account for joint coupling constraints\n",
" for eq_idx in range(n_eq): # for each joint equality constraints\n",
" passive_joint = env.model.joint(env.model.eq_obj1id[eq_idx]).name\n",
" active_joint = env.model.joint(env.model.eq_obj2id[eq_idx]).name\n",
" coef = env.model.eq_data[eq_idx,:5]\n",
" x0 = env.model.joint(active_joint).qpos0[0]\n",
" x = env.data.joint(active_joint).qpos[0]\n",
" idx_passive = env.rev_joint_names.index(passive_joint)\n",
" idx_active = env.rev_joint_names.index(active_joint)\n",
" J_col_passive = J_full_rev[:,idx_passive].copy()\n",
" J_full_rev[:,idx_passive] = 0.0 # zero out passive colum\n",
" J_full_rev[:,idx_active] += J_col_passive*evaluate_poly_prime(x, x0, coef)\n",
"\n",
" # Solve IK step with damped least squares\n",
" dq_rev = env.damped_ls(J_full_rev, err, eps=1e-6, stepsize=1.0, th=np.deg2rad(45)) # (60,)\n",
"\n",
" # Update active joints only\n",
" dq_active = dq_rev[get_idxs(env.rev_joint_names,active_joint_names)] # (48,)\n",
" q_active = env.get_qpos_joints(active_joint_names) + dq_active # (48,)\n",
" \n",
" # Rebuild full revoulte joints\n",
" q_rev = env.get_qpos_joints(env.rev_joint_names) # (60,)\n",
" q_rev[get_idxs(env.rev_joint_names,active_joint_names)] = q_active # (48,)\n",
" for eq_idx in range(n_eq): # for each joint equality constraints\n",
" passive_joint = env.model.joint(env.model.eq_obj1id[eq_idx]).name\n",
" active_joint = env.model.joint(env.model.eq_obj2id[eq_idx]).name\n",
" coef = env.model.eq_data[eq_idx,:5]\n",
" x0 = env.model.joint(active_joint).qpos0[0]\n",
" x = q_rev[env.rev_joint_names.index(active_joint)]\n",
" y = evaluate_poly(x=x,x0=x0,coef=coef)\n",
" q_rev[env.rev_joint_names.index(passive_joint)] = y\n",
"\n",
" # Forward kinematics\n",
" env.forward(q=q_rev, joint_names=env.rev_joint_names)\n",
"\n",
" # Render\n",
" env.plot_T(T=T_rpalm,axis_len=0.1)\n",
" env.plot_T(T=pr2t(p_trgt,R_trgt),axis_len=0.1)\n",
" env.render()\n",
"\n",
"# Close \n",
"sliders.close() \n",
"print_green (\"Done.\")"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "6af2efdd-26f4-4f5c-b850-b22b41fb1c95",
"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