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
4 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