xiaohu-art · fro-sting · Feb 23, 2026 · Feb 23, 2026 · Feb 23, 2026
diff --git a/.gitignore b/.gitignore
@@ -58,8 +58,11 @@ temp/
 data/AMASS/Transitions/
 data/g1_29dof/
 data/smpl/
+data/
 
 # Pickle files
 # *.pkl
 
-homework.md
+homework.md
+
+assets/robot/gr3_6dof
diff --git a/README.md b/README.md
@@ -32,6 +32,7 @@ Of course, you can download the other motion files you like from the [AMASS](htt
 ## Fitting motion data from AMASS
 
 First we need to fit the smpl shape to the humanoid robot:
+export XDG_SESSION_TYPE=x11
 ```bash
 uv run scripts/1-fit_smpl_shape.py
 ```

diff --git a/cfg/fourier_gr3_fitting.yaml b/cfg/fourier_gr3_fitting.yaml
@@ -0,0 +1,58 @@
+humanoid_type: gr3_6dof
+
+asset:
+  assetFileName: "assets/robot/gr3_6dof/gr3_6dof/urdf/gr3_fourier_hand_6dof.urdf"
+
+extend_config:
+  - joint_name: "left_toe_link"
+    parent_name: "left_foot_roll_link"
+    pos: [0.1, 0.0, -0.02]
+    rot: [1.0, 0.0, 0.0, 0.0]
+
+  - joint_name: "right_toe_link"
+    parent_name: "right_foot_roll_link"
+    pos: [0.1, 0.0, -0.02]
+    rot: [1.0, 0.0, 0.0, 0.0]
+
+base_link: "base_link"
+joint_matches:
+  # - ["base_link", "Pelvis"]
+  - ["left_thigh_pitch_link", "L_Hip"]
+  - ["left_shank_pitch_link", "L_Knee"]
+  - ["left_foot_roll_link", "L_Ankle"]
+  - ["right_thigh_pitch_link", "R_Hip"]
+  - ["right_shank_pitch_link", "R_Knee"]
+  - ["right_foot_roll_link", "R_Ankle"]
+  - ["left_upper_arm_roll_link", "L_Shoulder"]
+  - ["left_lower_arm_pitch_link", "L_Elbow"]
+  - ["left_hand_roll_link", "L_Hand"]
+  - ["right_upper_arm_roll_link", "R_Shoulder"]
+  - ["right_lower_arm_pitch_link", "R_Elbow"]
+  - ["right_hand_roll_link", "R_Hand"]
+  - ["left_toe_link", "L_Toe"]
+  - ["right_toe_link", "R_Toe"]
+
+
+smpl_pose_modifier:
+  Pelvis: "[np.pi/2, 0, np.pi/2]"
+  L_Shoulder: "[0, 0, -np.pi/2]"
+  R_Shoulder: "[0, 0, np.pi/2]"
+  # L_Elbow: "[0, -np.pi/2, 0]"
+  # R_Elbow: "[0, np.pi/2, 0]"
+
+quat_offset:
+  Pelvis: "[0.5, -0.5, -0.5, -0.5]"
+  L_Hip: "[0.5, -0.5, -0.5, -0.5]"
+  L_Knee: "[0.5, -0.5, -0.5, -0.5]"
+  L_Ankle: "[0.5, -0.5, -0.5, -0.5]"
+  R_Hip: "[0.5, -0.5, -0.5, -0.5]"
+  R_Knee: "[0.5, -0.5, -0.5, -0.5]"
+  R_Ankle: "[0.5, -0.5, -0.5, -0.5]"
+  L_Toe: "[0.5, -0.5, -0.5, -0.5]"
+  R_Toe: "[0.5, -0.5, -0.5, -0.5]"
+  L_Shoulder: "[0.7071, 0, -0.7071, 0]"
+  L_Elbow: "[-0.7071067811865475, 0.0, 0.7071067811865475, 0.0]"
+  L_Hand: "[-0.7071067811865475, 0.0, 0.7071067811865475, 0.0]"
+  R_Shoulder: "[0, 0.7071, 0, 0.7071]"
+  R_Elbow: "[0.0, 0.7071067811865475, 0.0, 0.7071067811865475]"
+  R_Hand: "[0.0, 0.7071067811865475, 0.0, 0.7071067811865475]"
diff --git a/check_root.py b/check_root.py
@@ -0,0 +1,18 @@
+import torch
+import pytorch_kinematics as pk
+import xml.etree.ElementTree as ET
+import os
+
+path = "assets/robot/gr3_6dof/gr3_6dof/urdf/gr3_fourier_hand_6dof.urdf"
+tree = ET.parse(path)
+root = tree.getroot()
+chain = pk.build_chain_from_urdf(ET.tostring(root, encoding='utf8'))
+
+print("Chain root:", chain.get_link_names()[0])
+print("Joint names:", chain.get_joint_parent_child_names())
+
+th = torch.zeros([1, chain.n_joints])
+pos = chain.forward_kinematics(th)
+print("Base link pose:", pos['base_link'].get_matrix())
+print("Right hip pose:", pos['right_thigh_pitch_link'].get_matrix())
+print("Left hip pose:", pos['left_thigh_pitch_link'].get_matrix())
diff --git a/check_smpl.py b/check_smpl.py
@@ -0,0 +1,19 @@
+import torch
+from smplx import SMPL
+import os
+
+DATA_PATH = "data"
+body_model = SMPL(model_path=os.path.join(DATA_PATH, "smpl"), gender="neutral")
+betas = torch.zeros([1, 10])
+result = body_model(betas=betas)
+joints = result.joints[0] # [24, 3]
+pelvis = joints[0]
+l_hip = joints[1]
+r_hip = joints[2]
+l_shoulder = joints[16]
+r_shoulder = joints[17]
+
+print("SMPL Pelvis:", pelvis.tolist())
+print("SMPL L_Hip relative to Pelvis:", (l_hip - pelvis).tolist())
+print("SMPL R_Hip relative to Pelvis:", (r_hip - pelvis).tolist())
+print("SMPL L_Shoulder relative to Pelvis:", (l_shoulder - pelvis).tolist())
diff --git a/data/AMASS/SFU/0005/twistdance_kick_stageii.npz b/data/AMASS/SFU/0005/twistdance_kick_stageii.npz
diff --git a/data/SFU.tar.bz2 b/data/SFU.tar.bz2
diff --git a/meta.json b/meta.json
@@ -1,72 +1,134 @@
 {
     "body_names": [
-        "pelvis",
-        "left_hip_pitch_link",
-        "left_hip_roll_link",
-        "left_hip_yaw_link",
-        "left_knee_link",
-        "left_ankle_pitch_link",
-        "left_ankle_roll_link",
-        "left_toe_link",
-        "right_hip_pitch_link",
-        "right_hip_roll_link",
-        "right_hip_yaw_link",
-        "right_knee_link",
-        "right_ankle_pitch_link",
-        "right_ankle_roll_link",
-        "right_toe_link",
+        "base_link",
         "waist_yaw_link",
         "waist_roll_link",
+        "waist_pitch_link",
         "torso_link",
-        "left_shoulder_pitch_link",
-        "left_shoulder_roll_link",
-        "left_shoulder_yaw_link",
-        "left_elbow_link",
-        "left_wrist_roll_link",
-        "left_wrist_pitch_link",
-        "left_wrist_yaw_link",
-        "left_rubber_hand",
-        "right_shoulder_pitch_link",
-        "right_shoulder_roll_link",
-        "right_shoulder_yaw_link",
-        "right_elbow_link",
-        "right_wrist_roll_link",
-        "right_wrist_pitch_link",
-        "right_wrist_yaw_link",
-        "right_rubber_hand",
-        "imu_in_torso",
-        "head_link",
-        "imu_in_pelvis"
+        "head_yaw_link",
+        "head_pitch_link",
+        "camera_link",
+        "right_upper_arm_pitch_link",
+        "right_upper_arm_roll_link",
+        "right_upper_arm_yaw_link",
+        "right_lower_arm_pitch_link",
+        "right_hand_yaw_link",
+        "right_hand_pitch_link",
+        "right_hand_roll_link",
+        "right_end_effector_link",
+        "R_hand_base_link",
+        "R_hand_wrist_link",
+        "R_thumb_proximal_yaw_link",
+        "R_thumb_proximal_pitch_link",
+        "R_thumb_distal_link",
+        "R_thumb_tip_link",
+        "R_index_proximal_link",
+        "R_index_intermediate_link",
+        "R_index_tip_link",
+        "R_middle_proximal_link",
+        "R_middle_intermediate_link",
+        "R_middle_tip_link",
+        "R_ring_proximal_link",
+        "R_ring_intermediate_link",
+        "R_ring_tip_link",
+        "R_pinky_proximal_link",
+        "R_pinky_intermediate_link",
+        "R_pinky_tip_link",
+        "left_upper_arm_pitch_link",
+        "left_upper_arm_roll_link",
+        "left_upper_arm_yaw_link",
+        "left_lower_arm_pitch_link",
+        "left_hand_yaw_link",
+        "left_hand_pitch_link",
+        "left_hand_roll_link",
+        "left_end_effector_link",
+        "L_hand_base_link",
+        "L_hand_wrist_link",
+        "L_thumb_proximal_yaw_link",
+        "L_thumb_proximal_pitch_link",
+        "L_thumb_distal_link",
+        "L_thumb_tip_link",
+        "L_index_proximal_link",
+        "L_index_intermediate_link",
+        "L_index_tip_link",
+        "L_middle_proximal_link",
+        "L_middle_intermediate_link",
+        "L_middle_tip_link",
+        "L_ring_proximal_link",
+        "L_ring_intermediate_link",
+        "L_ring_tip_link",
+        "L_pinky_proximal_link",
+        "L_pinky_intermediate_link",
+        "L_pinky_tip_link",
+        "right_thigh_pitch_link",
+        "right_thigh_roll_link",
+        "right_thigh_yaw_link",
+        "right_shank_pitch_link",
+        "right_foot_pitch_link",
+        "right_foot_roll_link",
+        "right_toe_link",
+        "left_thigh_pitch_link",
+        "left_thigh_roll_link",
+        "left_thigh_yaw_link",
+        "left_shank_pitch_link",
+        "left_foot_pitch_link",
+        "left_foot_roll_link",
+        "left_toe_link",
+        "imu_link"
     ],
     "joint_names": [
-        "left_hip_pitch_joint",
-        "left_hip_roll_joint",
-        "left_hip_yaw_joint",
-        "left_knee_joint",
-        "left_ankle_pitch_joint",
-        "left_ankle_roll_joint",
-        "right_hip_pitch_joint",
-        "right_hip_roll_joint",
-        "right_hip_yaw_joint",
-        "right_knee_joint",
-        "right_ankle_pitch_joint",
-        "right_ankle_roll_joint",
         "waist_yaw_joint",
         "waist_roll_joint",
         "waist_pitch_joint",
-        "left_shoulder_pitch_joint",
-        "left_shoulder_roll_joint",
-        "left_shoulder_yaw_joint",
-        "left_elbow_joint",
-        "left_wrist_roll_joint",
-        "left_wrist_pitch_joint",
-        "left_wrist_yaw_joint",
+        "head_yaw_joint",
+        "head_pitch_joint",
         "right_shoulder_pitch_joint",
         "right_shoulder_roll_joint",
         "right_shoulder_yaw_joint",
-        "right_elbow_joint",
-        "right_wrist_roll_joint",
+        "right_elbow_pitch_joint",
+        "right_wrist_yaw_joint",
         "right_wrist_pitch_joint",
-        "right_wrist_yaw_joint"
+        "right_wrist_roll_joint",
+        "R_thumb_proximal_yaw_joint",
+        "R_thumb_proximal_pitch_joint",
+        "R_thumb_distal_joint",
+        "R_index_proximal_joint",
+        "R_index_intermediate_joint",
+        "R_middle_proximal_joint",
+        "R_middle_intermediate_joint",
+        "R_ring_proximal_joint",
+        "R_ring_intermediate_joint",
+        "R_pinky_proximal_joint",
+        "R_pinky_intermediate_joint",
+        "left_shoulder_pitch_joint",
+        "left_shoulder_roll_joint",
+        "left_shoulder_yaw_joint",
+        "left_elbow_pitch_joint",
+        "left_wrist_yaw_joint",
+        "left_wrist_pitch_joint",
+        "left_wrist_roll_joint",
+        "L_thumb_proximal_yaw_joint",
+        "L_thumb_proximal_pitch_joint",
+        "L_thumb_distal_joint",
+        "L_index_proximal_joint",
+        "L_index_intermediate_joint",
+        "L_middle_proximal_joint",
+        "L_middle_intermediate_joint",
+        "L_ring_proximal_joint",
+        "L_ring_intermediate_joint",
+        "L_pinky_proximal_joint",
+        "L_pinky_intermediate_joint",
+        "right_hip_pitch_joint",
+        "right_hip_roll_joint",
+        "right_hip_yaw_joint",
+        "right_knee_pitch_joint",
+        "right_ankle_pitch_joint",
+        "right_ankle_roll_joint",
+        "left_hip_pitch_joint",
+        "left_hip_roll_joint",
+        "left_hip_yaw_joint",
+        "left_knee_pitch_joint",
+        "left_ankle_pitch_joint",
+        "left_ankle_roll_joint"
     ]
 }