Skip to content

Instantly share code, notes, and snippets.

@sjchoi86

sjchoi86/g1_contact_model_explained.ipynb

Created October 20, 2025 10:49
Show Gist options

  • Select an option

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

Select an option

Learn more about clone URLs
Save sjchoi86/866127d079166e3a9db37a41879cd05d 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_model_explained.ipynb
{
"cells": [
{
"cell_type": "markdown",
"id": "e664c9a5-aaaf-4771-9c23-a7e9d0fda36f",
"metadata": {},
"source": [
"### Contact modeling using `G1`"
]
},
{
"cell_type": "code",
"execution_count": 1,
"id": "cc4c8494-44e6-459f-94c2-1adf53b37152",
"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": 18,
"id": "18529aa4-cf48-43b5-ac2f-48b39755fcf2",
"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())"
]
},
{
"cell_type": "code",
"execution_count": 20,
"id": "879fd39b-c46d-40cb-9b21-58207871b4e8",
"metadata": {},
"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",
"# 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",
"\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)\n",
"\n",
"# Reset sliders\n",
"slider_kwargs = {'window_width':500,'slider_width':300,'label_width':150}\n",
"sliders_joint = MultiSliderQtClass(\n",
" y_offset=0,window_height=800,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=900,window_height=100,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=1050,window_height=100,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=1200,window_height=100,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",
"# Loop\n",
"tick = 0\n",
"while env.is_viewer_alive():\n",
" # Update\n",
" tick = 0\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 = contact_info['n_contact']\n",
" p_contact_list = contact_info['p_contact_list']\n",
" geom1_idx_list = contact_info['geom1_idx_list']\n",
" geom2_idx_list = contact_info['geom2_idx_list']\n",
" body1_idx_list = contact_info['body1_idx_list']\n",
" body2_idx_list = contact_info['body2_idx_list']\n",
" norm_dir_list = contact_info['norm_dir_list']\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",
" \n",
" for g_idx in range(env.n_geom): env.model.geom(g_idx).rgba = geom_rgba_list[g_idx] # restore color\n",
" colors_geom = get_colors(n_color=n_contact*2,cmap_name='rainbow',alpha=0.75)\n",
" colors_arrow = get_colors(n_color=n_contact*2,cmap_name='rainbow',alpha=0.9)\n",
" color_dark_geom,color_dark_arrow = (0.5,0.5,0.2,0.75),(0.5,0.5,0.2,0.1)\n",
" for c_idx in range(n_contact):\n",
" p_contact = p_contact_list[c_idx]\n",
" env.plot_sphere(p_contact, 0.0025, (0,0,0,1)) # sphere\n",
" geom1_idx,geom2_idx = geom1_idx_list[c_idx],geom2_idx_list[c_idx]\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",
" color_geom1,color_geom2 = colors_geom[2*c_idx],colors_geom[2*c_idx+1]\n",
" color_arrow1,color_arrow2 = colors_arrow[2*c_idx],colors_arrow[2*c_idx+1]\n",
" geom1_idxs = [idx for idx,val in enumerate(env.model.geom_bodyid) if val==body1_idx]\n",
" geom2_idxs = [idx for idx,val in enumerate(env.model.geom_bodyid) if val==body2_idx]\n",
" norm_dir = norm_dir_list[c_idx] # (3,)\n",
" p1_to = p_contact - 0.05*norm_dir\n",
" p2_to = p_contact + 0.05*norm_dir\n",
" if body1_name in g1_body_name_list:\n",
" for geom1_idx in geom1_idxs: env.model.geom(geom1_idx).rgba = color_geom1\n",
" env.plot_arrow_fr2to(p_contact, p1_to, 0.005, color_arrow1) # arrow\n",
" else:\n",
" for geom1_idx in geom1_idxs: env.model.geom(geom1_idx).rgba = color_dark_geom\n",
" env.plot_arrow_fr2to(p_contact, p1_to, 0.005, color_dark_arrow) # arrow \n",
" if body2_name in g1_body_name_list:\n",
" for geom2_idx in geom2_idxs: env.model.geom(geom2_idx).rgba = color_geom2\n",
" env.plot_arrow_fr2to(p_contact, p2_to, 0.005, color_arrow2) # arrow\n",
" else:\n",
" for geom2_idx in geom2_idxs: env.model.geom(geom2_idx).rgba = color_dark_geom\n",
" env.plot_arrow_fr2to(p_contact, p2_to, 0.005, color_dark_arrow) # arrow\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": "5f12a525-d785-4df1-b921-b09b0f139978",
"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