Skip to content

Instantly share code, notes, and snippets.

@sjchoi86

sjchoi86/g1_contact_handling_explained.ipynb

Created October 20, 2025 10:49
Show Gist options

  • Select an option

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

Select an option

Learn more about clone URLs
Save sjchoi86/b3ee17ad4f8c501773c0ca5925de33c9 to your computer and use it in GitHub Desktop.
Download ZIP
Uploaded from jupyterlab
Display the source blob
Display the rendered blob
Raw
g1_contact_handling_explained.ipynb
{
"cells": [
{
"cell_type": "markdown",
"id": "b47a07e4-92f6-4ef8-97ea-c7a63dc68835",
"metadata": {},
"source": [
"### Contact handling using `G1`"
]
},
{
"cell_type": "code",
"execution_count": 1,
"id": "8ed0c188-a474-46ac-a972-c8c8d4a5093f",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\u001b[32mJupyter Notebook GUI initialized: Retina, inline, qt\u001b[0m\n",
"MuJoCo version: (3, 3, 0)\n"
]
}
],
"source": [
"%run ../../package/init_jupyter.py"
]
},
{
"cell_type": "code",
"execution_count": 2,
"id": "ed470c03-48ff-4e97-9a86-c012ed79152a",
"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/unitree_g1/g1_joi.xml]\n",
" - [2] [../../asset/object/sphere_10cm_radius_1.xml]\n",
" - [3] [../../asset/object/sphere_10cm_radius_2.xml]\n",
"[merge_mjcfs] Saved merged XML to:[xml/scene_g1.xml]\n",
"\n",
"-----------------------------------------------------------------------------\n",
"name:[scene] dt:[0.002] HZ:[500]\n",
" n_qpos:[58] n_qvel:[55] n_qacc:[55] n_ctrl:[37]\n",
" integrator:[IMPLICITFAST]\n",
"\n",
"n_body:[43]\n",
" [0/43] [world] mass:[0.00]kg\n",
" [1/43] [pelvis] mass:[2.86]kg\n",
" [2/43] [left_hip_pitch_link] mass:[1.30]kg\n",
" [3/43] [left_hip_roll_link] mass:[1.45]kg\n",
" [4/43] [left_hip_yaw_link] mass:[2.05]kg\n",
" [5/43] [left_knee_link] mass:[2.25]kg\n",
" [6/43] [left_ankle_pitch_link] mass:[0.07]kg\n",
" [7/43] [left_ankle_roll_link] mass:[0.39]kg\n",
" [8/43] [right_hip_pitch_link] mass:[1.30]kg\n",
" [9/43] [right_hip_roll_link] mass:[1.45]kg\n",
" [10/43] [right_hip_yaw_link] mass:[2.05]kg\n",
" [11/43] [right_knee_link] mass:[2.25]kg\n",
" [12/43] [right_ankle_pitch_link] mass:[0.07]kg\n",
" [13/43] [right_ankle_roll_link] mass:[0.39]kg\n",
" [14/43] [torso_link] mass:[7.52]kg\n",
" [15/43] [left_shoulder_pitch_link] mass:[0.71]kg\n",
" [16/43] [left_shoulder_roll_link] mass:[0.64]kg\n",
" [17/43] [left_shoulder_yaw_link] mass:[0.71]kg\n",
" [18/43] [left_elbow_pitch_link] mass:[0.60]kg\n",
" [19/43] [left_elbow_roll_link] mass:[0.51]kg\n",
" [20/43] [virtual_left_wrist_link] mass:[0.01]kg\n",
" [21/43] [left_zero_link] mass:[0.05]kg\n",
" [22/43] [left_one_link] mass:[0.05]kg\n",
" [23/43] [left_two_link] mass:[0.01]kg\n",
" [24/43] [left_three_link] mass:[0.05]kg\n",
" [25/43] [left_four_link] mass:[0.01]kg\n",
" [26/43] [left_five_link] mass:[0.05]kg\n",
" [27/43] [left_six_link] mass:[0.01]kg\n",
" [28/43] [right_shoulder_pitch_link] mass:[0.71]kg\n",
" [29/43] [right_shoulder_roll_link] mass:[0.64]kg\n",
" [30/43] [right_shoulder_yaw_link] mass:[0.71]kg\n",
" [31/43] [right_elbow_pitch_link] mass:[0.60]kg\n",
" [32/43] [right_elbow_roll_link] mass:[0.51]kg\n",
" [33/43] [virtual_right_wrist_link] mass:[0.01]kg\n",
" [34/43] [right_zero_link] mass:[0.05]kg\n",
" [35/43] [right_one_link] mass:[0.05]kg\n",
" [36/43] [right_two_link] mass:[0.01]kg\n",
" [37/43] [right_three_link] mass:[0.05]kg\n",
" [38/43] [right_four_link] mass:[0.01]kg\n",
" [39/43] [right_five_link] mass:[0.05]kg\n",
" [40/43] [right_six_link] mass:[0.01]kg\n",
" [41/43] [body_sphere_10cm_radius_1] mass:[1.00]kg\n",
" [42/43] [body_sphere_10cm_radius_2] mass:[1.00]kg\n",
"body_total_mass:[34.25]kg\n",
"\n",
"n_geom:[94]\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, 'geom_sphere_10cm_radius_1', 'geom_sphere_10cm_radius_2']\n",
"\n",
"n_mesh:[43]\n",
"mesh_names:['pelvis', 'pelvis_contour_link', 'left_hip_pitch_link', 'left_hip_roll_link', 'left_hip_yaw_link', 'left_knee_link', 'left_ankle_pitch_link', 'left_ankle_roll_link', 'right_hip_pitch_link', 'right_hip_roll_link', 'right_hip_yaw_link', 'right_knee_link', 'right_ankle_pitch_link', 'right_ankle_roll_link', 'torso_link', 'head_link', 'left_shoulder_pitch_link', 'left_shoulder_roll_link', 'left_shoulder_yaw_link', 'left_elbow_pitch_link', 'left_elbow_roll_link', 'right_shoulder_pitch_link', 'right_shoulder_roll_link', 'right_shoulder_yaw_link', 'right_elbow_pitch_link', 'right_elbow_roll_link', 'logo_link', 'left_palm_link', 'left_zero_link', 'left_one_link', 'left_two_link', 'left_three_link', 'left_four_link', 'left_five_link', 'left_six_link', 'right_palm_link', 'right_zero_link', 'right_one_link', 'right_two_link', 'right_three_link', 'right_four_link', 'right_five_link', 'right_six_link']\n",
"\n",
"n_joint:[40]\n",
" [0/40] [floating_base_joint] axis:[0. 0. 1.]\n",
" [1/40] [left_hip_pitch_joint] axis:[0. 1. 0.]\n",
" [2/40] [left_hip_roll_joint] axis:[1. 0. 0.]\n",
" [3/40] [left_hip_yaw_joint] axis:[0. 0. 1.]\n",
" [4/40] [left_knee_joint] axis:[0. 1. 0.]\n",
" [5/40] [left_ankle_pitch_joint] axis:[0. 1. 0.]\n",
" [6/40] [left_ankle_roll_joint] axis:[1. 0. 0.]\n",
" [7/40] [right_hip_pitch_joint] axis:[0. 1. 0.]\n",
" [8/40] [right_hip_roll_joint] axis:[1. 0. 0.]\n",
" [9/40] [right_hip_yaw_joint] axis:[0. 0. 1.]\n",
" [10/40] [right_knee_joint] axis:[0. 1. 0.]\n",
" [11/40] [right_ankle_pitch_joint] axis:[0. 1. 0.]\n",
" [12/40] [right_ankle_roll_joint] axis:[1. 0. 0.]\n",
" [13/40] [torso_joint] axis:[0. 0. 1.]\n",
" [14/40] [left_shoulder_pitch_joint] axis:[0. 1. 0.]\n",
" [15/40] [left_shoulder_roll_joint] axis:[1. 0. 0.]\n",
" [16/40] [left_shoulder_yaw_joint] axis:[0. 0. 1.]\n",
" [17/40] [left_elbow_pitch_joint] axis:[0. 1. 0.]\n",
" [18/40] [left_elbow_roll_joint] axis:[1. 0. 0.]\n",
" [19/40] [left_zero_joint] axis:[0. 1. 0.]\n",
" [20/40] [left_one_joint] axis:[0. 0. 1.]\n",
" [21/40] [left_two_joint] axis:[0. 0. 1.]\n",
" [22/40] [left_three_joint] axis:[0. 0. 1.]\n",
" [23/40] [left_four_joint] axis:[0. 0. 1.]\n",
" [24/40] [left_five_joint] axis:[0. 0. 1.]\n",
" [25/40] [left_six_joint] axis:[0. 0. 1.]\n",
" [26/40] [right_shoulder_pitch_joint] axis:[0. 1. 0.]\n",
" [27/40] [right_shoulder_roll_joint] axis:[1. 0. 0.]\n",
" [28/40] [right_shoulder_yaw_joint] axis:[0. 0. 1.]\n",
" [29/40] [right_elbow_pitch_joint] axis:[0. 1. 0.]\n",
" [30/40] [right_elbow_roll_joint] axis:[1. 0. 0.]\n",
" [31/40] [right_zero_joint] axis:[0. 1. 0.]\n",
" [32/40] [right_one_joint] axis:[0. 0. 1.]\n",
" [33/40] [right_two_joint] axis:[0. 0. 1.]\n",
" [34/40] [right_three_joint] axis:[0. 0. 1.]\n",
" [35/40] [right_four_joint] axis:[0. 0. 1.]\n",
" [36/40] [right_five_joint] axis:[0. 0. 1.]\n",
" [37/40] [right_six_joint] axis:[0. 0. 1.]\n",
" [38/40] [None] axis:[0. 0. 1.]\n",
" [39/40] [None] axis:[0. 0. 1.]\n",
"\n",
"n_dof:[55] (=number of rows of Jacobian)\n",
" [0/55] [None] attached joint:[floating_base_joint] body:[pelvis]\n",
" [1/55] [None] attached joint:[floating_base_joint] body:[pelvis]\n",
" [2/55] [None] attached joint:[floating_base_joint] body:[pelvis]\n",
" [3/55] [None] attached joint:[floating_base_joint] body:[pelvis]\n",
" [4/55] [None] attached joint:[floating_base_joint] body:[pelvis]\n",
" [5/55] [None] attached joint:[floating_base_joint] body:[pelvis]\n",
" [6/55] [None] attached joint:[left_hip_pitch_joint] body:[left_hip_pitch_link]\n",
" [7/55] [None] attached joint:[left_hip_roll_joint] body:[left_hip_roll_link]\n",
" [8/55] [None] attached joint:[left_hip_yaw_joint] body:[left_hip_yaw_link]\n",
" [9/55] [None] attached joint:[left_knee_joint] body:[left_knee_link]\n",
" [10/55] [None] attached joint:[left_ankle_pitch_joint] body:[left_ankle_pitch_link]\n",
" [11/55] [None] attached joint:[left_ankle_roll_joint] body:[left_ankle_roll_link]\n",
" [12/55] [None] attached joint:[right_hip_pitch_joint] body:[right_hip_pitch_link]\n",
" [13/55] [None] attached joint:[right_hip_roll_joint] body:[right_hip_roll_link]\n",
" [14/55] [None] attached joint:[right_hip_yaw_joint] body:[right_hip_yaw_link]\n",
" [15/55] [None] attached joint:[right_knee_joint] body:[right_knee_link]\n",
" [16/55] [None] attached joint:[right_ankle_pitch_joint] body:[right_ankle_pitch_link]\n",
" [17/55] [None] attached joint:[right_ankle_roll_joint] body:[right_ankle_roll_link]\n",
" [18/55] [None] attached joint:[torso_joint] body:[torso_link]\n",
" [19/55] [None] attached joint:[left_shoulder_pitch_joint] body:[left_shoulder_pitch_link]\n",
" [20/55] [None] attached joint:[left_shoulder_roll_joint] body:[left_shoulder_roll_link]\n",
" [21/55] [None] attached joint:[left_shoulder_yaw_joint] body:[left_shoulder_yaw_link]\n",
" [22/55] [None] attached joint:[left_elbow_pitch_joint] body:[left_elbow_pitch_link]\n",
" [23/55] [None] attached joint:[left_elbow_roll_joint] body:[left_elbow_roll_link]\n",
" [24/55] [None] attached joint:[left_zero_joint] body:[left_zero_link]\n",
" [25/55] [None] attached joint:[left_one_joint] body:[left_one_link]\n",
" [26/55] [None] attached joint:[left_two_joint] body:[left_two_link]\n",
" [27/55] [None] attached joint:[left_three_joint] body:[left_three_link]\n",
" [28/55] [None] attached joint:[left_four_joint] body:[left_four_link]\n",
" [29/55] [None] attached joint:[left_five_joint] body:[left_five_link]\n",
" [30/55] [None] attached joint:[left_six_joint] body:[left_six_link]\n",
" [31/55] [None] attached joint:[right_shoulder_pitch_joint] body:[right_shoulder_pitch_link]\n",
" [32/55] [None] attached joint:[right_shoulder_roll_joint] body:[right_shoulder_roll_link]\n",
" [33/55] [None] attached joint:[right_shoulder_yaw_joint] body:[right_shoulder_yaw_link]\n",
" [34/55] [None] attached joint:[right_elbow_pitch_joint] body:[right_elbow_pitch_link]\n",
" [35/55] [None] attached joint:[right_elbow_roll_joint] body:[right_elbow_roll_link]\n",
" [36/55] [None] attached joint:[right_zero_joint] body:[right_zero_link]\n",
" [37/55] [None] attached joint:[right_one_joint] body:[right_one_link]\n",
" [38/55] [None] attached joint:[right_two_joint] body:[right_two_link]\n",
" [39/55] [None] attached joint:[right_three_joint] body:[right_three_link]\n",
" [40/55] [None] attached joint:[right_four_joint] body:[right_four_link]\n",
" [41/55] [None] attached joint:[right_five_joint] body:[right_five_link]\n",
" [42/55] [None] attached joint:[right_six_joint] body:[right_six_link]\n",
" [43/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_1]\n",
" [44/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_1]\n",
" [45/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_1]\n",
" [46/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_1]\n",
" [47/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_1]\n",
" [48/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_1]\n",
" [49/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_2]\n",
" [50/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_2]\n",
" [51/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_2]\n",
" [52/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_2]\n",
" [53/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_2]\n",
" [54/55] [None] attached joint:[None] body:[body_sphere_10cm_radius_2]\n",
"\n",
"Free joint information. n_free_joint:[3]\n",
" [0/3] [floating_base_joint] body_name_attached:[pelvis]\n",
" [1/3] [None] body_name_attached:[body_sphere_10cm_radius_1]\n",
" [2/3] [None] body_name_attached:[body_sphere_10cm_radius_2]\n",
"\n",
"Revolute joint information. n_rev_joint:[37]\n",
" [0/37] [left_hip_pitch_joint] range:[-2.350]~[3.050]\n",
" [1/37] [left_hip_roll_joint] range:[-0.260]~[2.530]\n",
" [2/37] [left_hip_yaw_joint] range:[-2.750]~[2.750]\n",
" [3/37] [left_knee_joint] range:[-0.335]~[2.545]\n",
" [4/37] [left_ankle_pitch_joint] range:[-0.680]~[0.730]\n",
" [5/37] [left_ankle_roll_joint] range:[-0.262]~[0.262]\n",
" [6/37] [right_hip_pitch_joint] range:[-2.350]~[3.050]\n",
" [7/37] [right_hip_roll_joint] range:[-2.530]~[0.260]\n",
" [8/37] [right_hip_yaw_joint] range:[-2.750]~[2.750]\n",
" [9/37] [right_knee_joint] range:[-0.335]~[2.545]\n",
" [10/37] [right_ankle_pitch_joint] range:[-0.680]~[0.730]\n",
" [11/37] [right_ankle_roll_joint] range:[-0.262]~[0.262]\n",
" [12/37] [torso_joint] range:[-2.618]~[2.618]\n",
" [13/37] [left_shoulder_pitch_joint] range:[-2.967]~[2.792]\n",
" [14/37] [left_shoulder_roll_joint] range:[-1.588]~[2.252]\n",
" [15/37] [left_shoulder_yaw_joint] range:[-2.618]~[2.618]\n",
" [16/37] [left_elbow_pitch_joint] range:[-0.227]~[3.421]\n",
" [17/37] [left_elbow_roll_joint] range:[-2.094]~[2.094]\n",
" [18/37] [left_zero_joint] range:[-0.524]~[0.524]\n",
" [19/37] [left_one_joint] range:[-1.000]~[1.200]\n",
" [20/37] [left_two_joint] range:[0.000]~[1.840]\n",
" [21/37] [left_three_joint] range:[-1.840]~[0.300]\n",
" [22/37] [left_four_joint] range:[-1.840]~[0.000]\n",
" [23/37] [left_five_joint] range:[-1.840]~[0.300]\n",
" [24/37] [left_six_joint] range:[-1.840]~[0.000]\n",
" [25/37] [right_shoulder_pitch_joint] range:[-2.967]~[2.792]\n",
" [26/37] [right_shoulder_roll_joint] range:[-2.252]~[1.588]\n",
" [27/37] [right_shoulder_yaw_joint] range:[-2.618]~[2.618]\n",
" [28/37] [right_elbow_pitch_joint] range:[-0.227]~[3.421]\n",
" [29/37] [right_elbow_roll_joint] range:[-2.094]~[2.094]\n",
" [30/37] [right_zero_joint] range:[-0.524]~[0.524]\n",
" [31/37] [right_one_joint] range:[-1.200]~[1.000]\n",
" [32/37] [right_two_joint] range:[-1.840]~[0.000]\n",
" [33/37] [right_three_joint] range:[-0.300]~[1.840]\n",
" [34/37] [right_four_joint] range:[0.000]~[1.840]\n",
" [35/37] [right_five_joint] range:[-0.300]~[1.840]\n",
" [36/37] [right_six_joint] range:[0.000]~[1.840]\n",
"\n",
"Prismatic joint information. n_pri_joint:[0]\n",
"\n",
"Active joint information. n_active_joint:[37]\n",
" [0/37] [left_hip_pitch_joint] range:[-2.350]~[3.050]\n",
" [1/37] [left_hip_roll_joint] range:[-0.260]~[2.530]\n",
" [2/37] [left_hip_yaw_joint] range:[-2.750]~[2.750]\n",
" [3/37] [left_knee_joint] range:[-0.335]~[2.545]\n",
" [4/37] [left_ankle_pitch_joint] range:[-0.680]~[0.730]\n",
" [5/37] [left_ankle_roll_joint] range:[-0.262]~[0.262]\n",
" [6/37] [right_hip_pitch_joint] range:[-2.350]~[3.050]\n",
" [7/37] [right_hip_roll_joint] range:[-2.530]~[0.260]\n",
" [8/37] [right_hip_yaw_joint] range:[-2.750]~[2.750]\n",
" [9/37] [right_knee_joint] range:[-0.335]~[2.545]\n",
" [10/37] [right_ankle_pitch_joint] range:[-0.680]~[0.730]\n",
" [11/37] [right_ankle_roll_joint] range:[-0.262]~[0.262]\n",
" [12/37] [torso_joint] range:[-2.618]~[2.618]\n",
" [13/37] [left_shoulder_pitch_joint] range:[-2.967]~[2.792]\n",
" [14/37] [left_shoulder_roll_joint] range:[-1.588]~[2.252]\n",
" [15/37] [left_shoulder_yaw_joint] range:[-2.618]~[2.618]\n",
" [16/37] [left_elbow_pitch_joint] range:[-0.227]~[3.421]\n",
" [17/37] [left_elbow_roll_joint] range:[-2.094]~[2.094]\n",
" [18/37] [left_zero_joint] range:[-0.524]~[0.524]\n",
" [19/37] [left_one_joint] range:[-1.000]~[1.200]\n",
" [20/37] [left_two_joint] range:[0.000]~[1.840]\n",
" [21/37] [left_three_joint] range:[-1.840]~[0.300]\n",
" [22/37] [left_four_joint] range:[-1.840]~[0.000]\n",
" [23/37] [left_five_joint] range:[-1.840]~[0.300]\n",
" [24/37] [left_six_joint] range:[-1.840]~[0.000]\n",
" [25/37] [right_shoulder_pitch_joint] range:[-2.967]~[2.792]\n",
" [26/37] [right_shoulder_roll_joint] range:[-2.252]~[1.588]\n",
" [27/37] [right_shoulder_yaw_joint] range:[-2.618]~[2.618]\n",
" [28/37] [right_elbow_pitch_joint] range:[-0.227]~[3.421]\n",
" [29/37] [right_elbow_roll_joint] range:[-2.094]~[2.094]\n",
" [30/37] [right_zero_joint] range:[-0.524]~[0.524]\n",
" [31/37] [right_one_joint] range:[-1.200]~[1.000]\n",
" [32/37] [right_two_joint] range:[-1.840]~[0.000]\n",
" [33/37] [right_three_joint] range:[-0.300]~[1.840]\n",
" [34/37] [right_four_joint] range:[0.000]~[1.840]\n",
" [35/37] [right_five_joint] range:[-0.300]~[1.840]\n",
" [36/37] [right_six_joint] range:[0.000]~[1.840]\n",
"\n",
"Passive joint information. n_passive_joint:[0]\n",
"\n",
"Control information. n_ctrl:[37]\n",
" [0/37] [left_hip_pitch_joint] range:[-88.000]~[88.000] gear:[1.00] type:[JOINT]\n",
" [1/37] [left_hip_roll_joint] range:[-88.000]~[88.000] gear:[1.00] type:[JOINT]\n",
" [2/37] [left_hip_yaw_joint] range:[-88.000]~[88.000] gear:[1.00] type:[JOINT]\n",
" [3/37] [left_knee_joint] range:[-139.000]~[139.000] gear:[1.00] type:[JOINT]\n",
" [4/37] [left_ankle_pitch_joint] range:[-40.000]~[40.000] gear:[1.00] type:[JOINT]\n",
" [5/37] [left_ankle_roll_joint] range:[-40.000]~[40.000] gear:[1.00] type:[JOINT]\n",
" [6/37] [right_hip_pitch_joint] range:[-88.000]~[88.000] gear:[1.00] type:[JOINT]\n",
" [7/37] [right_hip_roll_joint] range:[-88.000]~[88.000] gear:[1.00] type:[JOINT]\n",
" [8/37] [right_hip_yaw_joint] range:[-88.000]~[88.000] gear:[1.00] type:[JOINT]\n",
" [9/37] [right_knee_joint] range:[-139.000]~[139.000] gear:[1.00] type:[JOINT]\n",
" [10/37] [right_ankle_pitch_joint] range:[-40.000]~[40.000] gear:[1.00] type:[JOINT]\n",
" [11/37] [right_ankle_roll_joint] range:[-40.000]~[40.000] gear:[1.00] type:[JOINT]\n",
" [12/37] [torso_joint] range:[-88.000]~[88.000] gear:[1.00] type:[JOINT]\n",
" [13/37] [left_shoulder_pitch_joint] range:[-20.000]~[20.000] gear:[1.00] type:[JOINT]\n",
" [14/37] [left_shoulder_roll_joint] range:[-20.000]~[20.000] gear:[1.00] type:[JOINT]\n",
" [15/37] [left_shoulder_yaw_joint] range:[-20.000]~[20.000] gear:[1.00] type:[JOINT]\n",
" [16/37] [left_elbow_pitch_joint] range:[-20.000]~[20.000] gear:[1.00] type:[JOINT]\n",
" [17/37] [left_elbow_roll_joint] range:[-20.000]~[20.000] gear:[1.00] type:[JOINT]\n",
" [18/37] [right_shoulder_pitch_joint] range:[-20.000]~[20.000] gear:[1.00] type:[JOINT]\n",
" [19/37] [right_shoulder_roll_joint] range:[-20.000]~[20.000] gear:[1.00] type:[JOINT]\n",
" [20/37] [right_shoulder_yaw_joint] range:[-20.000]~[20.000] gear:[1.00] type:[JOINT]\n",
" [21/37] [right_elbow_pitch_joint] range:[-20.000]~[20.000] gear:[1.00] type:[JOINT]\n",
" [22/37] [right_elbow_roll_joint] range:[-20.000]~[20.000] gear:[1.00] type:[JOINT]\n",
" [23/37] [left_zero_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [24/37] [left_one_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [25/37] [left_two_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [26/37] [left_three_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [27/37] [left_four_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [28/37] [left_five_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [29/37] [left_six_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [30/37] [right_zero_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [31/37] [right_one_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [32/37] [right_two_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [33/37] [right_three_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [34/37] [right_four_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [35/37] [right_five_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
" [36/37] [right_six_joint] range:[-0.700]~[0.700] gear:[1.00] type:[JOINT]\n",
"\n",
"Camera information. n_cam:[0]\n",
"\n",
"n_sensor:[3]\n",
"sensor_names:['imu-angular-velocity', 'imu-linear-acceleration', 'imu-body-quat']\n",
"n_site:[25]\n",
"site_names:['joi_root', 'joi_base', 'joi_spine', 'joi_lp', 'joi_lk', 'joi_la', 'left_foot', 'joi_lf', 'joi_rp', 'joi_rk', 'joi_ra', 'right_foot', 'joi_rf', 'imu', 'head', 'joi_ls', 'joi_le', 'virtual_left_wrist', 'joi_lw', 'joi_lh', 'joi_rs', 'joi_re', 'virtual_right_wrist', 'joi_rw', 'joi_rh']\n",
"-----------------------------------------------------------------------------\n",
"env:[scene] reset\n"
]
}
],
"source": [
"xml_path = merge_mjcfs(\n",
" included_mjcf_files=[\n",
" '../../asset/floor/floor_white_gray.xml',\n",
" '../../asset/unitree_g1/g1_joi.xml',\n",
" '../../asset/object/sphere_10cm_radius_1.xml',\n",
" '../../asset/object/sphere_10cm_radius_2.xml',\n",
" ],\n",
" output_xml_path = 'xml/scene_g1.xml',\n",
")\n",
"env = MuJoCoParserClass(rel_xml_path=xml_path,verbose=True)\n",
"# Backup geom colors\n",
"geom_rgba_list = []\n",
"for g_idx in range(env.n_geom): \n",
" geom_rgba_list.append(env.model.geom(g_idx).rgba.copy())\n",
"# Set geom colors\n",
"g1_body_name_list = env.get_sub_body_names('pelvis')\n",
"for body_name in g1_body_name_list:\n",
" body_idx = env.body_names.index(body_name)\n",
" geom_idxs = [idx for idx,val in enumerate(env.model.geom_bodyid) if val==body_idx]\n",
" for geom_idx in geom_idxs: env.model.geom(geom_idx).rgba = (0.5,0.5,0.5,0.9)\n",
"env.model.geom('geom_sphere_10cm_radius_1').rgba = (0.5,0.5,0.5,0.9)\n",
"env.model.geom('geom_sphere_10cm_radius_2').rgba = (0.5,0.5,0.5,0.9)\n",
"# Move spheres\n",
"p_obj1 = np.array([0.3,-0.25,0.85])\n",
"p_obj2 = np.array([0.3,+0.25,0.85])\n",
"env.set_p_base_body('body_sphere_10cm_radius_1',p_obj1)\n",
"env.set_p_base_body('body_sphere_10cm_radius_2',p_obj2)"
]
},
{
"cell_type": "code",
"execution_count": 8,
"id": "75a88545-fd15-4625-bf10-b22618b6f9a1",
"metadata": {
"scrolled": true
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"env:[scene] reset\n",
"env:[scene] initalize viewer\n",
"\u001b[32mDone.\u001b[0m\n"
]
}
],
"source": [
"# Reset env\n",
"env.reset()\n",
"viewer_kwargs = {'azimuth':151,'distance':2.41,'elevation':-28.24,'lookat':[-0.03,0,0.51]}\n",
"env.init_viewer(width=0.6,height=1.0,transparent=True,**viewer_kwargs)\n",
"\n",
"# Reset sliders\n",
"h = env.monitor_height # monitor height\n",
"slider_kwargs = {'window_width':500,'slider_width':300,'label_width':150}\n",
"sliders_joint = MultiSliderQtClass(\n",
" y_offset=0,window_height=0.7*h,n_slider=env.n_rev_joint,label_texts=env.rev_joint_names,\n",
" slider_mins=env.rev_joint_mins,slider_maxs=env.rev_joint_maxs,\n",
" slider_vals=env.get_qpos_joints(joint_names=env.rev_joint_names),**slider_kwargs,\n",
")\n",
"sliders_obj1 = MultiSliderQtClass(\n",
" y_offset=0.7*h,window_height=0.1*h,n_slider=3,label_texts=['X','Y','Z'],\n",
" slider_mins=p_obj1-0.5*np.ones(3),slider_maxs=p_obj1+0.5*np.ones(3),\n",
" slider_vals=p_obj1,**slider_kwargs,\n",
")\n",
"sliders_obj2 = MultiSliderQtClass(\n",
" y_offset=0.8*h,window_height=0.1*h,n_slider=3,label_texts=['X','Y','Z'],\n",
" slider_mins=p_obj2-0.5*np.ones(3),slider_maxs=p_obj2+0.5*np.ones(3),\n",
" slider_vals=p_obj2,**slider_kwargs,\n",
")\n",
"sliders_geom = MultiSliderQtClass(\n",
" y_offset=0.9*h,window_height=0.1*h,n_slider=2,label_texts=['geom_margin','geom_gap'],\n",
" slider_mins=[-0.1,-0.1],slider_maxs=[+0.1,+0.1],slider_vals=[0,0],**slider_kwargs,\n",
")\n",
"\n",
"# IK configurations\n",
"eps = 1e-6\n",
"# joints_use = env.rev_joint_names\n",
"joints_use = [\n",
" 'left_hip_pitch_joint','left_hip_roll_joint','left_hip_yaw_joint','left_knee_joint',\n",
" 'left_ankle_pitch_joint','left_ankle_roll_joint',\n",
" 'right_hip_pitch_joint','right_hip_roll_joint','right_hip_yaw_joint','right_knee_joint',\n",
" 'right_ankle_pitch_joint','right_ankle_roll_joint',\n",
" 'torso_joint',\n",
" 'left_shoulder_pitch_joint','left_shoulder_roll_joint','left_shoulder_yaw_joint',\n",
" 'left_elbow_pitch_joint','left_elbow_roll_joint',\n",
" # 'left_zero_joint','left_one_joint','left_two_joint','left_three_joint',\n",
" # 'left_four_joint','left_five_joint','left_six_joint',\n",
" 'right_shoulder_pitch_joint','right_shoulder_roll_joint','right_shoulder_yaw_joint',\n",
" 'right_elbow_pitch_joint','right_elbow_roll_joint',\n",
" # 'right_zero_joint','right_one_joint','right_two_joint','right_three_joint',\n",
" # 'right_four_joint','right_five_joint','right_six_joint',\n",
"]\n",
"n_use = len(joints_use)\n",
"idxs_use = env.get_idxs_jac(joint_names=joints_use)\n",
"S = np.zeros((env.model.nv,n_use),dtype=np.float64) # (nv x n_use)\n",
"S[idxs_use,np.arange(n_use)] = 1.0\n",
"\n",
"# --- Nullspace additions (setup, once) ---\n",
"ik_gain_pri = 1.0\n",
"ik_gain_sec = 0.1\n",
"ik_dt = 0.1\n",
"def wrap_to_pi(x): return (x+np.pi)%(2*np.pi)-np.pi\n",
"q_use0 = env.get_qpos_joints(joint_names=joints_use) # home posture for secondary task\n",
"# --- End of nullspace additions ---\n",
"\n",
"# Loop\n",
"tick = 0\n",
"while env.is_viewer_alive():\n",
" # Update\n",
" tick = tick + 1\n",
" sv_joint = sliders_joint.get_values()\n",
" sv_obj1 = sliders_obj1.get_values()\n",
" sv_obj2 = sliders_obj2.get_values()\n",
" sv_geom = sliders_geom.get_values()\n",
" env.model.geom_margin[:] = sv_geom[0] # geom_margin, detect contact if dist<margin\n",
" env.model.geom_gap[:] = sv_geom[1] # geom_gap, include in solver if dist<margin-gap\n",
" env.set_p_base_body('body_sphere_10cm_radius_1',sv_obj1)\n",
" env.set_p_base_body('body_sphere_10cm_radius_2',sv_obj2)\n",
" env.forward(q=sv_joint,joint_names=env.rev_joint_names)\n",
"\n",
" # Get contact information\n",
" contact_info = env.get_contact_info_v2()\n",
" n_contact,p_contact_list = contact_info['n_contact'],contact_info['p_contact_list']\n",
" geom1_idx_list,geom2_idx_list = contact_info['geom1_idx_list'],contact_info['geom2_idx_list']\n",
" body1_idx_list,body2_idx_list = contact_info['body1_idx_list'],contact_info['body2_idx_list']\n",
" norm_dir_list = contact_info['norm_dir_list']\n",
"\n",
" # Contact handling with IK\n",
" J_p_list,p_fr_list,p_to_list = [],[],[]\n",
" for c_idx in range(n_contact):\n",
" p_contact = p_contact_list[c_idx]\n",
" norm_dir = norm_dir_list[c_idx] # (3,)\n",
" body1_idx,body2_idx = body1_idx_list[c_idx],body2_idx_list[c_idx]\n",
" body1_name,body2_name = env.body_names[body1_idx],env.body_names[body2_idx]\n",
" p1_fr,p2_fr = p_contact,p_contact # (3,)\n",
" len_dir = 0.1\n",
" p1_to,p2_to = p_contact-len_dir*norm_dir,p_contact+len_dir*norm_dir # (3,)\n",
" # geom1\n",
" if body1_name in g1_body_name_list:\n",
" J_p1,J_R1 = np.zeros((3,env.model.nv)),np.zeros((3,env.model.nv))\n",
" mujoco.mj_jac(env.model,env.data,J_p1,J_R1,p1_fr,body1_idx)\n",
" J_p_list.append(J_p1); p_fr_list.append(p1_fr); p_to_list.append(p1_to)\n",
" # geom2\n",
" if body2_name in g1_body_name_list:\n",
" J_p2,J_R2 = np.zeros((3,env.model.nv)),np.zeros((3,env.model.nv))\n",
" mujoco.mj_jac(env.model,env.data,J_p2,J_R2,p2_fr,body2_idx)\n",
" J_p_list.append(J_p2); p_fr_list.append(p2_fr); p_to_list.append(p2_to)\n",
" \n",
" # Stack Jacobian matrices and error vectors\n",
" M = len(J_p_list) # number of IK positional targets\n",
" if len(J_p_list) > 0:\n",
" J_p_stack = np.vstack(J_p_list) # (3*M x nv)\n",
" p_fr_stack = np.hstack(p_fr_list) # (3*M,)\n",
" p_to_stack = np.hstack(p_to_list) # (3*M,)\n",
" p_err_stack = p_to_stack - p_fr_stack # (3*M,)\n",
"\n",
" J_use = J_p_stack @ S # (3*M x n_use)\n",
" JTJ_use = J_use.T @ J_use # (n_use x n_use)\n",
" Jsharp_use = np.linalg.solve(JTJ_use + eps*np.eye(n_use), J_use.T) # (n_use x 3*M)\n",
"\n",
" # --- Nullspace additions (inside solve) ---\n",
" # Primary task\n",
" dq1_use = Jsharp_use @ (ik_gain_pri * p_err_stack) # (n_use,)\n",
"\n",
" # Secondary task (posture regulation) projected to nullspace\n",
" N_use = np.eye(n_use) - Jsharp_use @ J_use # (n_use x n_use)\n",
" q_curr_use = env.get_qpos_joints(joint_names=joints_use)\n",
" err_sec_use = wrap_to_pi(q_use0 - q_curr_use) # (n_use,)\n",
" dq2_use_des = ik_gain_sec * err_sec_use # (n_use,)\n",
" dq2_use = N_use @ dq2_use_des # (n_use,)\n",
"\n",
" # Composite\n",
" dq_use = dq1_use + dq2_use # (n_use,)\n",
" dq_use = trim_scale(dq_use,th=np.deg2rad(10)) # (n_use,)\n",
" dq = S @ dq_use # (nv,), not used\n",
"\n",
" q_use = env.get_qpos_joints(joint_names=joints_use) # (n_use,)\n",
" q_use = q_use + ik_dt*dq_use\n",
" q_use_min = env.joint_mins[get_idxs(env.joint_names,joints_use)]\n",
" q_use_max = env.joint_maxs[get_idxs(env.joint_names,joints_use)]\n",
" q_use_clip = np.clip(q_use,q_use_min,q_use_max) # clip\n",
" env.forward(q=q_use_clip,joint_names=joints_use)\n",
"\n",
" sliders_joint.set_values(env.get_qpos_joints(env.rev_joint_names))\n",
" \n",
" # Render\n",
" env.viewer_text_overlay('tick','[%d]'%(tick))\n",
" env.viewer_text_overlay('geom_margin','[%.2f]mm'%(1000*env.model.geom_margin[0]))\n",
" env.viewer_text_overlay('geom_gap','[%.2f]mm'%(1000*env.model.geom_gap[0]))\n",
" env.viewer_text_overlay('#contact','[%d]'%(n_contact))\n",
" for g_idx in range(env.n_geom): env.model.geom(g_idx).rgba = geom_rgba_list[g_idx] # restore color\n",
" alpha_kwargs = {'alpha_geom':0.1,'alpha_arrow':0.1,'alpha_point':0.1,\n",
" 'color_env_geom':(0.5,0.5,0.5,0.5),'color_env_arrow':(0.5,0.5,0.5,0.2)}\n",
" env.plot_contact_info_v2(contact_info,g1_body_name_list,**alpha_kwargs)\n",
" # Plot IK information\n",
" if len(J_p_list) > 0:\n",
" colors = get_colors(M,alpha=1.0)\n",
" for idx in range(M):\n",
" p_fr = p_fr_stack[3*idx:3*(idx+1)]\n",
" p_to = p_to_stack[3*idx:3*(idx+1)]\n",
" rgba = colors[idx]\n",
" env.plot_sphere(p_fr, 0.005, rgba)\n",
" env.plot_sphere(p_to, 0.005, rgba)\n",
" env.plot_line_fr2to(p_fr, p_to, rgba)\n",
" env.render()\n",
"\n",
"# Close\n",
"sliders_joint.close()\n",
"sliders_obj1.close()\n",
"sliders_obj2.close()\n",
"sliders_geom.close()\n",
"print_green (\"Done.\")"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "00bc3553-f921-4368-86b7-e9e064fb419c",
"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