Created
September 4, 2025 15:10
-
-
Save sjchoi86/f5b0374f3d6e09752c27a494b6ae3f35 to your computer and use it in GitHub Desktop.
Revisions
-
sjchoi86 created this gist
Sep 4, 2025 .There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode charactersOriginal file line number Diff line number Diff line change @@ -0,0 +1,729 @@ { "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 }