|
|
@@ -34,12 +34,20 @@ def parse_args() -> argparse.Namespace:
|
|
|
parser.add_argument('--deploy-config', default=str(HOST_ROOT / 'configs' / 'guguji_real_robot_bench.yaml'))
|
|
|
parser.add_argument('--rl-config', default=str(PROJECT_ROOT / 'guguji_rl' / 'configs' / 'balance_ppo.yaml'))
|
|
|
parser.add_argument('--joint', required=True, help='关节名，或 1~8 的序号')
|
|
|
- parser.add_argument('--delta-rad', type=float, default=0.015, help='小步测试角度，建议 0.005~0.02rad')
|
|
|
+ parser.add_argument('--delta-rad', type=float, default=0.03, help='小步测试角度，建议 0.03~0.05rad')
|
|
|
+ parser.add_argument('--max-delta-rad', type=float, default=0.08, help='允许的最大测试角度绝对值，默认 0.08rad')
|
|
|
parser.add_argument('--duration', type=float, default=0.8, help='保持小步目标的时间，单位秒')
|
|
|
parser.add_argument('--ramp-seconds', type=float, default=0.6, help='从当前位置渐变到目标的时间，单位秒')
|
|
|
+ parser.add_argument('--return-ramp-seconds', type=float, default=None, help='从目标位置渐变回起始位置的时间，默认等于 ramp-seconds')
|
|
|
parser.add_argument('--settle-seconds', type=float, default=0.6, help='小步前后保持当前位置的时间，单位秒')
|
|
|
+ parser.add_argument('--ready-timeout', type=float, default=4.0, help='使能前等待 faults=ok 的最长时间，单位秒')
|
|
|
+ parser.add_argument('--joint-kp', type=float, default=None, help='临时覆盖被测关节 Kp，用于方向标定')
|
|
|
+ parser.add_argument('--joint-kd', type=float, default=None, help='临时覆盖被测关节 Kd，用于方向标定')
|
|
|
+ parser.add_argument('--joint-torque-ff-nm', type=float, default=0.0, help='被测关节前馈力矩，方向不确定前建议保持 0')
|
|
|
+ parser.add_argument('--max-joint-velocity-rad-s', type=float, default=None, help='临时覆盖关节速度保护阈值')
|
|
|
parser.add_argument('--arm', action='store_true', help='真正使能并执行 jog；不加时只打印遥测')
|
|
|
parser.add_argument('--confirm-calibrated', action='store_true', help='确认零位已在限位内后允许使能')
|
|
|
+ parser.add_argument('--ignore-attitude-safety', action='store_true', help='方向标定时忽略 roll/pitch 检查，仍检查故障和关节速度')
|
|
|
parser.add_argument('--clear-faults', action='store_true', help='启动时请求 ESP32S3 清除 RS00 故障')
|
|
|
return parser.parse_args()
|
|
|
|
|
|
@@ -62,8 +70,10 @@ def send_command(
|
|
|
kp: np.ndarray,
|
|
|
kd: np.ndarray,
|
|
|
flags: int,
|
|
|
+ torque_ff_nm: np.ndarray | None = None,
|
|
|
) -> int:
|
|
|
zero = np.zeros_like(positions_rad, dtype=np.float32)
|
|
|
+ torque = zero if torque_ff_nm is None else torque_ff_nm.astype(np.float32)
|
|
|
link.send_command(
|
|
|
CommandPacket(
|
|
|
sequence=sequence,
|
|
|
@@ -71,7 +81,7 @@ def send_command(
|
|
|
velocities_rad_s=zero,
|
|
|
kp=kp.astype(np.float32),
|
|
|
kd=kd.astype(np.float32),
|
|
|
- torque_ff_nm=zero,
|
|
|
+ torque_ff_nm=torque,
|
|
|
flags=flags,
|
|
|
)
|
|
|
)
|
|
|
@@ -84,8 +94,12 @@ def send_stop(
|
|
|
sequence: int,
|
|
|
positions_rad: np.ndarray,
|
|
|
repeats: int = 10,
|
|
|
+ estop: bool = True,
|
|
|
) -> int:
|
|
|
zero = np.zeros_like(positions_rad, dtype=np.float32)
|
|
|
+ flags = COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD
|
|
|
+ if estop:
|
|
|
+ flags |= COMMAND_FLAG_ESTOP
|
|
|
for _ in range(repeats):
|
|
|
sequence = send_command(
|
|
|
link,
|
|
|
@@ -93,7 +107,7 @@ def send_stop(
|
|
|
positions_rad=positions_rad,
|
|
|
kp=zero,
|
|
|
kd=zero,
|
|
|
- flags=COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD | COMMAND_FLAG_ESTOP,
|
|
|
+ flags=flags,
|
|
|
)
|
|
|
time.sleep(0.02)
|
|
|
return sequence
|
|
|
@@ -106,12 +120,13 @@ def assert_safe(
|
|
|
max_roll_rad: float,
|
|
|
max_pitch_rad: float,
|
|
|
max_joint_velocity_rad_s: float,
|
|
|
+ ignore_attitude_safety: bool = False,
|
|
|
) -> None:
|
|
|
if telemetry.fault_mask != 0:
|
|
|
raise RuntimeError(f'ESP32S3 fault_mask=0x{telemetry.fault_mask:08X}')
|
|
|
- if abs(telemetry.roll) > max_roll_rad:
|
|
|
+ if not ignore_attitude_safety and abs(telemetry.roll) > max_roll_rad:
|
|
|
raise RuntimeError(f'roll 超限 {telemetry.roll:.3f}rad')
|
|
|
- if abs(telemetry.pitch) > max_pitch_rad:
|
|
|
+ if not ignore_attitude_safety and abs(telemetry.pitch) > max_pitch_rad:
|
|
|
raise RuntimeError(f'pitch 超限 {telemetry.pitch:.3f}rad')
|
|
|
max_velocity_index = int(np.argmax(np.abs(telemetry.joint_velocity_rad_s)))
|
|
|
max_velocity = float(abs(telemetry.joint_velocity_rad_s[max_velocity_index]))
|
|
|
@@ -122,19 +137,95 @@ def assert_safe(
|
|
|
)
|
|
|
|
|
|
|
|
|
-def print_joint_snapshot(telemetry: TelemetryPacket, joint_names: list[str], selected_index: int) -> None:
|
|
|
+def print_joint_snapshot(
|
|
|
+ telemetry: TelemetryPacket,
|
|
|
+ joint_names: list[str],
|
|
|
+ selected_index: int,
|
|
|
+ *,
|
|
|
+ target_rad: float | None = None,
|
|
|
+) -> None:
|
|
|
+ target_text = ''
|
|
|
+ if target_rad is not None:
|
|
|
+ error_rad = float(target_rad) - float(telemetry.joint_position_rad[selected_index])
|
|
|
+ target_text = f' target={target_rad:.4f}rad err={error_rad:.4f}rad'
|
|
|
print(
|
|
|
f'{joint_names[selected_index]}: '
|
|
|
f'pos={telemetry.joint_position_rad[selected_index]:.4f}rad '
|
|
|
f'vel={telemetry.joint_velocity_rad_s[selected_index]:.4f}rad/s '
|
|
|
f'state={telemetry.robot_state_name} fault=0x{telemetry.fault_mask:08X}'
|
|
|
+ f'{target_text}'
|
|
|
)
|
|
|
|
|
|
|
|
|
+def wait_for_ready_telemetry(
|
|
|
+ link: UdpRobotLink,
|
|
|
+ *,
|
|
|
+ sequence: int,
|
|
|
+ hold_positions: np.ndarray,
|
|
|
+ joint_count: int,
|
|
|
+ timeout_s: float,
|
|
|
+ require_faults_ok: bool,
|
|
|
+ clear_faults: bool,
|
|
|
+) -> tuple[int, TelemetryPacket]:
|
|
|
+ zero = np.zeros(joint_count, dtype=np.float32)
|
|
|
+ deadline = time.monotonic() + max(timeout_s, 0.0)
|
|
|
+ last_log_time = 0.0
|
|
|
+ last_telemetry: TelemetryPacket | None = None
|
|
|
+
|
|
|
+ while True:
|
|
|
+ flags = COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD
|
|
|
+ if clear_faults:
|
|
|
+ flags |= COMMAND_FLAG_CLEAR_FAULTS
|
|
|
+ sequence = send_command(
|
|
|
+ link,
|
|
|
+ sequence=sequence,
|
|
|
+ positions_rad=hold_positions,
|
|
|
+ kp=zero,
|
|
|
+ kd=zero,
|
|
|
+ flags=flags,
|
|
|
+ )
|
|
|
+ try:
|
|
|
+ telemetry = link.receive_telemetry()
|
|
|
+ except TimeoutError:
|
|
|
+ if time.monotonic() >= deadline:
|
|
|
+ raise
|
|
|
+ continue
|
|
|
+
|
|
|
+ last_telemetry = telemetry
|
|
|
+ if not require_faults_ok or telemetry.fault_mask == 0:
|
|
|
+ return sequence, telemetry
|
|
|
+
|
|
|
+ now = time.monotonic()
|
|
|
+ if now - last_log_time >= 0.8:
|
|
|
+ print(
|
|
|
+ '等待电机反馈恢复: '
|
|
|
+ f'state={telemetry.robot_state_name} '
|
|
|
+ f'fault=0x{telemetry.fault_mask:08X} '
|
|
|
+ f'cmd_age={telemetry.last_command_age_ms}ms'
|
|
|
+ )
|
|
|
+ last_log_time = now
|
|
|
+ if now >= deadline:
|
|
|
+ return sequence, telemetry
|
|
|
+ time.sleep(0.05)
|
|
|
+
|
|
|
+
|
|
|
def main() -> int:
|
|
|
args = parse_args()
|
|
|
- if abs(args.delta_rad) > 0.03:
|
|
|
- print('安全拦截：--delta-rad 绝对值不能超过 0.03rad。')
|
|
|
+ max_delta_rad = abs(float(args.max_delta_rad))
|
|
|
+ if max_delta_rad <= 0.0:
|
|
|
+ print('安全拦截：--max-delta-rad 必须大于 0。')
|
|
|
+ return 2
|
|
|
+ if abs(args.delta_rad) > max_delta_rad:
|
|
|
+ print(f'安全拦截：--delta-rad 绝对值不能超过 {max_delta_rad:.3f}rad。')
|
|
|
+ return 2
|
|
|
+ if args.joint_kp is not None and not (0.0 <= float(args.joint_kp) <= 50.0):
|
|
|
+ print('安全拦截：--joint-kp 必须在 0~50 之间。')
|
|
|
+ return 2
|
|
|
+ if args.joint_kd is not None and not (0.0 <= float(args.joint_kd) <= 2.0):
|
|
|
+ print('安全拦截：--joint-kd 必须在 0~2 之间。')
|
|
|
+ return 2
|
|
|
+ if abs(float(args.joint_torque_ff_nm)) > 2.0:
|
|
|
+ print('安全拦截：--joint-torque-ff-nm 绝对值不能超过 2Nm。')
|
|
|
return 2
|
|
|
if args.arm and not args.confirm_calibrated:
|
|
|
print('安全拦截：执行 jog 需要同时添加 --arm 和 --confirm-calibrated。')
|
|
|
@@ -149,6 +240,14 @@ def main() -> int:
|
|
|
|
|
|
kp = np.asarray(control_config['kp'], dtype=np.float32)
|
|
|
kd = np.asarray(control_config['kd'], dtype=np.float32)
|
|
|
+ jog_kp = kp.copy()
|
|
|
+ jog_kd = kd.copy()
|
|
|
+ jog_torque = np.zeros(len(mapper.joint_names), dtype=np.float32)
|
|
|
+ if args.joint_kp is not None:
|
|
|
+ jog_kp[selected_index] = float(args.joint_kp)
|
|
|
+ if args.joint_kd is not None:
|
|
|
+ jog_kd[selected_index] = float(args.joint_kd)
|
|
|
+ jog_torque[selected_index] = float(args.joint_torque_ff_nm)
|
|
|
link = UdpRobotLink(
|
|
|
command_host=str(network_config['esp32_host']),
|
|
|
command_port=int(network_config['command_port']),
|
|
|
@@ -161,35 +260,67 @@ def main() -> int:
|
|
|
hold = mapper.nominal_joint_targets.astype(np.float32)
|
|
|
max_roll_rad = float(safety_config['max_roll_rad'])
|
|
|
max_pitch_rad = float(safety_config['max_pitch_rad'])
|
|
|
- max_joint_velocity_rad_s = float(safety_config.get('max_joint_velocity_rad_s', 0.8))
|
|
|
+ max_joint_velocity_rad_s = float(
|
|
|
+ args.max_joint_velocity_rad_s
|
|
|
+ if args.max_joint_velocity_rad_s is not None
|
|
|
+ else safety_config.get('max_joint_velocity_rad_s', 0.8)
|
|
|
+ )
|
|
|
+ armed_started = False
|
|
|
+ request_estop_on_exit = False
|
|
|
|
|
|
try:
|
|
|
- for _ in range(5):
|
|
|
- sequence = send_command(
|
|
|
- link,
|
|
|
- sequence=sequence,
|
|
|
- positions_rad=hold,
|
|
|
- kp=np.zeros_like(kp),
|
|
|
- kd=np.zeros_like(kd),
|
|
|
- flags=COMMAND_FLAG_DISARM | COMMAND_FLAG_HOLD,
|
|
|
- )
|
|
|
- time.sleep(0.05)
|
|
|
-
|
|
|
- telemetry = link.receive_telemetry()
|
|
|
+ sequence, telemetry = wait_for_ready_telemetry(
|
|
|
+ link,
|
|
|
+ sequence=sequence,
|
|
|
+ hold_positions=hold,
|
|
|
+ joint_count=len(mapper.joint_names),
|
|
|
+ timeout_s=float(args.ready_timeout),
|
|
|
+ require_faults_ok=args.arm,
|
|
|
+ clear_faults=args.clear_faults,
|
|
|
+ )
|
|
|
hold = telemetry.joint_position_rad.astype(np.float32).copy()
|
|
|
print('当前反馈:')
|
|
|
print_joint_snapshot(telemetry, mapper.joint_names, selected_index)
|
|
|
+ print(
|
|
|
+ f'被测关节临时控制参数: '
|
|
|
+ f'kp={jog_kp[selected_index]:.3f} '
|
|
|
+ f'kd={jog_kd[selected_index]:.3f} '
|
|
|
+ f'torque_ff={jog_torque[selected_index]:.3f}Nm'
|
|
|
+ )
|
|
|
if not args.arm:
|
|
|
print('未添加 --arm，只读取遥测，不执行 jog。')
|
|
|
return 0
|
|
|
|
|
|
+ try:
|
|
|
+ assert_safe(
|
|
|
+ telemetry,
|
|
|
+ joint_names=mapper.joint_names,
|
|
|
+ max_roll_rad=max_roll_rad,
|
|
|
+ max_pitch_rad=max_pitch_rad,
|
|
|
+ max_joint_velocity_rad_s=max_joint_velocity_rad_s,
|
|
|
+ ignore_attitude_safety=args.ignore_attitude_safety,
|
|
|
+ )
|
|
|
+ except RuntimeError as exc:
|
|
|
+ print(f'禁止使能: {exc}')
|
|
|
+ print('请先恢复到 DISARMED/faults=ok；如果只是姿态角超限，可在方向标定时添加 --ignore-attitude-safety。')
|
|
|
+ return 3
|
|
|
+
|
|
|
flags = COMMAND_FLAG_ARM
|
|
|
if args.clear_faults:
|
|
|
flags |= COMMAND_FLAG_CLEAR_FAULTS
|
|
|
|
|
|
start = time.monotonic()
|
|
|
while time.monotonic() - start < max(args.settle_seconds, 0.0):
|
|
|
- sequence = send_command(link, sequence=sequence, positions_rad=hold, kp=kp, kd=kd, flags=flags)
|
|
|
+ sequence = send_command(
|
|
|
+ link,
|
|
|
+ sequence=sequence,
|
|
|
+ positions_rad=hold,
|
|
|
+ kp=jog_kp,
|
|
|
+ kd=jog_kd,
|
|
|
+ torque_ff_nm=jog_torque,
|
|
|
+ flags=flags,
|
|
|
+ )
|
|
|
+ armed_started = True
|
|
|
telemetry = link.receive_telemetry()
|
|
|
assert_safe(
|
|
|
telemetry,
|
|
|
@@ -197,6 +328,7 @@ def main() -> int:
|
|
|
max_roll_rad=max_roll_rad,
|
|
|
max_pitch_rad=max_pitch_rad,
|
|
|
max_joint_velocity_rad_s=max_joint_velocity_rad_s,
|
|
|
+ ignore_attitude_safety=args.ignore_attitude_safety,
|
|
|
)
|
|
|
|
|
|
jog_target = hold.copy()
|
|
|
@@ -213,7 +345,49 @@ def main() -> int:
|
|
|
progress = min(elapsed / max(args.ramp_seconds, 0.05), 1.0)
|
|
|
target = hold.copy()
|
|
|
target[selected_index] = hold[selected_index] + float(args.delta_rad) * progress
|
|
|
- sequence = send_command(link, sequence=sequence, positions_rad=target, kp=kp, kd=kd, flags=COMMAND_FLAG_ARM)
|
|
|
+ sequence = send_command(
|
|
|
+ link,
|
|
|
+ sequence=sequence,
|
|
|
+ positions_rad=target,
|
|
|
+ kp=jog_kp,
|
|
|
+ kd=jog_kd,
|
|
|
+ torque_ff_nm=jog_torque,
|
|
|
+ flags=COMMAND_FLAG_ARM,
|
|
|
+ )
|
|
|
+ armed_started = True
|
|
|
+ telemetry = link.receive_telemetry()
|
|
|
+ assert_safe(
|
|
|
+ telemetry,
|
|
|
+ joint_names=mapper.joint_names,
|
|
|
+ max_roll_rad=max_roll_rad,
|
|
|
+ max_pitch_rad=max_pitch_rad,
|
|
|
+ max_joint_velocity_rad_s=max_joint_velocity_rad_s,
|
|
|
+ ignore_attitude_safety=args.ignore_attitude_safety,
|
|
|
+ )
|
|
|
+ print_joint_snapshot(
|
|
|
+ telemetry,
|
|
|
+ mapper.joint_names,
|
|
|
+ selected_index,
|
|
|
+ target_rad=float(target[selected_index]),
|
|
|
+ )
|
|
|
+
|
|
|
+ return_start = telemetry.joint_position_rad.astype(np.float32).copy()
|
|
|
+ return_ramp_seconds = args.ramp_seconds if args.return_ramp_seconds is None else float(args.return_ramp_seconds)
|
|
|
+ start = time.monotonic()
|
|
|
+ while time.monotonic() - start < max(return_ramp_seconds, 0.05):
|
|
|
+ elapsed = time.monotonic() - start
|
|
|
+ progress = min(elapsed / max(return_ramp_seconds, 0.05), 1.0)
|
|
|
+ target = return_start + (hold - return_start) * progress
|
|
|
+ sequence = send_command(
|
|
|
+ link,
|
|
|
+ sequence=sequence,
|
|
|
+ positions_rad=target,
|
|
|
+ kp=jog_kp,
|
|
|
+ kd=jog_kd,
|
|
|
+ torque_ff_nm=jog_torque,
|
|
|
+ flags=COMMAND_FLAG_ARM,
|
|
|
+ )
|
|
|
+ armed_started = True
|
|
|
telemetry = link.receive_telemetry()
|
|
|
assert_safe(
|
|
|
telemetry,
|
|
|
@@ -221,12 +395,27 @@ def main() -> int:
|
|
|
max_roll_rad=max_roll_rad,
|
|
|
max_pitch_rad=max_pitch_rad,
|
|
|
max_joint_velocity_rad_s=max_joint_velocity_rad_s,
|
|
|
+ ignore_attitude_safety=args.ignore_attitude_safety,
|
|
|
+ )
|
|
|
+ print_joint_snapshot(
|
|
|
+ telemetry,
|
|
|
+ mapper.joint_names,
|
|
|
+ selected_index,
|
|
|
+ target_rad=float(target[selected_index]),
|
|
|
)
|
|
|
- print_joint_snapshot(telemetry, mapper.joint_names, selected_index)
|
|
|
|
|
|
start = time.monotonic()
|
|
|
while time.monotonic() - start < max(args.settle_seconds, 0.0):
|
|
|
- sequence = send_command(link, sequence=sequence, positions_rad=hold, kp=kp, kd=kd, flags=COMMAND_FLAG_ARM)
|
|
|
+ sequence = send_command(
|
|
|
+ link,
|
|
|
+ sequence=sequence,
|
|
|
+ positions_rad=hold,
|
|
|
+ kp=jog_kp,
|
|
|
+ kd=jog_kd,
|
|
|
+ torque_ff_nm=jog_torque,
|
|
|
+ flags=COMMAND_FLAG_ARM,
|
|
|
+ )
|
|
|
+ armed_started = True
|
|
|
telemetry = link.receive_telemetry()
|
|
|
assert_safe(
|
|
|
telemetry,
|
|
|
@@ -234,14 +423,16 @@ def main() -> int:
|
|
|
max_roll_rad=max_roll_rad,
|
|
|
max_pitch_rad=max_pitch_rad,
|
|
|
max_joint_velocity_rad_s=max_joint_velocity_rad_s,
|
|
|
+ ignore_attitude_safety=args.ignore_attitude_safety,
|
|
|
)
|
|
|
print('jog 完成，已回到起始保持位置。')
|
|
|
except (KeyboardInterrupt, TimeoutError, RuntimeError) as exc:
|
|
|
print(f'测试中止: {exc}')
|
|
|
- sequence = send_stop(link, sequence=sequence, positions_rad=hold)
|
|
|
+ request_estop_on_exit = armed_started
|
|
|
+ sequence = send_stop(link, sequence=sequence, positions_rad=hold, estop=request_estop_on_exit)
|
|
|
return 4
|
|
|
finally:
|
|
|
- send_stop(link, sequence=sequence, positions_rad=hold, repeats=5)
|
|
|
+ send_stop(link, sequence=sequence, positions_rad=hold, repeats=5, estop=request_estop_on_exit)
|
|
|
link.close()
|
|
|
return 0
|
|
|
|
