Browse Source
Add per-joint scale+offset calibration, hand tracker wrist, calibration tools
Add per-joint scale+offset calibration, hand tracker wrist, calibration tools
- Replace flat JOINT_GAIN with per-joint JOINT_SCALE/JOINT_OFFSET linear mapping for accurate retargeting (l_shoulder_pitch/roll/yaw, l_elbow calibrated) - Switch shoulder parent from chest to hips to avoid head rotation contamination - Add solo-joint mode (--solo-joint) for isolating single joints during testing - Use XRHandTracker wrist transforms in body_tracker.gd for better wrist rotation - Add joint_test.py for single-joint animation with pause-at-peak support - Add joint_calibrate.py for XR-to-robot range calibration with scale+offset output - Add calibration_log.md documenting extent tuning and axis mappings - Disable waist/right arm/hands temporarily for left-arm-only testing Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>master
melancholytron
3 weeks ago
7 changed files with 1341 additions and 657 deletions
-
19scripts/body_tracker.gd
-
131server/calibration_log.md
-
265server/joint_calibrate.py
-
255server/joint_test.py
-
90server/native_tv_wrapper.py
-
1234server/retarget_bridge.py
-
4server/teleop_server.py
@ -0,0 +1,131 @@ |
|||
# Joint Calibration Log |
|||
|
|||
Using simple basis change: `R_delta_robot = R_basis @ R_delta_xr @ R_basis.T` |
|||
Decomposition order for shoulders: YXZ Euler (matches URDF: pitch(Y), roll(X), yaw(Z)) |
|||
|
|||
## l_shoulder_pitch (joint 15) - DONE |
|||
- Robot motion: sine ±0.6 rad, period 8s (arm forward/backward) |
|||
- XR parent-child: chest -> l_upper |
|||
- **Result**: Y component (YXZ Euler) correlates positively with robot motion |
|||
- Robot +0.57 → XR Y ≈ +0.3 to +0.6 |
|||
- Robot -0.58 → XR Y ≈ -1.0 to -1.1 |
|||
- **Sign**: `SIGN_SH_L[0] = +1` (CORRECT, same sign) |
|||
- Note: X (roll) had constant negative offset (~-0.4 to -0.8), likely natural arm rotation during pitch motion |
|||
|
|||
## l_shoulder_roll (joint 16) - DONE |
|||
- Robot motion: sine ±0.6 rad, period 8s (arm out to left side) |
|||
- XR parent-child: chest -> l_upper |
|||
- **Result**: Z component (YXZ Euler) correlates with robot roll, NOT X |
|||
- Robot +0.60 → Z ≈ +0.93 to +1.23 |
|||
- Robot -0.60 → Z ≈ +0.05 (can't push arm into body) |
|||
- **Sign**: +1 (same sign) |
|||
- **Important**: This means YXZ Euler order may be wrong, or axis assignment needs swapping (roll=Z, yaw=X?) |
|||
## l_shoulder_yaw (joint 17) - DONE |
|||
- Robot motion: sine ±0.4 rad, period 8s (upper arm twist inward/outward) |
|||
- XR parent-child: chest -> l_upper |
|||
- **Result**: X component (YXZ Euler) correlates negatively with robot yaw |
|||
- Robot +0.40 → X ≈ -0.90 to -0.98 |
|||
- Robot -0.40 → X ≈ -0.001 to -0.09 |
|||
- **Sign**: -1 (inverted) |
|||
|
|||
### LEFT SHOULDER SUMMARY |
|||
- Pitch → Y component of YXZ, sign +1 |
|||
- Roll → Z component of YXZ, sign +1 |
|||
- Yaw → X component of YXZ, sign -1 |
|||
- **Effective Euler order is YZX, not YXZ!** |
|||
- Code fix: `as_euler('YZX')` gives (pitch, roll, yaw) directly, with yaw negated |
|||
## l_elbow (joint 18) - DONE |
|||
- Robot motion: sine ±0.7 rad, period 8s (elbow bend/extend) |
|||
- XR parent-child: l_upper -> l_lower |
|||
- **Result**: Z component (YXZ Euler) correlates positively with robot elbow |
|||
- Robot +0.69 → Z ≈ +0.82 to +0.92 |
|||
- Robot -0.69 → Z ≈ -0.65 to -0.93 |
|||
- **Sign**: +1 (same sign) |
|||
- Note: Currently code extracts Y[0] for elbow, should use Z[2] |
|||
## r_shoulder_pitch (joint 22) - DONE |
|||
- Robot motion: sine ±0.6 rad, period 8s (right arm forward/backward) |
|||
- XR parent-child: chest -> r_upper |
|||
- **Result**: Y component (YXZ Euler) has weak positive correlation |
|||
- Large constant offset (~0.8 rad) - user's arm drifted forward |
|||
- Robot +0.6 → Y ≈ 0.92, Robot -0.6 → Y ≈ 0.76 (difference ~0.16) |
|||
- **Sign**: +1 (same as left, weak signal) |
|||
- **Note**: Right arm Quest tracking is significantly weaker than left |
|||
## r_shoulder_roll (joint 23) - DONE |
|||
- Robot motion: sine ±0.6 rad, period 8s (right arm out to side) |
|||
- XR parent-child: chest -> r_upper |
|||
- **Result**: Z component (YXZ Euler) correlates (weak/intermittent signal) |
|||
- Robot -0.60 (arm out) → Z ≈ -0.82 to -0.96 (strong cycles) |
|||
- Robot +0.60 (arm in) → Z ≈ +0.07 |
|||
- **Sign**: +1 (same sign) |
|||
- **Note**: Very noisy, right arm Quest tracking weak |
|||
## r_shoulder_yaw (joint 24) - SKIPPED |
|||
- Assumed same as left: X component, sign -1 (right arm tracking too weak for subtle yaw) |
|||
|
|||
## r_elbow (joint 25) - DONE |
|||
- Robot motion: sine ±0.7 rad, period 8s (right elbow bend) |
|||
- XR parent-child: r_upper -> r_lower |
|||
- **Result**: ALL axes near zero (max ±0.07 despite ±0.7 robot motion) |
|||
- Quest right arm tracking is extremely weak - barely detects elbow bend |
|||
- **Assumed**: Same mapping as left elbow (Z component, sign +1) |
|||
|
|||
## EXTENT TUNING |
|||
|
|||
### l_shoulder_pitch (joint 15) |
|||
- Backward (positive): +0.8 rad (+45.8 deg) — OK |
|||
- Forward (negative): -2.4 rad (-137.5 deg) — OK |
|||
- Note: positive = backward, negative = forward for this joint |
|||
- Settings: amplitude=1.6, offset=-0.8 |
|||
|
|||
### l_shoulder_roll (joint 16) |
|||
- Inward (negative): ~0 rad (0 deg) — just touching body, no clipping |
|||
- Outward (positive): +1.585 rad (+91 deg) — arm straight out to side |
|||
- Settings: amplitude=0.8, offset=0.785 |
|||
- Note: needs outward bias to avoid body clipping |
|||
|
|||
### l_shoulder_yaw (joint 17) |
|||
- Inward (positive): +1.15 rad (+66 deg) |
|||
- Outward (negative): -1.15 rad (-66 deg) |
|||
- Settings: amplitude=1.15, offset=0.0 |
|||
- Note: clips into body at rest pose but OK since other joints move arm away in practice |
|||
|
|||
### l_elbow (joint 18) |
|||
- Extend (negative): -0.8 rad (-45.8 deg) |
|||
- Bend (positive): +1.33 rad (+76.2 deg) |
|||
- Settings: amplitude=1.065, offset=0.265 |
|||
- Note: positive = bend, negative = extend (opposite of label in joint_test.py) |
|||
|
|||
### l_wrist_roll (joint 19) |
|||
- Palm down (positive): +1.35 rad (+77.3 deg) |
|||
- Palm up (negative): -1.35 rad (-77.3 deg) |
|||
- Settings: amplitude=1.35, offset=0.0 |
|||
- Note: positive = palm down, negative = palm up (opposite of label) |
|||
|
|||
### l_wrist_pitch (joint 20) |
|||
- Wrist down (positive): +0.8 rad (+45.8 deg) |
|||
- Wrist up (negative): -0.8 rad (-45.8 deg) |
|||
- Settings: amplitude=0.8, offset=0.0 |
|||
- Directions confirmed correct |
|||
|
|||
### l_wrist_yaw (joint 21) |
|||
- Inward (negative): -1.28 rad (-73 deg) |
|||
- Outward (positive): +0.5 rad (+29 deg) |
|||
- Settings: amplitude=0.89, offset=-0.39 |
|||
- Note: negative = inward, positive = outward |
|||
|
|||
## OVERALL AXIS MAPPING SUMMARY |
|||
|
|||
After `R_basis @ R_delta @ R_basis.T` + `as_euler('YXZ')` → (Y, X, Z): |
|||
|
|||
### Shoulder (chest → upper_arm) — parent roughly upright: |
|||
- Robot pitch (Y joint) = Y component [0], sign +1 |
|||
- Robot roll (X joint) = Z component [2], sign +1 |
|||
- Robot yaw (Z joint) = X component [1], sign -1 |
|||
|
|||
### Elbow (upper_arm → lower_arm) — parent hangs down: |
|||
- Robot pitch (Y joint) = Z component [2], sign +1 |
|||
- Note: axis mapping differs from shoulder because parent orientation differs |
|||
|
|||
### Why axes differ per joint: |
|||
The basis change `R_basis @ R_delta @ R_basis.T` treats the delta as if in XR world-aligned frame, |
|||
but the delta is in the PARENT's local frame. When the parent hangs down (upper arm), its local |
|||
axes are rotated ~90° from world, causing the axis mapping to shift. |
|||
@ -0,0 +1,265 @@ |
|||
#!/usr/bin/env python3 |
|||
""" |
|||
Joint calibration tool: animates ONE robot joint while recording Quest tracking data. |
|||
|
|||
The user copies the robot's motion. By comparing the known robot joint angle |
|||
with the raw XR tracking deltas, we determine the correct axis mapping and sign. |
|||
|
|||
Usage: |
|||
python joint_calibrate.py l_shoulder_pitch [--period 6] [--amplitude 0.8] |
|||
""" |
|||
|
|||
import sys |
|||
import os |
|||
import time |
|||
import argparse |
|||
import signal |
|||
import numpy as np |
|||
from scipy.spatial.transform import Rotation |
|||
|
|||
# Add xr_teleoperate to path |
|||
sys.path.insert(0, "/home/sparky/git/xr_teleoperate") |
|||
import logging_mp |
|||
if not hasattr(logging_mp, 'getLogger'): |
|||
logging_mp.getLogger = logging_mp.get_logger |
|||
import dex_retargeting |
|||
if not hasattr(dex_retargeting, 'RetargetingConfig'): |
|||
from dex_retargeting.retargeting_config import RetargetingConfig |
|||
dex_retargeting.RetargetingConfig = RetargetingConfig |
|||
|
|||
from teleop_server import TeleopServer |
|||
from native_tv_wrapper import T_ROBOT_OPENXR, NativeTeleWrapper |
|||
|
|||
NUM_JOINTS = 29 |
|||
R3_ROBOT_OPENXR = T_ROBOT_OPENXR[:3, :3] |
|||
|
|||
JOINTS = { |
|||
'l_shoulder_pitch': 15, 'l_shoulder_roll': 16, 'l_shoulder_yaw': 17, |
|||
'l_elbow': 18, |
|||
'l_wrist_roll': 19, 'l_wrist_pitch': 20, 'l_wrist_yaw': 21, |
|||
'r_shoulder_pitch': 22, 'r_shoulder_roll': 23, 'r_shoulder_yaw': 24, |
|||
'r_elbow': 25, |
|||
} |
|||
|
|||
# Which XR parent-child pair to monitor for each robot joint |
|||
# Using hips (not chest) for shoulder to avoid head rotation contamination |
|||
JOINT_PARENTS = { |
|||
'l_shoulder_pitch': ('hips', 'l_upper'), |
|||
'l_shoulder_roll': ('hips', 'l_upper'), |
|||
'l_shoulder_yaw': ('hips', 'l_upper'), |
|||
'l_elbow': ('l_upper', 'l_lower'), |
|||
'l_wrist_roll': ('l_lower', 'l_wrist'), |
|||
'l_wrist_pitch': ('l_lower', 'l_wrist'), |
|||
'l_wrist_yaw': ('l_lower', 'l_wrist'), |
|||
'r_shoulder_pitch': ('hips', 'r_upper'), |
|||
'r_shoulder_roll': ('hips', 'r_upper'), |
|||
'r_shoulder_yaw': ('hips', 'r_upper'), |
|||
'r_elbow': ('r_upper', 'r_lower'), |
|||
} |
|||
|
|||
KP = [60,60,60,100,40,40, 60,60,60,100,40,40, 60,40,40, 40,40,40,40,40,40,40, 40,40,40,40,40,40,40] |
|||
KD = [1,1,1,2,1,1, 1,1,1,2,1,1, 1,1,1, 1,1,1,1,1,1,1, 1,1,1,1,1,1,1] |
|||
|
|||
# Calibrated extents from joint testing |
|||
DEFAULT_AMP = { |
|||
'l_shoulder_pitch': 1.6, 'l_shoulder_roll': 0.8, 'l_shoulder_yaw': 1.15, |
|||
'l_elbow': 1.065, |
|||
'l_wrist_roll': 1.35, 'l_wrist_pitch': 0.8, 'l_wrist_yaw': 0.89, |
|||
'r_shoulder_pitch': 1.6, 'r_shoulder_roll': 0.8, 'r_shoulder_yaw': 1.15, |
|||
'r_elbow': 1.065, |
|||
} |
|||
DEFAULT_OFFSET = { |
|||
'l_shoulder_pitch': -0.8, 'l_shoulder_roll': 0.785, 'l_shoulder_yaw': 0.0, |
|||
'l_elbow': 0.265, |
|||
'l_wrist_roll': 0.0, 'l_wrist_pitch': 0.0, 'l_wrist_yaw': -0.39, |
|||
'r_shoulder_pitch': -0.8, 'r_shoulder_roll': -0.785, 'r_shoulder_yaw': 0.0, |
|||
'r_elbow': 0.265, |
|||
} |
|||
|
|||
|
|||
def pose7_to_mat4(pose7): |
|||
mat = np.eye(4) |
|||
r = Rotation.from_quat(pose7[3:7]) |
|||
mat[:3, :3] = r.as_matrix() |
|||
mat[:3, 3] = pose7[:3] |
|||
return mat |
|||
|
|||
|
|||
def parse_body_joints(flat): |
|||
names = ['hips', 'chest', 'l_upper', 'l_lower', 'l_wrist', |
|||
'r_upper', 'r_lower', 'r_wrist'] |
|||
joints = {} |
|||
for i, name in enumerate(names): |
|||
p7 = flat[i*7:(i+1)*7] |
|||
if np.all(p7 == 0): |
|||
joints[name] = None |
|||
else: |
|||
joints[name] = pose7_to_mat4(p7) |
|||
return joints |
|||
|
|||
|
|||
def main(): |
|||
parser = argparse.ArgumentParser(description="Joint calibration with Quest tracking") |
|||
parser.add_argument("joint", choices=list(JOINTS.keys())) |
|||
parser.add_argument("--amplitude", "-a", type=float, default=None) |
|||
parser.add_argument("--offset", "-o", type=float, default=None) |
|||
parser.add_argument("--period", "-p", type=float, default=8.0) |
|||
parser.add_argument("--hz", type=float, default=50.0) |
|||
parser.add_argument("--port", type=int, default=8765) |
|||
parser.add_argument("--dds-domain", type=int, default=1) |
|||
parser.add_argument("--cal-delay", type=float, default=10.0, |
|||
help="Seconds to wait before calibrating XR (default: 10.0)") |
|||
args = parser.parse_args() |
|||
|
|||
joint_idx = JOINTS[args.joint] |
|||
amplitude = args.amplitude if args.amplitude is not None else DEFAULT_AMP.get(args.joint, 0.5) |
|||
offset = args.offset if args.offset is not None else DEFAULT_OFFSET.get(args.joint, 0.0) |
|||
parent_name, child_name = JOINT_PARENTS[args.joint] |
|||
|
|||
print(f"\n{'='*70}") |
|||
print(f"JOINT CALIBRATION: {args.joint} (index {joint_idx})") |
|||
print(f"Monitoring XR: {parent_name} -> {child_name}") |
|||
print(f"Animation: sine amp={amplitude:.2f} offset={offset:.2f} rad, period {args.period:.1f}s") |
|||
print(f"Range: [{offset-amplitude:.2f}, {offset+amplitude:.2f}] rad") |
|||
print(f"XR cal delay: {args.cal_delay}s") |
|||
print(f"{'='*70}") |
|||
print(f"\nStarting WebSocket server on port {args.port}...") |
|||
|
|||
# Start teleop server |
|||
tv = NativeTeleWrapper(port=args.port, host="0.0.0.0") |
|||
tv.start() |
|||
time.sleep(0.5) |
|||
|
|||
# Init DDS |
|||
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize |
|||
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ |
|||
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ |
|||
from unitree_sdk2py.utils.crc import CRC |
|||
|
|||
ChannelFactoryInitialize(args.dds_domain) |
|||
pub = ChannelPublisher("rt/lowcmd", LowCmd_) |
|||
pub.Init() |
|||
cmd = unitree_hg_msg_dds__LowCmd_() |
|||
crc = CRC() |
|||
|
|||
running = True |
|||
def handler(sig, frame): |
|||
nonlocal running |
|||
running = False |
|||
signal.signal(signal.SIGINT, handler) |
|||
signal.signal(signal.SIGTERM, handler) |
|||
|
|||
# Wait for Quest connection |
|||
print("\nConnect Quest 3 and hold the ROBOT'S REST POSE for calibration...") |
|||
print("(Arms at sides, elbows bent 90 degrees, forearms forward)") |
|||
print(f"XR calibration will happen {args.cal_delay}s after first tracking frame.\n") |
|||
|
|||
cal_ref = None |
|||
first_tracking_time = None |
|||
interval = 1.0 / args.hz |
|||
t0 = time.time() # Start animation immediately |
|||
step = 0 |
|||
|
|||
while running: |
|||
t_start = time.perf_counter() |
|||
now = time.time() |
|||
step += 1 |
|||
|
|||
# 1) Compute robot animation value |
|||
elapsed = now - t0 |
|||
robot_val = offset + amplitude * np.sin(2 * np.pi * elapsed / args.period) |
|||
angles = np.zeros(NUM_JOINTS) |
|||
angles[joint_idx] = robot_val |
|||
|
|||
# 2) Read XR tracking and compute comparison (if available) |
|||
xr_info = None |
|||
with tv.server.body_joints_shared.get_lock(): |
|||
bj_raw = np.array(tv.server.body_joints_shared[:]) |
|||
has_data = np.any(bj_raw != 0) |
|||
data_time = tv.last_data_time |
|||
tracking = data_time > 0 and (now - data_time) < 0.5 |
|||
|
|||
if tracking and has_data: |
|||
if first_tracking_time is None: |
|||
first_tracking_time = now |
|||
print(f"\nFirst tracking data! Calibrating in {args.cal_delay}s — hold the rest pose...") |
|||
|
|||
bj = parse_body_joints(bj_raw) |
|||
parent = bj.get(parent_name) |
|||
child = bj.get(child_name) |
|||
|
|||
if parent is not None and child is not None: |
|||
R_parent = parent[:3, :3] |
|||
R_child = child[:3, :3] |
|||
R_rel = R_parent.T @ R_child |
|||
|
|||
# Calibrate after delay |
|||
if cal_ref is None and first_tracking_time is not None and (now - first_tracking_time) >= args.cal_delay: |
|||
cal_ref = R_rel.copy() |
|||
print("\nCALIBRATED! Now copy the robot's motion.\n") |
|||
print(f"{'Robot Value':>12s} | {'XR Delta (robot frame YXZ Euler)':>40s} | {'XR Delta (robot frame XYZ)':>35s}") |
|||
print("-" * 95) |
|||
|
|||
if cal_ref is not None: |
|||
# Simple basis change: XR frame -> robot frame |
|||
R_delta = cal_ref.T @ R_rel |
|||
R_delta_robot = R3_ROBOT_OPENXR @ R_delta @ R3_ROBOT_OPENXR.T |
|||
yxz = Rotation.from_matrix(R_delta_robot).as_euler('YXZ') |
|||
xyz = Rotation.from_matrix(R_delta_robot).as_euler('XYZ') |
|||
xr_info = (yxz, xyz) |
|||
|
|||
# 3) Publish DDS (once per frame, always with current angles) |
|||
cmd.mode_pr = 0 |
|||
cmd.mode_machine = 0 |
|||
for i in range(NUM_JOINTS): |
|||
cmd.motor_cmd[i].mode = 1 |
|||
cmd.motor_cmd[i].q = float(angles[i]) |
|||
cmd.motor_cmd[i].dq = 0.0 |
|||
cmd.motor_cmd[i].tau = 0.0 |
|||
cmd.motor_cmd[i].kp = float(KP[i]) |
|||
cmd.motor_cmd[i].kd = float(KD[i]) |
|||
for i in range(12): |
|||
cmd.motor_cmd[i].mode = 0 |
|||
cmd.motor_cmd[i].kp = 0.0 |
|||
cmd.motor_cmd[i].kd = 0.0 |
|||
cmd.crc = crc.Crc(cmd) |
|||
pub.Write(cmd) |
|||
|
|||
# 4) Log |
|||
if xr_info and step % int(args.hz) == 0: |
|||
yxz, xyz = xr_info |
|||
print(f" {robot_val:+.3f} rad | " |
|||
f"Y={yxz[0]:+.3f} X={yxz[1]:+.3f} Z={yxz[2]:+.3f} | " |
|||
f"X={xyz[0]:+.3f} Y={xyz[1]:+.3f} Z={xyz[2]:+.3f}") |
|||
elif not xr_info and step % int(args.hz * 2) == 0: |
|||
print(f" robot={robot_val:+.3f} (Waiting for Quest tracking data...)") |
|||
|
|||
sleep_time = interval - (time.perf_counter() - t_start) |
|||
if sleep_time > 0: |
|||
time.sleep(sleep_time) |
|||
|
|||
# Return to zero |
|||
print("\nReturning to zero...") |
|||
angles = np.zeros(NUM_JOINTS) |
|||
for _ in range(25): |
|||
cmd.mode_pr = 0 |
|||
cmd.mode_machine = 0 |
|||
for i in range(NUM_JOINTS): |
|||
cmd.motor_cmd[i].mode = 1 |
|||
cmd.motor_cmd[i].q = 0.0 |
|||
cmd.motor_cmd[i].dq = 0.0 |
|||
cmd.motor_cmd[i].tau = 0.0 |
|||
cmd.motor_cmd[i].kp = float(KP[i]) |
|||
cmd.motor_cmd[i].kd = float(KD[i]) |
|||
for i in range(12): |
|||
cmd.motor_cmd[i].mode = 0 |
|||
cmd.motor_cmd[i].kp = 0.0 |
|||
cmd.motor_cmd[i].kd = 0.0 |
|||
cmd.crc = crc.Crc(cmd) |
|||
pub.Write(cmd) |
|||
time.sleep(0.02) |
|||
print("Done.") |
|||
|
|||
|
|||
if __name__ == "__main__": |
|||
main() |
|||
@ -0,0 +1,255 @@ |
|||
#!/usr/bin/env python3 |
|||
""" |
|||
Systematic joint-by-joint testing for sign calibration. |
|||
|
|||
Animates ONE joint at a time with a slow sine wave so the user can see |
|||
exactly what each joint does, then match the motion with their Quest 3. |
|||
|
|||
Usage: |
|||
python joint_test.py <joint_name> [--amplitude 0.5] [--period 4.0] |
|||
|
|||
Joint names: |
|||
waist_yaw (joint 12) - Z axis |
|||
waist_roll (joint 13) - X axis |
|||
waist_pitch (joint 14) - Y axis |
|||
l_shoulder_pitch (joint 15) - Y axis |
|||
l_shoulder_roll (joint 16) - X axis |
|||
l_shoulder_yaw (joint 17) - Z axis |
|||
l_elbow (joint 18) - Y axis |
|||
l_wrist_roll (joint 19) - X axis |
|||
l_wrist_pitch (joint 20) - Y axis |
|||
l_wrist_yaw (joint 21) - Z axis |
|||
r_shoulder_pitch (joint 22) - Y axis |
|||
r_shoulder_roll (joint 23) - X axis |
|||
r_shoulder_yaw (joint 24) - Z axis |
|||
r_elbow (joint 25) - Y axis |
|||
r_wrist_roll (joint 26) - X axis |
|||
r_wrist_pitch (joint 27) - Y axis |
|||
r_wrist_yaw (joint 28) - Z axis |
|||
|
|||
Also: "list" to show all joints, "all_shoulders" to cycle through shoulder joints |
|||
""" |
|||
|
|||
import sys |
|||
import os |
|||
import time |
|||
import argparse |
|||
import signal |
|||
import numpy as np |
|||
|
|||
NUM_JOINTS = 29 |
|||
|
|||
JOINTS = { |
|||
'waist_yaw': 12, |
|||
'waist_roll': 13, |
|||
'waist_pitch': 14, |
|||
'l_shoulder_pitch': 15, |
|||
'l_shoulder_roll': 16, |
|||
'l_shoulder_yaw': 17, |
|||
'l_elbow': 18, |
|||
'l_wrist_roll': 19, |
|||
'l_wrist_pitch': 20, |
|||
'l_wrist_yaw': 21, |
|||
'r_shoulder_pitch': 22, |
|||
'r_shoulder_roll': 23, |
|||
'r_shoulder_yaw': 24, |
|||
'r_elbow': 25, |
|||
'r_wrist_roll': 26, |
|||
'r_wrist_pitch': 27, |
|||
'r_wrist_yaw': 28, |
|||
} |
|||
|
|||
# Joint descriptions - what POSITIVE values should do |
|||
DESCRIPTIONS = { |
|||
'waist_yaw': 'Torso rotates LEFT (counterclockwise from above)', |
|||
'waist_roll': 'Torso leans RIGHT', |
|||
'waist_pitch': 'Torso leans FORWARD', |
|||
'l_shoulder_pitch': 'Left arm swings FORWARD', |
|||
'l_shoulder_roll': 'Left arm raises OUT to the LEFT side', |
|||
'l_shoulder_yaw': 'Left upper arm rotates INWARD', |
|||
'l_elbow': 'Left elbow BENDS (forearm toward shoulder)', |
|||
'l_wrist_roll': 'Left wrist rotates (palm faces down→up)', |
|||
'l_wrist_pitch': 'Left wrist bends DOWN (flexion)', |
|||
'l_wrist_yaw': 'Left wrist bends toward THUMB side', |
|||
'r_shoulder_pitch': 'Right arm swings FORWARD', |
|||
'r_shoulder_roll': 'Right arm raises OUT to the RIGHT side (NEGATIVE values)', |
|||
'r_shoulder_yaw': 'Right upper arm rotates INWARD', |
|||
'r_elbow': 'Right elbow BENDS (forearm toward shoulder)', |
|||
'r_wrist_roll': 'Right wrist rotates', |
|||
'r_wrist_pitch': 'Right wrist bends DOWN (flexion)', |
|||
'r_wrist_yaw': 'Right wrist bends toward THUMB side', |
|||
} |
|||
|
|||
# Default amplitudes (stay within safe range) |
|||
DEFAULT_AMPLITUDES = { |
|||
'waist_yaw': 0.4, |
|||
'waist_roll': 0.3, |
|||
'waist_pitch': 0.3, |
|||
'l_shoulder_pitch': 0.8, |
|||
'l_shoulder_roll': 0.8, |
|||
'l_shoulder_yaw': 0.5, |
|||
'l_elbow': 0.8, |
|||
'l_wrist_roll': 0.5, |
|||
'l_wrist_pitch': 0.5, |
|||
'l_wrist_yaw': 0.5, |
|||
'r_shoulder_pitch': 0.8, |
|||
'r_shoulder_roll': 0.8, |
|||
'r_shoulder_yaw': 0.5, |
|||
'r_elbow': 0.8, |
|||
'r_wrist_roll': 0.5, |
|||
'r_wrist_pitch': 0.5, |
|||
'r_wrist_yaw': 0.5, |
|||
} |
|||
|
|||
# Default Kp/Kd gains |
|||
KP = [ |
|||
60, 60, 60, 100, 40, 40, |
|||
60, 60, 60, 100, 40, 40, |
|||
60, 40, 40, |
|||
40, 40, 40, 40, 40, 40, 40, |
|||
40, 40, 40, 40, 40, 40, 40, |
|||
] |
|||
KD = [ |
|||
1, 1, 1, 2, 1, 1, |
|||
1, 1, 1, 2, 1, 1, |
|||
1, 1, 1, |
|||
1, 1, 1, 1, 1, 1, 1, |
|||
1, 1, 1, 1, 1, 1, 1, |
|||
] |
|||
|
|||
|
|||
def init_dds(domain_id=1): |
|||
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize |
|||
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ |
|||
from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ |
|||
from unitree_sdk2py.utils.crc import CRC |
|||
|
|||
ChannelFactoryInitialize(domain_id) |
|||
pub = ChannelPublisher("rt/lowcmd", LowCmd_) |
|||
pub.Init() |
|||
cmd = unitree_hg_msg_dds__LowCmd_() |
|||
crc = CRC() |
|||
return pub, cmd, crc |
|||
|
|||
|
|||
def publish(pub, cmd, crc, angles): |
|||
cmd.mode_pr = 0 |
|||
cmd.mode_machine = 0 |
|||
for i in range(NUM_JOINTS): |
|||
cmd.motor_cmd[i].mode = 1 |
|||
cmd.motor_cmd[i].q = float(angles[i]) |
|||
cmd.motor_cmd[i].dq = 0.0 |
|||
cmd.motor_cmd[i].tau = 0.0 |
|||
cmd.motor_cmd[i].kp = float(KP[i]) |
|||
cmd.motor_cmd[i].kd = float(KD[i]) |
|||
# Zero legs |
|||
for i in range(12): |
|||
cmd.motor_cmd[i].mode = 0 |
|||
cmd.motor_cmd[i].kp = 0.0 |
|||
cmd.motor_cmd[i].kd = 0.0 |
|||
cmd.crc = crc.Crc(cmd) |
|||
pub.Write(cmd) |
|||
|
|||
|
|||
def main(): |
|||
parser = argparse.ArgumentParser(description="Joint-by-joint animation tester") |
|||
parser.add_argument("joint", help="Joint name (or 'list' to show all)") |
|||
parser.add_argument("--amplitude", "-a", type=float, default=None, |
|||
help="Sine wave amplitude in radians (default: joint-specific)") |
|||
parser.add_argument("--period", "-p", type=float, default=4.0, |
|||
help="Sine wave period in seconds (default: 4.0)") |
|||
parser.add_argument("--hz", type=float, default=50.0) |
|||
parser.add_argument("--offset", "-o", type=float, default=0.0, |
|||
help="Static offset added to sine wave") |
|||
parser.add_argument("--dds-domain", type=int, default=1) |
|||
parser.add_argument("--pause-at-peak", choices=["pos", "neg"], default=None, |
|||
help="Pause at peak each cycle: 'pos' or 'neg'") |
|||
parser.add_argument("--pause-duration", type=float, default=3.0, |
|||
help="How long to pause in seconds (default: 3.0)") |
|||
args = parser.parse_args() |
|||
|
|||
if args.joint == 'list': |
|||
print("\nAvailable joints:\n") |
|||
for name, idx in sorted(JOINTS.items(), key=lambda x: x[1]): |
|||
desc = DESCRIPTIONS.get(name, '') |
|||
amp = DEFAULT_AMPLITUDES.get(name, 0.5) |
|||
print(f" {name:20s} (joint {idx:2d}) +val: {desc}") |
|||
print() |
|||
return |
|||
|
|||
if args.joint not in JOINTS: |
|||
print(f"Unknown joint: {args.joint}") |
|||
print(f"Available: {', '.join(sorted(JOINTS.keys()))}") |
|||
return |
|||
|
|||
joint_idx = JOINTS[args.joint] |
|||
amplitude = args.amplitude if args.amplitude is not None else DEFAULT_AMPLITUDES.get(args.joint, 0.5) |
|||
desc = DESCRIPTIONS.get(args.joint, '') |
|||
|
|||
print(f"\n{'='*60}") |
|||
print(f"Testing: {args.joint} (joint index {joint_idx})") |
|||
print(f"Positive direction: {desc}") |
|||
print(f"Animation: sine wave, amplitude={amplitude:.2f} rad, period={args.period:.1f}s") |
|||
print(f"Offset: {args.offset:.2f} rad") |
|||
print(f"{'='*60}\n") |
|||
|
|||
pub, cmd, crc = init_dds(args.dds_domain) |
|||
|
|||
running = True |
|||
def handler(sig, frame): |
|||
nonlocal running |
|||
running = False |
|||
signal.signal(signal.SIGINT, handler) |
|||
signal.signal(signal.SIGTERM, handler) |
|||
|
|||
interval = 1.0 / args.hz |
|||
t0 = time.time() |
|||
step = 0 |
|||
|
|||
print("Animating... (Ctrl+C to stop)\n") |
|||
|
|||
last_pause_time = 0 |
|||
while running: |
|||
t_start = time.perf_counter() |
|||
elapsed = time.time() - t0 |
|||
angles = np.zeros(NUM_JOINTS) |
|||
|
|||
value = args.offset + amplitude * np.sin(2 * np.pi * elapsed / args.period) |
|||
angles[joint_idx] = value |
|||
|
|||
publish(pub, cmd, crc, angles) |
|||
|
|||
step += 1 |
|||
if step % int(args.hz * 1) == 0: |
|||
print(f" {args.joint} = {value:+.3f} rad ({np.degrees(value):+.1f} deg)") |
|||
|
|||
# Pause at peak (with cooldown to avoid re-triggering) |
|||
if args.pause_at_peak and (time.time() - last_pause_time) > args.period * 0.5: |
|||
if args.pause_at_peak == "pos": |
|||
peak_val = args.offset + amplitude |
|||
else: |
|||
peak_val = args.offset - amplitude |
|||
if abs(value - peak_val) < 0.05: |
|||
print(f"\n >>> PAUSED at {args.pause_at_peak} peak: {value:+.3f} rad ({np.degrees(value):+.1f} deg) <<<") |
|||
pause_end = time.time() + args.pause_duration |
|||
while time.time() < pause_end and running: |
|||
publish(pub, cmd, crc, angles) |
|||
time.sleep(interval) |
|||
last_pause_time = time.time() |
|||
print(f" >>> Resuming...\n") |
|||
|
|||
sleep_time = interval - (time.perf_counter() - t_start) |
|||
if sleep_time > 0: |
|||
time.sleep(sleep_time) |
|||
|
|||
# Return to zero |
|||
print("\nReturning to zero...") |
|||
angles = np.zeros(NUM_JOINTS) |
|||
for _ in range(25): |
|||
publish(pub, cmd, crc, angles) |
|||
time.sleep(0.02) |
|||
print("Done.") |
|||
|
|||
|
|||
if __name__ == "__main__": |
|||
main() |
|||
1234
server/retarget_bridge.py
File diff suppressed because it is too large
View File
File diff suppressed because it is too large
View File
Write
Preview
Loading…
Cancel
Save
Reference in new issue