ZhengLiu-cart/acRealman_xr_from_Yikai
1
0
Fork 0
forked from YikaiFu-cart/acRealman_xr
Code Pull Requests Activity

Enhance orientation control for RM75 arms in XR teleoperation

Browse Source
This commit is contained in:
YikaiFu-cart
2026-06-09 14:50:04 +08:00
parent 7f8ebefadc
commit 07517e0c49
13 changed files with 606 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

127
xr_rm_input/xr_rm_input/sample_udp_sender.py
View File

@ -32,6 +32,14 @@ AXIS_SWEEP_SEGMENTS = (
("XR -Z front", (0.0, 0.0, -1.0), "hold"),
("CENTER / Z done", (0.0, 0.0, 0.0), "center"),
)
RPY_DIRECTIONS = (
("XR roll +", (1.0, 0.0, 0.0)),
("XR roll -", (-1.0, 0.0, 0.0)),
("XR pitch +", (0.0, 1.0, 0.0)),
("XR pitch -", (0.0, -1.0, 0.0)),
("XR yaw +", (0.0, 0.0, 1.0)),
("XR yaw -", (0.0, 0.0, -1.0)),
)
def _validate_positive(name: str, value: float) -> float:
@ -66,6 +74,12 @@ def _pattern_period(pattern: str, hold_seconds: float, center_seconds: float) ->
return 5.0
def _rotation_pattern_period(pattern: str, hold_seconds: float, center_seconds: float) -> float:
if pattern == "rpy_steps":
return (hold_seconds + center_seconds) * len(RPY_DIRECTIONS)
return 0.0
def _axis_sweep_position(
t: float,
amplitude: float,
@ -121,6 +135,66 @@ def _sine_position(t: float, amplitude: float, phase: float) -> tuple[str, list[
]
def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> list[float]:
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
return [
sr * cp * cy - cr * sp * sy,
cr * sp * cy + sr * cp * sy,
cr * cp * sy - sr * sp * cy,
cr * cp * cy + sr * sp * sy,
]
def _rpy_steps_quaternion(
t: float,
amplitude_rad: float,
hold_seconds: float,
center_seconds: float,
initial_center_seconds: float,
) -> tuple[str, list[float]]:
if t < initial_center_seconds:
return "ROT center / lock origin", [0.0, 0.0, 0.0, 1.0]
segment_seconds = hold_seconds + center_seconds
period = segment_seconds * len(RPY_DIRECTIONS)
cycle_t = (t - initial_center_seconds) % period
direction_index = int(cycle_t // segment_seconds)
segment_t = cycle_t - direction_index * segment_seconds
label, direction = RPY_DIRECTIONS[direction_index]
if segment_t >= hold_seconds:
return f"ROT center after {label}", [0.0, 0.0, 0.0, 1.0]
return label, _euler_to_quaternion(
amplitude_rad * direction[0],
amplitude_rad * direction[1],
amplitude_rad * direction[2],
)
def _rotation_quaternion(
pattern: str,
t: float,
amplitude_rad: float,
hold_seconds: float,
center_seconds: float,
initial_center_seconds: float,
) -> tuple[str, list[float]]:
if pattern == "rpy_steps":
return _rpy_steps_quaternion(
t,
amplitude_rad,
hold_seconds,
center_seconds,
initial_center_seconds,
)
return "ROT none", [0.0, 0.0, 0.0, 1.0]
def _pattern_position(
pattern: str,
t: float,
@ -186,6 +260,18 @@ def main() -> None:
default=0.0,
help="发送的 trigger 值。方向调试默认不驱动夹爪。",
)
parser.add_argument(
"--rotation-pattern",
choices=("none", "rpy_steps"),
default="none",
help="可选姿态测试轨迹；none 保持 identity 四元数，rpy_steps 逐轴发送小角度姿态。",
)
parser.add_argument(
"--rotation-amplitude-deg",
type=_positive_float("rotation-amplitude-deg"),
default=25.0,
help="rpy_steps 模式下的姿态幅度，单位 degree。",
)
parser.add_argument("--hand", choices=("left", "right", "both"), default="right")
parser.add_argument(
"--both-mode",
@ -212,17 +298,22 @@ def main() -> None:
}
for hand in hands
}
pattern_period = _pattern_period(args.pattern, args.hold_seconds, args.center_seconds)
pattern_period = max(
_pattern_period(args.pattern, args.hold_seconds, args.center_seconds),
_rotation_pattern_period(args.rotation_pattern, args.hold_seconds, args.center_seconds),
)
one_hand_seconds = args.initial_center_seconds + pattern_period
full_sweep_seconds = one_hand_seconds * len(hands) if args.both_mode == "staggered" else one_hand_seconds
rotation_amplitude_rad = math.radians(args.rotation_amplitude_deg)
print(
"Sample UDP sender: "
f"hand={args.hand}, both_mode={args.both_mode}, pattern={args.pattern}, "
f"rotation_pattern={args.rotation_pattern}, "
f"amplitude={args.amplitude:.3f}m, hold={args.hold_seconds:.2f}s, "
f"center={args.center_seconds:.2f}s, seconds={args.seconds:.1f}s",
flush=True,
)
if args.pattern != "sine" and args.seconds < full_sweep_seconds:
if (args.pattern != "sine" or args.rotation_pattern != "none") and args.seconds < full_sweep_seconds:
print(
"提示：当前 seconds 不足以跑完一轮完整方向序列；"
f"建议至少 {full_sweep_seconds:.1f}s。",
@ -241,8 +332,10 @@ def main() -> None:
hand_end_t = hand_start_t + one_hand_seconds
if t < hand_start_t:
label, pos = "WAIT / center", BASE_POS.copy()
rot_label, quat = "ROT wait / center", [0.0, 0.0, 0.0, 1.0]
elif t >= hand_end_t:
label, pos = "DONE / center", BASE_POS.copy()
rot_label, quat = "ROT done / center", [0.0, 0.0, 0.0, 1.0]
else:
pattern_t = t - hand_start_t
label, pos = _pattern_position(
@ -254,6 +347,14 @@ def main() -> None:
args.initial_center_seconds,
index * math.pi,
)
rot_label, quat = _rotation_quaternion(
args.rotation_pattern,
pattern_t,
rotation_amplitude_rad,
args.hold_seconds,
args.center_seconds,
args.initial_center_seconds,
)
else:
label, pos = _pattern_position(
args.pattern,
@ -264,15 +365,30 @@ def main() -> None:
args.initial_center_seconds,
index * math.pi,
)
rot_label, quat = _rotation_quaternion(
args.rotation_pattern,
pattern_t,
rotation_amplitude_rad,
args.hold_seconds,
args.center_seconds,
args.initial_center_seconds,
)
if label != last_labels[hand]:
print(f"[{t:5.2f}s] {hand:<5} {label:<22} pos={pos}", flush=True)
last_labels[hand] = label
combined_label = label if args.rotation_pattern == "none" else f"{label} | {rot_label}"
if combined_label != last_labels[hand]:
quat_text = [round(value, 4) for value in quat]
print(
f"[{t:5.2f}s] {hand:<5} {combined_label:<46} "
f"pos={pos} quat={quat_text}",
flush=True,
)
last_labels[hand] = combined_label
packet["t"] = t
packet["grip"] = True
packet["trigger"] = min(max(args.trigger, 0.0), 1.0)
packet["pos"] = pos
packet["quat"] = quat
sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))
time.sleep(dt)
@ -280,6 +396,7 @@ def main() -> None:
packet["grip"] = False
packet["trigger"] = 0.0
packet["pos"] = BASE_POS.copy()
packet["quat"] = [0.0, 0.0, 0.0, 1.0]
sock.sendto(json.dumps(packet).encode("utf-8"), (args.host, args.port))