前期工作参考这位博主
后续代码是ai写的。此处不做讲解。
完成的工作是：使用机械臂推动正方体到指定地点。方式是始终垂直于接触面（不过路径规划并非按照论文中的形式）
对话过程参考gemini pro和gpt免费版
gpt版本是在gemini版本上修改而来。
视频见bilibili
gemini
python
import mujoco.viewer
import ikpy.chain
import transforms3d as tf
import numpy as np
from collections import deque
import matplotlib.pyplot as plt
import time
step_trigger = False
is_paused = True
def key_callback(keycode):
global step_trigger, is_paused
if keycode == 32:
is_paused = not is_paused
print("运行中..." if not is_paused else "已暂停")
elif keycode in (ord('N'), ord('n')):
step_trigger = True
def viewer_init(viewer):
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE
viewer.cam.lookat[:] = [-0.1, 0.55, 0.1]
viewer.cam.distance = 1.3
viewer.cam.azimuth = 140
viewer.cam.elevation = -25
# ==========================================
# 1. 轨迹与位姿可视化器
# ==========================================
class BoxTrackerPlotter:
def __init__(self, axis_length=0.06, update_interval=35):
plt.ion()
self.fig = plt.figure(figsize=(6, 5))
self.ax = self.fig.add_subplot(111, projection='3d')
self.axis_length = axis_length
self.update_interval = update_interval
self.frame_count = 0
self.initial_pos = None
self.initial_R = None
self.trajectory_pts = []
def _draw_frame(self, pos, R, alpha=1.0):
colors = ['r', 'g', 'b']
for i in range(3):
axis_vec = R[:, i] * self.axis_length
self.ax.quiver(pos[0], pos[1], pos[2],
axis_vec[0], axis_vec[1], axis_vec[2],
color=colors[i], alpha=alpha,
arrow_length_ratio=0.2, linewidth=1.5)
def update(self, current_pos, current_R, target_pos=None):
if self.initial_pos is None:
self.initial_pos = current_pos.copy()
self.initial_R = current_R.copy()
self.trajectory_pts.append(current_pos.copy())
self.frame_count += 1
if self.frame_count % self.update_interval != 0:
return
self.ax.clear()
self._draw_frame(self.initial_pos, self.initial_R, alpha=0.3)
self.ax.scatter(*self.initial_pos, color='k', s=20, label='Start')
if target_pos is not None:
self.ax.scatter(*target_pos, color='g', s=45, marker='*', label='Goal')
pts = np.array(self.trajectory_pts)
if len(pts) > 1:
self.ax.plot(pts[:, 0], pts[:, 1], pts[:, 2], 'm-', linewidth=2.0, label='Actual Traj')
self._draw_frame(current_pos, current_R, alpha=1.0)
self.ax.scatter(*current_pos, color='red', s=25, label='Current')
mid = current_pos
self.ax.set_xlim([mid[0] - 0.2, mid[0] + 0.2])
self.ax.set_ylim([mid[1] - 0.2, mid[1] + 0.2])
self.ax.set_zlim([0.0, 0.25])
self.ax.set_xlabel('X')
self.ax.set_ylabel('Y')
self.ax.set_zlabel('Z')
self.ax.set_title('Real-time Closed-Loop Pushing')
self.ax.legend(loc='upper right', prop={'size': 8})
plt.draw()
plt.pause(0.001)
self.frame_count = 0
class ForceSensor:
def __init__(self, model, data, window_size=6):
self.model = model
self.data = data
self.window_size = window_size
self.force_history = deque(maxlen=window_size)
def get_raw_force(self):
return self.data.sensordata[:3].copy() * -1
def filter(self):
raw = self.get_raw_force()
self.force_history.append(raw)
return np.mean(self.force_history, axis=0)
# ==========================================
# 2. 几何与推点实时解算
# ==========================================
BOX_HALF_SIZE = 0.10 # 方块半边长 0.1m
PROBE_RADIUS = 0.04 # 探针球体半径 0.04m
def get_face_geometry(box_pos, box_R, local_normal):
"""计算指定平面的世界法线、中心推点及推进方向"""
n_world = box_R @ local_normal
n_world[2] = 0.0
norm_xy = np.linalg.norm(n_world[:2])
n_world = n_world / norm_xy if norm_xy > 1e-4 else np.array([0.0, 1.0, 0.0])
contact_pt = box_pos.copy()
contact_pt[:2] += n_world[:2] * (BOX_HALF_SIZE + PROBE_RADIUS)
contact_pt[2] = 0.10 # 绝对锁定质心平面
push_dir = -n_world # 垂直推压方向
return n_world, contact_pt, push_dir
def main():
global step_trigger, is_paused
model = mujoco.MjModel.from_xml_path('model/universal_robots_ur5e/scene.xml')
data = mujoco.MjData(model)
# 1. 严格使用标准自然高肘预备姿态
start_joints = np.array([-1.57, -1.34, 2.65, -1.3, 1.55, 0.0])
data.qpos[:6] = start_joints
data.ctrl[:6] = start_joints
mujoco.mj_forward(model, data)
site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "force_sensor_site")
box_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "red_box")
force_sensor = ForceSensor(model, data, window_size=6)
box_plotter = BoxTrackerPlotter(axis_length=0.06, update_interval=35)
# 目标参数
box_target_pos = np.array([-0.2, 0.8, 0.10])
target_pos_tolerance = 0.015
# 控制参数
dt = model.opt.timestep
fsm_state = 'INIT_DECIDE'
state_timer = 0.0
loss_contact_duration = 0.0 # 失触计时器
current_active_normal = None
next_active_normal = None
f_contact_trigger = 0.45 # 接触力门限
push_speed = 0.008 # 标称推速 8 mm/s
relocate_speed = 0.035 # 空载转场速度 3.5 cm/s
target_qpos = data.qpos[:6].copy()
jacp = np.zeros((3, model.nv))
jacr = np.zeros((3, model.nv))
print("\n[控制说明]：点击视窗后，按 [空格] 切换运行/暂停，按 [N] 单步运行。\n")
with mujoco.viewer.launch_passive(model, data, key_callback=key_callback) as viewer:
viewer_init(viewer)
while viewer.is_running():
if not is_paused or step_trigger:
step_trigger = False
# 1. 实时读取物理状态
raw_force = force_sensor.get_raw_force()
raw_f_norm = np.linalg.norm(raw_force)
R_curr = data.site_xmat[site_id].reshape(3, 3)
p_curr = data.site_xpos[site_id].copy()
box_pos = data.xpos[box_body_id].copy()
box_R = data.xmat[box_body_id].reshape(3, 3)
box_plotter.update(box_pos, box_R, box_target_pos)
# 实时计算目标相对正方体质心的剩余位移
delta_world = box_target_pos[:2] - box_pos[:2]
dist_remain = np.linalg.norm(delta_world)
delta_local = box_R.T @ np.array([delta_world[0], delta_world[1], 0.0])
# ==========================================
# 全闭环自适应状态机（防滑脱 & 防失触）
# ==========================================
if fsm_state == 'INIT_DECIDE':
norm_x = np.array([-1.0, 0.0, 0.0]) if delta_local[0] > 0 else np.array([1.0, 0.0, 0.0])
norm_y = np.array([0.0, -1.0, 0.0]) if delta_local[1] > 0 else np.array([0.0, 1.0, 0.0])
_, pt_x, _ = get_face_geometry(box_pos, box_R, norm_x)
_, pt_y, _ = get_face_geometry(box_pos, box_R, norm_y)
if np.linalg.norm(pt_x[:2] - p_curr[:2]) <= np.linalg.norm(pt_y[:2] - p_curr[:2]):
current_active_normal = norm_x.copy()
next_active_normal = norm_y.copy()
first_face_name = "Face X"
else:
current_active_normal = norm_y.copy()
next_active_normal = norm_x.copy()
first_face_name = "Face Y"
fsm_state = 'APPROACH'
state_timer = data.time
print(f"[{data.time:.3f}s] >>> 初始规划：就近选定 {first_face_name}，直接平缓探触 <<<")
# 获取当前受推面的几何
n_curr, contact_pt, d_curr = get_face_geometry(box_pos, box_R, current_active_normal)
# ---------------- 换面步骤 1: 后退卸力 ----------------
if fsm_state == 'RELOCATE_RETRACT':
retract_pt = contact_pt + n_curr * 0.06
pos_err = retract_pt - p_curr
dist_err = np.linalg.norm(pos_err[:2])
v_cmd = relocate_speed * (pos_err / max(dist_err, 1e-4))
desired_axis = d_curr.copy()
if dist_err < 0.015 and raw_f_norm < 0.2:
fsm_state = 'RELOCATE_CORNER'
state_timer = data.time
print(f"[{data.time:.3f}s] >>> 卸力完成，执行几何避障绕过外侧棱角 <<<")
# ---------------- 换面步骤 2: 绕过外角安全中继点 ----------------
elif fsm_state == 'RELOCATE_CORNER':
c_local = BOX_HALF_SIZE * (current_active_normal + next_active_normal)
n_corner_local = (current_active_normal + next_active_normal) / np.sqrt(2.0)
corner_world = box_pos + box_R @ c_local
n_corner_world = box_R @ n_corner_local
corner_safe_pt = corner_world + n_corner_world * (PROBE_RADIUS + 0.06)
corner_safe_pt[2] = 0.10
pos_err = corner_safe_pt - p_curr
dist_err = np.linalg.norm(pos_err[:2])
v_cmd = relocate_speed * (pos_err / max(dist_err, 1e-4))
_, _, d_next_temp = get_face_geometry(box_pos, box_R, next_active_normal)
desired_axis = d_next_temp.copy()
if dist_err < 0.020:
fsm_state = 'RELOCATE_STANDOFF'
current_active_normal = next_active_normal.copy()
state_timer = data.time
print(f"[{data.time:.3f}s] >>> 绕角成功，飞抵第二阶段受推面外侧待命点 <<<")
# ---------------- 换面步骤 3: 抵达成型待命点 ----------------
elif fsm_state == 'RELOCATE_STANDOFF':
standoff_pt = contact_pt + n_curr * 0.035
pos_err = standoff_pt - p_curr
dist_err = np.linalg.norm(pos_err[:2])
v_cmd = relocate_speed * (pos_err / max(dist_err, 1e-4))
desired_axis = d_curr.copy()
if dist_err < 0.015:
fsm_state = 'APPROACH'
state_timer = data.time
print(f"[{data.time:.3f}s] >>> 抵达待命点，开始直线探触目标面 <<<")
# ---------------- 探寻并贴紧表面 ----------------
elif fsm_state == 'APPROACH':
pos_err = contact_pt - p_curr
if raw_f_norm > f_contact_trigger:
fsm_state = 'PUSH'
state_timer = data.time
loss_contact_duration = 0.0
target_qpos = data.qpos[:6].copy()
print(f"[{data.time:.3f}s] >>> 传感器实测接触面 (F={raw_f_norm:.2f}N)，开始垂直推行 <<<")
v_cmd = np.zeros(3)
else:
v_cmd = 0.012 * d_curr # 沿法线正前方匀速探入
desired_axis = d_curr.copy()
# ---------------- 垂直推进执行 (防滑脱 & 防失触核心) ----------------
elif fsm_state == 'PUSH':
# 计算当前推头相对受推面中心的误差向量
pos_err_center = contact_pt - p_curr
err_normal = np.dot(pos_err_center, d_curr)
err_tangent = pos_err_center - err_normal * d_curr
dist_tangent = np.linalg.norm(err_tangent[:2])
# 1. 动态法向给进：失触加速贴紧，正常匀速推
if raw_f_norm < 0.15:
v_advance = 0.014 * d_curr # 轻微脱开时主动加速追赶
loss_contact_duration += dt
else:
loss_contact_duration = 0.0
# 边缘防护：若偏离面中心超过 4.5cm，减缓前进，优先向中心拉回
if dist_tangent > 0.045:
v_advance = 0.002 * d_curr
else:
v_advance = push_speed * d_curr
# 2. 强力切向居中伺服：最大允许 2.5 cm/s 侧向拉回速度，绝不滑脱拐角
v_lateral = np.clip(2.5 * err_tangent, -0.025, 0.025)
v_cmd = v_advance + v_lateral
desired_axis = d_curr.copy()
# 3. 连续失触保护重捕机制：连续脱力 > 0.8s 自动切回探触状态重新对准
if loss_contact_duration > 0.8:
fsm_state = 'APPROACH'
loss_contact_duration = 0.0
print(f"[{data.time:.3f}s] >>> 提示：检测到推力脱离，自动切入闭环重寻对正 <<<")
# 4. 当前面推行完成判定
d_local = -current_active_normal[:2]
remain_on_face = np.dot(delta_local[:2], d_local)
if remain_on_face <= 0.012 or dist_remain < target_pos_tolerance:
if dist_remain < target_pos_tolerance:
fsm_state = 'FINISHED'
print(f"[{data.time:.3f}s] >>> 正方体质心已精准推达最终目标点！<<<")
else:
if abs(current_active_normal[0]) > 0.5:
next_active_normal = np.array([0.0, -1.0, 0.0]) if delta_local[1] > 0 else np.array([0.0, 1.0, 0.0])
else:
next_active_normal = np.array([-1.0, 0.0, 0.0]) if delta_local[0] > 0 else np.array([1.0, 0.0, 0.0])
fsm_state = 'RELOCATE_RETRACT'
state_timer = data.time
print(f"[{data.time:.3f}s] >>> 第一段推进达成！启动规避航线后退换面 <<<")
elif fsm_state == 'FINISHED':
v_cmd = np.zeros(3)
desired_axis = R_curr[:, 1].copy()
data.ctrl[:6] = data.qpos[:6]
# ==========================================
# 通用运动学执行与高度硬锁死
# ==========================================
if fsm_state != 'FINISHED':
# 严格锁死坚直高度在质心水平面 (Z=0.10m，绝不下沉触地)
v_cmd[2] = 2.5 * (0.10 - p_curr[2])
# 姿态闭环：工具推进轴严格对齐目标法向，水平无俯仰
tool_axis = R_curr[:, 1].copy()
tool_axis /= np.linalg.norm(tool_axis)
e_rot = np.cross(tool_axis, desired_axis)
omega_cmd = np.clip(3.5 * e_rot, -0.15, 0.15)
# 空间雅可比阻尼求解
mujoco.mj_jacSite(model, data, jacp, jacr, site_id)
J_pos = jacp[:, :6]
J_rot = jacr[:, :6]
V_task = np.concatenate([v_cmd, omega_cmd])
J_task = np.vstack([J_pos, J_rot])
damp = 0.05
reg = (damp ** 2) * np.eye(6)
J_inv = J_task.T @ np.linalg.inv(J_task @ J_task.T + reg)
qdot_task = J_inv @ V_task
# 零空间高肘构型维持
I_mat = np.eye(6)
N_null = I_mat - J_inv @ J_task
qdot_null = N_null @ (0.6 * (start_joints - data.qpos[:6]))
qdot = qdot_task + qdot_null
# 区分推进与空载转场速度限幅
q_limit = 0.08 if 'RELOCATE' in fsm_state else 0.04
qdot[:3] = np.clip(qdot[:3], -q_limit, q_limit)
qdot[3:] = np.clip(qdot[3:], -0.18, 0.18)
target_qpos += qdot * dt
data.ctrl[:6] = target_qpos
mujoco.mj_step(model, data)
viewer.sync()
time.sleep(0.002)
if __name__ == "__main__":
main()gpt
python
import time
from collections import deque
import mujoco
import mujoco.viewer
import numpy as np
import matplotlib.pyplot as plt
# ============================================================
# 修复版 v6：接触阶段禁用零空间姿态回拉，并诊断末端实际速度
# ============================================================
# 0. 可修改参数
# ============================================================
MODEL_PATH = "model/universal_robots_ur5e/scene.xml"
# 只需要修改这里的目标位置（单位：m）。本控制器只使用目标的 X、Y 坐标。
# 场景中的绿色 target 标记会自动移动到这里，避免 XML 标记与控制目标不一致。
BOX_TARGET_POS = np.array([-0.15, 0.85, 0.10], dtype=float)
# 方块和探针几何参数：必须与 scene.xml 中 red_box_geom、force_sensor_geom 一致。
BOX_HALF_SIZE = 0.10
PROBE_RADIUS = 0.04
PUSH_HEIGHT = 0.10
# 任务判据
TARGET_POS_TOLERANCE = 0.015 # 方块质心 XY 平面目标误差，15 mm
SEGMENT_TOLERANCE = 0.008 # 单段推送在当前推进方向上的剩余距离，8 mm
FACE_AXIS_EPS = 1e-6
# 力反馈参数
FORCE_CONTACT_TRIGGER = 0.45 # 滤波力超过该值，认定接触（N）
FORCE_CONTACT_LOST = 0.15 # 滤波力低于该值，认定暂时失触（N）
FORCE_RELEASE_THRESHOLD = 0.20 # 换面前要求的卸力阈值（N）
FORCE_FILTER_WINDOW = 6
# 速度与位置控制参数
PUSH_SPEED = 0.008 # 正常推送速度，8 mm/s
LOST_CONTACT_SPEED = 0.014 # 失触时法向追赶速度，14 mm/s
RELOCATE_SPEED = 0.035 # 空载转场最大平面速度，35 mm/s
APPROACH_SPEED = 0.012 # 最初探触的最大平面速度，12 mm/s
APPROACH_KP = 2.0
RELOCATE_KP = 2.0
# 允许在接触面中部区域建立法向预压，不要求末端切向误差必须小于 1.5 mm。
# 当前方块半边长 100 mm、探针半径 40 mm，50 mm 的中心区域余量可避免靠近棱边时预压。
APPROACH_PRELOAD_TANGENT_TOLERANCE = BOX_HALF_SIZE - PROBE_RADIUS - 0.01
APPROACH_PRELOAD_SPEED = 0.005 # 法向轻微预压速度 5 mm/s
STANDOFF_DISTANCE = 0.035 # 接触点外侧待命距离，35 mm
RETRACT_DISTANCE = 0.060 # 换面后退距离，60 mm
CORNER_CLEARANCE = 0.060 # 探针球心绕角的额外净空，60 mm
HEIGHT_KP = 2.5
ORIENTATION_KP = 3.5
MAX_ANGULAR_SPEED = 0.15
NULLSPACE_KP = 0.6
# 推送时连续失触超过该时长，转入 RECOVER_CONTACT 追踪当前接触点。
LOSS_CONTACT_TIMEOUT = 0.8
# 普通 APPROACH 超时处理；推送失触恢复使用单独的 RECOVER_CONTACT 状态。
APPROACH_TIMEOUT = 5.0
MAX_APPROACH_RETRIES = 3 # 普通初次/换面探触连续失败 3 次后进入 FAULT
RECOVERY_STANDOFF_TOLERANCE = 0.008
# 失触恢复：追踪随方块运动的面中心，而不是先远离接触面。
# v6 在 APPROACH/PUSH/RECOVER_CONTACT 阶段禁用零空间姿态回拉，优先保证末端任务跟踪。
RECOVERY_TRACK_KP = 3.0
RECOVERY_TRACK_SPEED = 0.035 # 失触恢复时最大 XY 速度，35 mm/s
RECOVERY_PRELOAD_SPEED = 0.004 # 接近相切位置时的轻微法向预压，4 mm/s
RECOVERY_PRELOAD_MAX_GAP = 0.012 # 法向间隙小于 12 mm 时允许预压
RECOVERY_CONTACT_TIMEOUT = 10.0 # 恢复追踪最长 10 s，失败则进入 FAULT
RECOVERY_DIAGNOSTIC_INTERVAL = 1.0 # 每隔 1 s 输出恢复阶段的命令速度/实际速度
CONTACT_VELOCITY_FILTER_ALPHA = 0.20
MAX_CONTACT_FEEDFORWARD_SPEED = 0.025
# 任务完成后的安全收尾：先从方块旁边撤离，再抬升，最后平滑返回起始关节姿态。
FINAL_RETRACT_DISTANCE = 0.08 # 完成推送后沿面外法线撤离 8 cm
FINAL_RETRACT_KP = 2.0
FINAL_RETRACT_SPEED = 0.025 # 撤离时末端最大速度 25 mm/s
FINAL_LIFT_HEIGHT = 0.28 # 工具球心抬升到 28 cm，高于方块/桌面
FINAL_LIFT_KP = 2.0
FINAL_LIFT_SPEED = 0.035 # 抬升阶段 Z 方向最大速度 35 mm/s
FINAL_POSE_TOLERANCE = 0.008 # 最终笛卡尔目标容差 8 mm
HOME_RETURN_MAX_JOINT_SPEED = 0.30 # 回到起始姿态时的关节目标速度上限（rad/s）
HOME_RETURN_MIN_DURATION = 6.0 # 关节空间返回最短时间（s）
# UR5e 六个执行器的关节速度上限（rad/s）；推送与转场使用不同的前三关节上限。
PUSH_JOINT_SPEED_LIMITS = np.array([0.04, 0.04, 0.04, 0.18, 0.18, 0.18])
RELOCATE_JOINT_SPEED_LIMITS = np.array([0.08, 0.08, 0.08, 0.18, 0.18, 0.18])
step_trigger = False
is_paused = True
def key_callback(keycode):
"""空格：运行/暂停；N：单步。"""
global step_trigger, is_paused
if keycode == 32:
is_paused = not is_paused
print("运行中..." if not is_paused else "已暂停")
elif keycode in (ord("N"), ord("n")):
step_trigger = True
def viewer_init(viewer):
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE
viewer.cam.lookat[:] = [-0.1, 0.55, 0.1]
viewer.cam.distance = 1.3
viewer.cam.azimuth = 140
viewer.cam.elevation = -25
# ============================================================
# 1. 轨迹与位姿可视化
# ============================================================
class BoxTrackerPlotter:
def __init__(self, axis_length=0.06, update_interval=35):
plt.ion()
self.fig = plt.figure(figsize=(7, 5.5))
self.ax = self.fig.add_subplot(111, projection="3d")
self.axis_length = axis_length
self.update_interval = update_interval
self.frame_count = 0
self.initial_pos = None
self.initial_R = None
self.trajectory_pts = []
def _draw_frame(self, pos, R, alpha=1.0):
for i, color in enumerate(("r", "g", "b")):
axis_vec = R[:, i] * self.axis_length
self.ax.quiver(
pos[0], pos[1], pos[2],
axis_vec[0], axis_vec[1], axis_vec[2],
color=color, alpha=alpha,
arrow_length_ratio=0.2, linewidth=1.5,
)
def update(self, current_pos, current_R, target_pos=None):
if self.initial_pos is None:
self.initial_pos = current_pos.copy()
self.initial_R = current_R.copy()
self.trajectory_pts.append(current_pos.copy())
self.frame_count += 1
if self.frame_count % self.update_interval != 0:
return
self.ax.clear()
self._draw_frame(self.initial_pos, self.initial_R, alpha=0.3)
self.ax.scatter(*self.initial_pos, color="k", s=20, label="Start")
if target_pos is not None:
self.ax.scatter(*target_pos, color="g", s=50, marker="*", label="Goal")
pts = np.asarray(self.trajectory_pts)
if len(pts) > 1:
self.ax.plot(pts[:, 0], pts[:, 1], pts[:, 2], "m-", linewidth=1.8, label="Box trajectory")
self._draw_frame(current_pos, current_R, alpha=1.0)
self.ax.scatter(*current_pos, color="red", s=25, label="Current")
# 视野同时覆盖方块轨迹、当前位置和目标，避免目标因超出固定窗口而看不见。
bound_points = [pts[:, :2], current_pos[:2].reshape(1, 2)]
if target_pos is not None:
bound_points.append(target_pos[:2].reshape(1, 2))
xy = np.vstack(bound_points)
lo = np.min(xy, axis=0) - 0.15
hi = np.max(xy, axis=0) + 0.15
self.ax.set_xlim(lo[0], hi[0])
self.ax.set_ylim(lo[1], hi[1])
self.ax.set_zlim(0.0, 0.25)
self.ax.set_xlabel("X (m)")
self.ax.set_ylabel("Y (m)")
self.ax.set_zlabel("Z (m)")
self.ax.set_title("Planar box pushing")
self.ax.legend(loc="upper right", prop={"size": 8})
plt.draw()
plt.pause(0.001)
self.frame_count = 0
# ============================================================
# 2. 力传感器：同时保留原始力与滑动平均力
# ============================================================
class ForceSensor:
def __init__(self, model, data, window_size=6):
self.model = model
self.data = data
self.force_magnitude_history = deque(maxlen=window_size)
if model.nsensordata < 3:
raise RuntimeError(
"传感器数据维数小于 3。请检查 scene.xml 中的 <sensor><force site=.../> 定义。"
)
def get_raw_force(self):
# 当前 scene.xml 只定义了一个三轴 force sensor，因此其数据位于 sensordata[0:3]。
# 取反仅沿用原程序的传感器符号约定；力的模长不受该符号影响。
return self.data.sensordata[:3].copy() * -1.0
def get_filtered_force_norm(self):
# 本状态机只使用力的大小，不使用力方向。因此对每一帧的模长做平均，
# 避免末端姿态变化时，不同传感器局部坐标系中的力向量相互抵消。
magnitude = float(np.linalg.norm(self.get_raw_force()))
self.force_magnitude_history.append(magnitude)
return float(np.mean(self.force_magnitude_history))
def reset_filter(self):
"""进入新的接近阶段时清除上一阶段的力历史，避免旧样本影响接触判定。"""
self.force_magnitude_history.clear()
# ============================================================
# 3. 平面几何：方块只在 XY 平移、只绕 Z 轴旋转
# ============================================================
def yaw_rotation(box_R):
"""从方块旋转矩阵提取 yaw，并构造严格的平面旋转矩阵。"""
yaw = np.arctan2(box_R[1, 0], box_R[0, 0])
c, s = np.cos(yaw), np.sin(yaw)
return np.array([
[c, -s, 0.0],
[s, c, 0.0],
[0.0, 0.0, 1.0],
])
def world_delta_to_box_local(box_R, delta_world_xy):
"""把世界坐标系中的平面位移转换到方块局部坐标系。"""
R_yaw = yaw_rotation(box_R)
delta_world = np.array([delta_world_xy[0], delta_world_xy[1], 0.0])
delta_local = R_yaw.T @ delta_world
delta_local[2] = 0.0
return delta_local
def get_face_geometry(box_pos, box_R, local_normal):
"""
local_normal 是方块局部坐标系中某个侧面的外法线，必须是 +/-X 或 +/-Y。
返回世界系面外法线、探针球心的目标接触点，以及向方块内部的推进方向。
本函数明确采用水平面假设，不处理方块俯仰/翻滚。
"""
R_yaw = yaw_rotation(box_R)
n_world = R_yaw @ np.asarray(local_normal, dtype=float)
n_world[2] = 0.0
n_norm = np.linalg.norm(n_world[:2])
if n_norm < 1e-8:
raise ValueError("Invalid planar face normal")
n_world /= n_norm
contact_pt = box_pos.copy()
contact_pt[:2] += n_world[:2] * (BOX_HALF_SIZE + PROBE_RADIUS)
contact_pt[2] = box_pos[2]
push_dir = -n_world
return n_world, contact_pt, push_dir
def face_normal_for_target_axis(delta_local, axis):
"""返回能把方块沿指定局部轴推向目标的受推面外法线。"""
normal = np.zeros(3)
# 目标在局部 +axis 方向，就从 -axis 面推；反之亦然。
normal[axis] = -1.0 if delta_local[axis] > 0.0 else 1.0
return normal
def choose_initial_face(box_pos, box_R, delta_local, tool_pos):
"""只在目标确实有位移需求的轴向中，选取接触点离当前末端最近的受推面。"""
candidate_normals = []
for axis in (0, 1):
if abs(delta_local[axis]) > FACE_AXIS_EPS:
candidate_normals.append(face_normal_for_target_axis(delta_local, axis))
if not candidate_normals:
return None
best_normal = None
best_distance = np.inf
for normal in candidate_normals:
_, contact_pt, _ = get_face_geometry(box_pos, box_R, normal)
distance = np.linalg.norm(contact_pt[:2] - tool_pos[:2])
if distance < best_distance:
best_distance = distance
best_normal = normal.copy()
return best_normal
def choose_next_face(box_pos, box_R, delta_local, current_normal, tool_pos):
"""
选择下一受推面。
正常情况切换到另一个局部轴；若当前轴发生明显越目标过冲且另一轴几乎无位移，
则先选择一个距离末端较近的相邻面作为中继，避免直接绕到对面时 corner 法线退化。
"""
current_axis = int(np.argmax(np.abs(current_normal[:2])))
other_axis = 1 - current_axis
# 优先处理另一轴上仍有实际意义的目标位移。
if abs(delta_local[other_axis]) > SEGMENT_TOLERANCE:
return face_normal_for_target_axis(delta_local, other_axis)
local_push_dir = -current_normal
remaining_in_push_direction = float(np.dot(delta_local[:2], local_push_dir[:2]))
# 若已明显越过目标，下一步需要反向推；用垂直相邻面作为安全换面中继。
if remaining_in_push_direction < -SEGMENT_TOLERANCE:
bridge_normals = []
for sign in (-1.0, 1.0):
n = np.zeros(3)
n[other_axis] = sign
bridge_normals.append(n)
return min(
bridge_normals,
key=lambda n: np.linalg.norm(
get_face_geometry(box_pos, box_R, n)[1][:2] - tool_pos[:2]
),
).copy()
# 此分支通常意味着平面目标误差已经落入完成阈值附近。
if abs(delta_local[other_axis]) > FACE_AXIS_EPS:
return face_normal_for_target_axis(delta_local, other_axis)
return None
def planar_p_velocity(target_pt, current_pt, kp, max_speed):
"""XY 平面位置比例控制，并限制整体平面速度模长。Z 方向由单独的高度闭环处理。"""
velocity = np.zeros(3)
velocity[:2] = kp * (target_pt[:2] - current_pt[:2])
speed = np.linalg.norm(velocity[:2])
if speed > max_speed:
velocity[:2] *= max_speed / speed
return velocity
def tangent_basis(axis):
"""构造两个与 axis 正交的单位基向量，供工具轴方向控制使用。"""
axis = np.asarray(axis, dtype=float)
axis = axis / max(np.linalg.norm(axis), 1e-12)
reference = np.array([0.0, 0.0, 1.0])
if abs(np.dot(axis, reference)) > 0.9:
reference = np.array([1.0, 0.0, 0.0])
b1 = np.cross(axis, reference)
b1 /= max(np.linalg.norm(b1), 1e-12)
b2 = np.cross(axis, b1)
b2 /= max(np.linalg.norm(b2), 1e-12)
return np.column_stack((b1, b2))
def angular_alignment_command(tool_axis, desired_axis):
"""让工具局部 Y 轴对齐 desired_axis，不额外约束绕 desired_axis 的滚转角。"""
tool_axis = tool_axis / max(np.linalg.norm(tool_axis), 1e-12)
desired_axis = desired_axis / max(np.linalg.norm(desired_axis), 1e-12)
cross = np.cross(tool_axis, desired_axis)
# 处理两向量几乎反向的特殊情况：此时叉积接近零，但姿态误差最大。
if np.linalg.norm(cross) < 1e-8 and np.dot(tool_axis, desired_axis) < 0.0:
reference = np.array([0.0, 0.0, 1.0])
if abs(np.dot(tool_axis, reference)) > 0.9:
reference = np.array([1.0, 0.0, 0.0])
cross = np.cross(tool_axis, reference)
cross /= max(np.linalg.norm(cross), 1e-12)
omega = ORIENTATION_KP * cross
omega_norm = np.linalg.norm(omega)
if omega_norm > MAX_ANGULAR_SPEED:
omega *= MAX_ANGULAR_SPEED / omega_norm
return omega
def wrap_angle_error(q_desired, q_current):
"""得到转动关节的最短角度误差，避免直接相减时跨越 2π 边界。"""
diff = q_desired - q_current
return np.arctan2(np.sin(diff), np.cos(diff))
def enforce_planar_box(model, data, box_body_id, box_joint_id):
"""
将 scene.xml 中的 free joint 方块投影回平面约束：
z 固定为 PUSH_HEIGHT，姿态只保留 yaw，且 vz、wx、wy 清零。
这是为了严格实现当前代码的"平面推送"假设。更物理化的做法是将 XML
中 free joint 改成 X/Y 两个 slide joint 与一个 Z 轴 hinge joint 的嵌套结构。
"""
qadr = int(model.jnt_qposadr[box_joint_id])
vadr = int(model.jnt_dofadr[box_joint_id])
# 从当前 qpos 四元数提取 yaw，避免依赖 mj_step 后可能尚未刷新的派生 xmat。
qw, qx, qy, qz = data.qpos[qadr + 3:qadr + 7].copy()
yaw = np.arctan2(
2.0 * (qw * qz + qx * qy),
1.0 - 2.0 * (qy * qy + qz * qz),
)
data.qpos[qadr + 2] = PUSH_HEIGHT
data.qpos[qadr + 3] = np.cos(yaw / 2.0) # free-joint quaternion order: w, x, y, z
data.qpos[qadr + 4] = 0.0
data.qpos[qadr + 5] = 0.0
data.qpos[qadr + 6] = np.sin(yaw / 2.0)
data.qvel[vadr + 2] = 0.0 # z 方向速度
data.qvel[vadr + 3] = 0.0 # 绕 X 轴角速度
data.qvel[vadr + 4] = 0.0 # 绕 Y 轴角速度
# qpos 被投影修改后，刷新 MuJoCo 的派生位姿、site 与传感器量。
mujoco.mj_forward(model, data)
# ============================================================
# 4. 主控制程序
# ============================================================
def main():
global step_trigger, is_paused
model = mujoco.MjModel.from_xml_path(MODEL_PATH)
data = mujoco.MjData(model)
# 此 scene.xml 中 UR5e 的六个关节和六个执行器均排在最前面。
if model.nu < 6 or model.nv < 6 or model.nq < 6:
raise RuntimeError(
f"模型维数不符合 UR5e 六关节控制的预期：nq={model.nq}, nv={model.nv}, nu={model.nu}"
)
site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "force_sensor_site")
box_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "red_box")
box_joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "red_box_joint")
target_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "target")
if site_id < 0:
raise RuntimeError("找不到 site 'force_sensor_site'，请检查 ur5e.xml。")
if box_body_id < 0:
raise RuntimeError("找不到 body 'red_box'，请检查 scene.xml。")
if box_joint_id < 0:
raise RuntimeError("找不到 joint 'red_box_joint'，请检查 scene.xml。")
if model.jnt_type[box_joint_id] != mujoco.mjtJoint.mjJNT_FREE:
raise RuntimeError("red_box_joint 不是 free joint，当前平面约束投影与该模型不匹配。")
if target_body_id < 0:
raise RuntimeError("找不到 body 'target'，请检查 scene.xml。")
# 目标只使用 XY。将视觉标记同步到代码中的目标，避免 scene.xml 中的 marker 位置与目标不同。
box_target_pos = BOX_TARGET_POS.copy()
box_target_pos[2] = PUSH_HEIGHT
model.body_pos[target_body_id] = box_target_pos
start_joints = np.array([-1.57, -1.34, 2.65, -1.3, 1.55, 0.0], dtype=float)
data.qpos[:6] = start_joints
data.ctrl[:6] = start_joints
mujoco.mj_forward(model, data)
enforce_planar_box(model, data, box_body_id, box_joint_id)
force_sensor = ForceSensor(model, data, window_size=FORCE_FILTER_WINDOW)
box_plotter = BoxTrackerPlotter(axis_length=0.06, update_interval=35)
dt = model.opt.timestep
fsm_state = "INIT_DECIDE"
state_timer = data.time
loss_contact_duration = 0.0
approach_retry_count = 0
current_active_normal = None
next_active_normal = None
target_qpos = data.qpos[:6].copy()
# 位置执行器必须保持一个固定的关节目标。不能在每个仿真步都把 ctrl
# 改成当前 qpos，否则目标会跟着机械臂下落，执行器无法形成有效的位置保持力。
hold_qpos = data.qpos[:6].copy()
# 任务完成后的状态目标
final_retract_target = None
final_lift_target = None
home_return_start_q = None
home_return_start_time = None
home_return_duration = HOME_RETURN_MIN_DURATION
# 接触点速度前馈：用相邻仿真步的接触点位置估计方块平移/旋转造成的目标点运动。
# 仅在同一受推面且处于接触跟踪状态时更新，切换面时清零，避免换面产生速度尖峰。
previous_contact_pt = None
previous_tracking_normal = None
filtered_contact_velocity = np.zeros(3, dtype=float)
previous_tool_pos = None
measured_tool_velocity = np.zeros(3, dtype=float)
last_recovery_debug_time = -np.inf
jacp = np.zeros((3, model.nv))
jacr = np.zeros((3, model.nv))
# actuator ctrlrange 用于限制累计积分得到的目标关节角。
ctrl_limited = np.asarray(model.actuator_ctrllimited[:6], dtype=bool)
ctrl_ranges = model.actuator_ctrlrange[:6].copy()
print("\n[控制说明] 点击 MuJoCo 视窗后，按 [空格] 切换运行/暂停，按 [N] 单步运行。")
print(f"[目标] box_target_pos = ({box_target_pos[0]:.3f}, {box_target_pos[1]:.3f}) m")
print("[假设] 方块只在 XY 平面平移，并只绕 Z 轴旋转。\n")
with mujoco.viewer.launch_passive(model, data, key_callback=key_callback) as viewer:
viewer_init(viewer)
while viewer.is_running():
if not is_paused or step_trigger:
step_trigger = False
# 1. 读取仿真状态和传感器数据
filtered_f_norm = force_sensor.get_filtered_force_norm()
R_curr = data.site_xmat[site_id].reshape(3, 3).copy()
p_curr = data.site_xpos[site_id].copy()
# 估计末端实际速度，用于判断机器人是否真正跟随速度指令。
# 这里用仿真相邻步差分；暂停期间不执行本分支，因此不会除以墙钟时间。
if previous_tool_pos is not None:
measured_tool_velocity = (p_curr - previous_tool_pos) / max(dt, 1e-9)
else:
measured_tool_velocity[:] = 0.0
previous_tool_pos = p_curr.copy()
box_pos = data.xpos[box_body_id].copy()
box_R = data.xmat[box_body_id].reshape(3, 3).copy()
box_plotter.update(box_pos, box_R, box_target_pos)
delta_world_xy = box_target_pos[:2] - box_pos[:2]
dist_remain = np.linalg.norm(delta_world_xy)
delta_local = world_delta_to_box_local(box_R, delta_world_xy)
# 2. 初始选面。先过滤掉已经足够接近目标的情况。
if fsm_state == "INIT_DECIDE":
if dist_remain < TARGET_POS_TOLERANCE:
fsm_state = "HOME_HOLD"
hold_qpos = data.qpos[:6].copy()
print(f"[{data.time:.3f}s] >>> 初始位置已在目标容差内，无需推送；保持当前姿态 <<<")
else:
current_active_normal = choose_initial_face(
box_pos, box_R, delta_local, p_curr
)
if current_active_normal is None:
fsm_state = "HOME_HOLD"
hold_qpos = data.qpos[:6].copy()
print(f"[{data.time:.3f}s] >>> 没有可用推送方向，保持当前姿态 <<<")
else:
fsm_state = "INITIAL_STANDOFF"
state_timer = data.time
face_axis = "X" if abs(current_active_normal[0]) > 0.5 else "Y"
print(
f"[{data.time:.3f}s] >>> 初始规划：选定局部 {face_axis} 面，"
"先到外侧待命点，再沿法线探触 <<<"
)
# HOME_HOLD / FAULT 必须使用固定的关节目标保持姿态。
# 绝不能每一帧都将目标重设为实时 qpos，否则机器人会在重力下下垂。
if fsm_state in ("HOME_HOLD", "FAULT"):
data.ctrl[:6] = hold_qpos
else:
n_curr, contact_pt, d_curr = get_face_geometry(
box_pos, box_R, current_active_normal
)
desired_axis = d_curr.copy()
v_cmd = np.zeros(3)
# 当前接触点随方块平移与 yaw 旋转而移动。对 APPROACH / PUSH /
# RECOVER_CONTACT 做低通差分估计，供失触恢复时进行速度前馈。
tracking_states = ("APPROACH", "PUSH", "RECOVER_CONTACT")
tracking_now = fsm_state in tracking_states
same_tracking_face = (
previous_contact_pt is not None
and previous_tracking_normal is not None
and np.allclose(
current_active_normal, previous_tracking_normal,
atol=1e-6, rtol=0.0
)
)
if tracking_now and same_tracking_face:
measured_contact_velocity = (contact_pt - previous_contact_pt) / max(dt, 1e-9)
measured_contact_velocity[2] = 0.0
measured_speed = np.linalg.norm(measured_contact_velocity[:2])
if measured_speed > MAX_CONTACT_FEEDFORWARD_SPEED:
measured_contact_velocity[:2] *= (
MAX_CONTACT_FEEDFORWARD_SPEED / measured_speed
)
alpha = CONTACT_VELOCITY_FILTER_ALPHA
filtered_contact_velocity = (
(1.0 - alpha) * filtered_contact_velocity
+ alpha * measured_contact_velocity
)
else:
filtered_contact_velocity[:] = 0.0
if tracking_now:
previous_contact_pt = contact_pt.copy()
previous_tracking_normal = current_active_normal.copy()
else:
previous_contact_pt = None
previous_tracking_normal = None
filtered_contact_velocity[:] = 0.0
# ----------------------------------------------------
# A. 初次接近：先去受推面外侧的待命点
# ----------------------------------------------------
if fsm_state == "INITIAL_STANDOFF":
standoff_pt = contact_pt + n_curr * STANDOFF_DISTANCE
v_cmd = planar_p_velocity(
standoff_pt, p_curr, RELOCATE_KP, RELOCATE_SPEED
)
if np.linalg.norm(standoff_pt[:2] - p_curr[:2]) < 0.008:
fsm_state = "APPROACH"
state_timer = data.time
target_qpos = data.qpos[:6].copy()
force_sensor.reset_filter()
v_cmd[:] = 0.0
print(f"[{data.time:.3f}s] >>> 到达初始待命点，开始直线探触 <<<")
# ----------------------------------------------------
# B. 换面步骤 1：后退卸力
# ----------------------------------------------------
elif fsm_state == "RELOCATE_RETRACT":
retract_pt = contact_pt + n_curr * RETRACT_DISTANCE
v_cmd = planar_p_velocity(
retract_pt, p_curr, RELOCATE_KP, RELOCATE_SPEED
)
desired_axis = d_curr.copy()
if (
np.linalg.norm(retract_pt[:2] - p_curr[:2]) < 0.008
and filtered_f_norm < FORCE_RELEASE_THRESHOLD
):
fsm_state = "RELOCATE_CORNER"
state_timer = data.time
v_cmd[:] = 0.0
print(f"[{data.time:.3f}s] >>> 后退卸力完成，开始绕过外侧棱角 <<<")
# ----------------------------------------------------
# C. 换面步骤 2：绕外角到安全中继点
# ----------------------------------------------------
elif fsm_state == "RELOCATE_CORNER":
if next_active_normal is None:
# 防御性分支：没有相邻目标面时回到选面逻辑。
fsm_state = "INIT_DECIDE"
v_cmd[:] = 0.0
else:
corner_local = BOX_HALF_SIZE * (
current_active_normal + next_active_normal
)
corner_normal_local = current_active_normal + next_active_normal
corner_normal_norm = np.linalg.norm(corner_normal_local)
if corner_normal_norm < 1e-8:
raise RuntimeError(
"换面法线相互抵消；RELOCATE_CORNER 只能接收相邻面，不能直接接收对面。"
)
corner_normal_local /= corner_normal_norm
R_yaw = yaw_rotation(box_R)
corner_world = box_pos + R_yaw @ corner_local
corner_normal_world = R_yaw @ corner_normal_local
corner_safe_pt = corner_world + corner_normal_world * (
PROBE_RADIUS + CORNER_CLEARANCE
)
corner_safe_pt[2] = box_pos[2]
v_cmd = planar_p_velocity(
corner_safe_pt, p_curr, RELOCATE_KP, RELOCATE_SPEED
)
desired_axis = get_face_geometry(
box_pos, box_R, next_active_normal
)[2]
if np.linalg.norm(corner_safe_pt[:2] - p_curr[:2]) < 0.012:
current_active_normal = next_active_normal.copy()
next_active_normal = None
fsm_state = "RELOCATE_STANDOFF"
state_timer = data.time
v_cmd[:] = 0.0
print(f"[{data.time:.3f}s] >>> 绕角完成，转向下一受推面待命点 <<<")
# ----------------------------------------------------
# D. 换面步骤 3：到达新受推面的待命点
# ----------------------------------------------------
elif fsm_state == "RELOCATE_STANDOFF":
standoff_pt = contact_pt + n_curr * STANDOFF_DISTANCE
v_cmd = planar_p_velocity(
standoff_pt, p_curr, RELOCATE_KP, RELOCATE_SPEED
)
desired_axis = d_curr.copy()
if np.linalg.norm(standoff_pt[:2] - p_curr[:2]) < 0.008:
fsm_state = "APPROACH"
state_timer = data.time
target_qpos = data.qpos[:6].copy()
force_sensor.reset_filter()
v_cmd[:] = 0.0
print(f"[{data.time:.3f}s] >>> 到达新受推面待命点，开始探触 <<<")
# ----------------------------------------------------
# D2. 普通 APPROACH 超时后的恢复：回到当前面的外侧待命点
# ----------------------------------------------------
elif fsm_state == "REACQUIRE_STANDOFF":
recovery_pt = contact_pt + n_curr * STANDOFF_DISTANCE
v_cmd = planar_p_velocity(
recovery_pt, p_curr, RELOCATE_KP, RELOCATE_SPEED
)
desired_axis = d_curr.copy()
recovery_dist = np.linalg.norm(recovery_pt[:2] - p_curr[:2])
if (
recovery_dist < RECOVERY_STANDOFF_TOLERANCE
and filtered_f_norm < FORCE_RELEASE_THRESHOLD
):
fsm_state = "APPROACH"
state_timer = data.time
target_qpos = data.qpos[:6].copy()
loss_contact_duration = 0.0
force_sensor.reset_filter()
v_cmd[:] = 0.0
print(
f"[{data.time:.3f}s] >>> 失触恢复：已退到面外待命点，"
"重新开始探触 <<<"
)
# ----------------------------------------------------
# E. 失触恢复：带接触点速度前馈地追踪当前受推面
# ----------------------------------------------------
elif fsm_state == "RECOVER_CONTACT":
pos_err = contact_pt - p_curr
pos_err[2] = 0.0
normal_error = float(np.dot(pos_err, d_curr))
tangent_error = pos_err - normal_error * d_curr
tangent_error[2] = 0.0
tangent_error_norm = float(np.linalg.norm(tangent_error[:2]))
xy_error = float(np.linalg.norm(pos_err[:2]))
if filtered_f_norm > FORCE_CONTACT_TRIGGER:
fsm_state = "PUSH"
state_timer = data.time
loss_contact_duration = 0.0
approach_retry_count = 0
target_qpos = data.qpos[:6].copy()
filtered_contact_velocity[:] = 0.0
v_cmd[:] = 0.0
print(
f"[{data.time:.3f}s] >>> 失触恢复成功：重新检测到接触 "
f"(filtered |F|={filtered_f_norm:.2f} N)，恢复推送 <<<"
)
else:
# 关键：位置反馈追踪 + 接触点运动前馈。前馈补偿方块在脱离接触后
# 因惯性仍在平移/旋转而导致的接触目标漂移；更高的追赶速度避免
# 仅使用 APPROACH_SPEED 时跟不上目标。
v_cmd = (
RECOVERY_TRACK_KP * pos_err
+ filtered_contact_velocity
)
v_cmd[2] = 0.0
# 如果末端位于受推面外侧且切向投影仍处于安全面内，增加轻微向内预压。
preload_allowed = (
0.0 <= normal_error < RECOVERY_PRELOAD_MAX_GAP
and tangent_error_norm < APPROACH_PRELOAD_TANGENT_TOLERANCE
)
if preload_allowed:
v_cmd += RECOVERY_PRELOAD_SPEED * d_curr
# 对合成后的 XY 速度做整体限幅。
recovery_speed = float(np.linalg.norm(v_cmd[:2]))
if recovery_speed > RECOVERY_TRACK_SPEED:
v_cmd[:2] *= RECOVERY_TRACK_SPEED / recovery_speed
# v6 诊断：把期望末端速度与实际速度并列打印。
# 若命令速度指向接触点、实际速度却长期反向/接近零，问题在底层 IK/关节跟踪；
# 若实际速度跟随命令但误差不收敛，则应继续检查几何目标与接触模型。
if data.time - last_recovery_debug_time >= RECOVERY_DIAGNOSTIC_INTERVAL:
print(
f"[{data.time:.3f}s] [恢复诊断] "
f"误差XY=({pos_err[0]*1000:+.1f},{pos_err[1]*1000:+.1f}) mm，"
f"期望v=({v_cmd[0]*1000:+.1f},{v_cmd[1]*1000:+.1f}) mm/s，"
f"实测v=({measured_tool_velocity[0]*1000:+.1f},"
f"{measured_tool_velocity[1]*1000:+.1f}) mm/s，"
f"|F|={filtered_f_norm:.2f} N"
)
last_recovery_debug_time = data.time
if data.time - state_timer > RECOVERY_CONTACT_TIMEOUT:
print(
f"[{data.time:.3f}s] >>> 失触恢复失败：已连续追踪 "
f"{RECOVERY_CONTACT_TIMEOUT:.1f}s，"
f"XY误差={xy_error*1000:.1f} mm，"
f"法向间隙={normal_error*1000:.1f} mm，"
f"切向误差={tangent_error_norm*1000:.1f} mm，"
f"接触点速度前馈=({filtered_contact_velocity[0]*1000:.1f},"
f"{filtered_contact_velocity[1]*1000:.1f}) mm/s，"
f"末端=({p_curr[0]:.3f},{p_curr[1]:.3f}) m，"
f"接触点=({contact_pt[0]:.3f},{contact_pt[1]:.3f}) m，"
f"滤波力={filtered_f_norm:.2f} N；进入 FAULT 停止运动 <<<"
)
fsm_state = "FAULT"
hold_qpos = data.qpos[:6].copy()
data.ctrl[:6] = hold_qpos
v_cmd[:] = 0.0
desired_axis = d_curr.copy()
# ----------------------------------------------------
# F. 普通接近接触点：根据位置误差调整速度
# ----------------------------------------------------
elif fsm_state == "APPROACH":
# 接触点是探针球心的目标位置。保留完整的 XY 位置误差，
# 不能在误差小于某个阈值时直接把指令替换成纯法向速度。
# 旧写法在误差约 4 mm 时停止纠正切向误差，容易形成极限环：
# 误差稍大时追位置，稍小时只沿法线走，随后又偏回阈值附近。
pos_err = contact_pt - p_curr
pos_err[2] = 0.0
normal_error = float(np.dot(pos_err, d_curr))
tangent_error = pos_err - normal_error * d_curr
tangent_error[2] = 0.0
tangent_error_norm = float(np.linalg.norm(tangent_error[:2]))
if filtered_f_norm > FORCE_CONTACT_TRIGGER:
fsm_state = "PUSH"
state_timer = data.time
loss_contact_duration = 0.0
approach_retry_count = 0
target_qpos = data.qpos[:6].copy()
v_cmd[:] = 0.0
print(
f"[{data.time:.3f}s] >>> 检测到接触 "
f"(filtered |F|={filtered_f_norm:.2f} N)，开始推送 <<<"
)
else:
# 始终先用位置误差纠正法向与切向偏差。
v_cmd = planar_p_velocity(
contact_pt, p_curr, APPROACH_KP, APPROACH_SPEED
)
# 接触点是"球心与方块表面刚好相切"的理论位置。只把球心
# 控制到这个位置，可能因数值接触容差而没有持续接触力。
# 因此，只要切向投影仍位于面内安全区域，且法向误差已很小，
# 就持续给一个小的向内预压速度；不再用过严的 1.5 mm 切向门限。
# 你的日志中切向误差约 2.3 mm，旧门限会使这一分支完全不执行。
preload_allowed = (
tangent_error_norm < APPROACH_PRELOAD_TANGENT_TOLERANCE
and abs(normal_error) < 0.003
)
if preload_allowed:
v_cmd += APPROACH_PRELOAD_SPEED * d_curr
v_norm = float(np.linalg.norm(v_cmd[:2]))
if v_norm > APPROACH_SPEED:
v_cmd[:2] *= APPROACH_SPEED / v_norm
# 超时只触发恢复，不再靠无限重试掩盖原因；日志输出法向、切向误差。
if data.time - state_timer > APPROACH_TIMEOUT:
approach_retry_count += 1
state_timer = data.time
target_qpos = data.qpos[:6].copy()
force_sensor.reset_filter()
v_cmd[:] = 0.0
preload_text = (
"满足"
if (
tangent_error_norm < APPROACH_PRELOAD_TANGENT_TOLERANCE
and abs(normal_error) < 0.003
)
else "未满足"
)
print(
f"[{data.time:.3f}s] >>> 探触超时（第 {approach_retry_count}/"
f"{MAX_APPROACH_RETRIES} 次）："
f"XY误差={np.linalg.norm(pos_err[:2])*1000:.1f} mm，"
f"法向误差={normal_error*1000:.1f} mm，"
f"切向误差={tangent_error_norm*1000:.1f} mm，"
f"预压条件={preload_text}，"
f"末端=({p_curr[0]:.3f},{p_curr[1]:.3f}) m，"
f"接触点=({contact_pt[0]:.3f},{contact_pt[1]:.3f}) m，"
f"滤波力={filtered_f_norm:.2f} N <<<"
)
if approach_retry_count >= MAX_APPROACH_RETRIES:
fsm_state = "FAULT"
hold_qpos = data.qpos[:6].copy()
data.ctrl[:6] = hold_qpos
print(
f"[{data.time:.3f}s] >>> 探触连续失败 {approach_retry_count} 次，"
"进入 FAULT 并停止运动；请根据误差检查几何、运动学与传感器 <<<"
)
else:
fsm_state = "REACQUIRE_STANDOFF"
print(
f"[{data.time:.3f}s] >>> 先退到面外待命点，再进行下一次探触 <<<"
)
desired_axis = d_curr.copy()
# ----------------------------------------------------
# F. 持续推送：法向推进 + 切向居中 + 失触重捕
# ----------------------------------------------------
elif fsm_state == "PUSH":
pos_err_center = contact_pt - p_curr
err_normal = float(np.dot(pos_err_center, d_curr))
err_tangent = pos_err_center - err_normal * d_curr
err_tangent[2] = 0.0
dist_tangent = np.linalg.norm(err_tangent[:2])
if filtered_f_norm < FORCE_CONTACT_LOST:
v_advance = LOST_CONTACT_SPEED * d_curr
loss_contact_duration += dt
else:
loss_contact_duration = 0.0
if dist_tangent > 0.045:
v_advance = 0.002 * d_curr
else:
v_advance = PUSH_SPEED * d_curr
# 切向纠偏采用整体速度限幅，而不是逐坐标分量限幅。
v_lateral = 2.5 * err_tangent
lateral_speed = np.linalg.norm(v_lateral[:2])
max_lateral_speed = 0.025
if lateral_speed > max_lateral_speed:
v_lateral[:2] *= max_lateral_speed / lateral_speed
v_cmd = v_advance + v_lateral
desired_axis = d_curr.copy()
if loss_contact_duration > LOSS_CONTACT_TIMEOUT:
# v5：失触后转入 RECOVER_CONTACT，使用当前接触点的位置反馈与
# 速度前馈追踪方块平移/旋转引起的目标漂移。恢复阶段不主动退到
# 35 mm 外的待命点；如果在限定时间内仍无法接触，则进入 FAULT。
fsm_state = "RECOVER_CONTACT"
state_timer = data.time
loss_contact_duration = 0.0
target_qpos = data.qpos[:6].copy()
force_sensor.reset_filter()
filtered_contact_velocity[:] = 0.0
last_recovery_debug_time = data.time
v_cmd[:] = 0.0
gap = contact_pt - p_curr
gap[2] = 0.0
normal_gap = float(np.dot(gap, d_curr))
tangent_gap = gap - normal_gap * d_curr
tangent_gap[2] = 0.0
print(
f"[{data.time:.3f}s] >>> 连续失触：转入带速度前馈的接触点追踪，"
f"不先向外撤退；法向间隙={normal_gap * 1000:.1f} mm，"
f"切向误差={np.linalg.norm(tangent_gap[:2]) * 1000:.1f} mm <<<"
)
else:
# 阶段完成与任务完成分别判断。
local_push_dir = -current_active_normal
remain_on_face = float(
np.dot(delta_local[:2], local_push_dir[:2])
)
if dist_remain < TARGET_POS_TOLERANCE:
# 不直接结束并把 ctrl 跟随实时 qpos：那会失去固定位置目标，
# 机械臂可能在重力作用下下垂。任务完成后执行安全撤离、抬升、回家。
fsm_state = "FINAL_RETRACT"
final_retract_target = p_curr.copy()
final_retract_target[:2] += n_curr[:2] * FINAL_RETRACT_DISTANCE
final_retract_target[2] = PUSH_HEIGHT
target_qpos = data.qpos[:6].copy()
state_timer = data.time
v_cmd[:] = 0.0
print(
f"[{data.time:.3f}s] >>> 方块进入目标容差 "
f"(XY误差 {dist_remain * 1000:.1f} mm)，"
"停止推送，沿当前面外法线安全撤离 <<<"
)
elif remain_on_face <= SEGMENT_TOLERANCE:
next_active_normal = choose_next_face(
box_pos,
box_R,
delta_local,
current_active_normal,
p_curr,
)
if next_active_normal is None:
# 按当前阈值设置，这通常表示目标已进入整体容差。
# 若数值误差使其未满足，则重新按当前位置进行初始选面。
fsm_state = "INIT_DECIDE"
current_active_normal = None
v_cmd[:] = 0.0
print(
f"[{data.time:.3f}s] >>> 当前段结束，重新评估剩余目标位移 <<<"
)
else:
fsm_state = "RELOCATE_RETRACT"
state_timer = data.time
v_cmd[:] = 0.0
print(
f"[{data.time:.3f}s] >>> 当前推送段完成，启动后退卸力与换面 <<<"
)
# ----------------------------------------------------
# G. 任务完成后的安全收尾：撤离 -> 抬升 -> 返回起始关节姿态
# ----------------------------------------------------
elif fsm_state == "FINAL_RETRACT":
err3 = final_retract_target - p_curr
v_cmd = FINAL_RETRACT_KP * err3
speed3 = float(np.linalg.norm(v_cmd))
if speed3 > FINAL_RETRACT_SPEED:
v_cmd *= FINAL_RETRACT_SPEED / speed3
desired_axis = d_curr.copy()
if np.linalg.norm(err3[:2]) < FINAL_POSE_TOLERANCE:
fsm_state = "FINAL_LIFT"
final_lift_target = final_retract_target.copy()
final_lift_target[2] = FINAL_LIFT_HEIGHT
target_qpos = data.qpos[:6].copy()
state_timer = data.time
v_cmd[:] = 0.0
print(
f"[{data.time:.3f}s] >>> 已离开方块，开始抬升至 "
f"Z={FINAL_LIFT_HEIGHT:.3f} m <<<"
)
elif fsm_state == "FINAL_LIFT":
err_xy = final_lift_target[:2] - p_curr[:2]
v_cmd[:2] = FINAL_LIFT_KP * err_xy
speed_xy = float(np.linalg.norm(v_cmd[:2]))
if speed_xy > FINAL_RETRACT_SPEED:
v_cmd[:2] *= FINAL_RETRACT_SPEED / speed_xy
desired_axis = d_curr.copy()
if np.linalg.norm(final_lift_target - p_curr) < FINAL_POSE_TOLERANCE:
fsm_state = "RETURN_HOME"
home_return_start_q = data.qpos[:6].copy()
home_return_start_time = data.time
max_delta = float(np.max(np.abs(start_joints - home_return_start_q)))
# 五次平滑插值的峰值归一化速度为 1.875；按最大关节目标速度自适应安排时长。
home_return_duration = max(
HOME_RETURN_MIN_DURATION,
1.875 * max_delta / HOME_RETURN_MAX_JOINT_SPEED,
)
target_qpos = home_return_start_q.copy()
v_cmd[:] = 0.0
print(
f"[{data.time:.3f}s] >>> 抬升完成，开始平滑返回起始关节姿态，"
f"预计用时 {home_return_duration:.1f} s <<<"
)
# 普通笛卡尔运动状态走雅可比控制；RETURN_HOME 使用单独的关节空间轨迹。
if fsm_state in ("HOME_HOLD", "FAULT"):
data.ctrl[:6] = hold_qpos
elif fsm_state == "RETURN_HOME":
elapsed = max(0.0, data.time - home_return_start_time)
u = float(np.clip(elapsed / max(home_return_duration, 1e-6), 0.0, 1.0))
# 五次多项式，起止速度和加速度均为零，比直接跳变到 start_joints 平滑。
blend = 10.0 * u**3 - 15.0 * u**4 + 6.0 * u**5
q_command = home_return_start_q + blend * (start_joints - home_return_start_q)
for i in range(6):
if ctrl_limited[i]:
q_command[i] = np.clip(
q_command[i], ctrl_ranges[i, 0], ctrl_ranges[i, 1]
)
data.ctrl[:6] = q_command
if u >= 1.0:
hold_qpos = start_joints.copy()
data.ctrl[:6] = hold_qpos
fsm_state = "HOME_HOLD"
target_qpos = hold_qpos.copy()
print(
f"[{data.time:.3f}s] >>> 已返回起始关节姿态；"
"固定位置目标保持，不再继续运动 <<<"
)
else:
# 普通任务阶段 Z 轴保持在推送高度；最终抬升阶段改用高处目标。
z_target = FINAL_LIFT_HEIGHT if fsm_state == "FINAL_LIFT" else PUSH_HEIGHT
v_cmd[2] = HEIGHT_KP * (z_target - p_curr[2])
if fsm_state == "FINAL_LIFT":
v_cmd[2] = float(np.clip(
FINAL_LIFT_KP * (z_target - p_curr[2]),
-FINAL_LIFT_SPEED, FINAL_LIFT_SPEED,
))
tool_axis = R_curr[:, 1].copy()
tool_axis /= max(np.linalg.norm(tool_axis), 1e-12)
omega_cmd = angular_alignment_command(tool_axis, desired_axis)
B = tangent_basis(desired_axis)
mujoco.mj_jacSite(model, data, jacp, jacr, site_id)
J_pos = jacp[:, :6]
J_rot = jacr[:, :6]
# 位置 3 维 + 工具轴方向 2 维；不额外约束绕目标轴的滚转，
# 给六自由度 UR5e 留出一个近似零空间自由度用于姿态偏好。
J_task = np.vstack((J_pos, B.T @ J_rot))
V_task = np.concatenate((v_cmd, B.T @ omega_cmd))
damp = 0.05
task_dim = J_task.shape[0] # 5
regularized = J_task @ J_task.T + (damp ** 2) * np.eye(task_dim)
J_pinv = J_task.T @ np.linalg.solve(
regularized, np.eye(task_dim)
)
qdot_task = J_pinv @ V_task
I_mat = np.eye(6)
N_null = I_mat - J_pinv @ J_task
q_err = wrap_angle_error(start_joints, data.qpos[:6])
# v6 关键修正：在接触、探触和失触恢复期间，完全禁用"回到初始关节姿态"的
# 零空间回拉。由于这里使用阻尼伪逆，I - J#J 不是严格的零空间投影，
# qdot_null 可能泄漏到末端任务空间，抵消接触点追踪速度；尤其在第二受推面
# 的可操作性较差时，可能让末端误差越追越大。转场阶段仍保留姿态偏好。
contact_critical_state = fsm_state in (
"APPROACH", "PUSH", "RECOVER_CONTACT",
"FINAL_RETRACT", "FINAL_LIFT",
)
if contact_critical_state:
qdot_null = np.zeros(6, dtype=float)
else:
qdot_null = N_null @ (NULLSPACE_KP * q_err)
qdot = qdot_task + qdot_null
# 统一缩放关节速度，避免逐关节 clip 改变关节速度向量方向。
in_relocation = (
fsm_state.startswith("RELOCATE")
or fsm_state in (
"INITIAL_STANDOFF", "REACQUIRE_STANDOFF", "RECOVER_CONTACT",
"FINAL_RETRACT", "FINAL_LIFT",
)
)
q_limits = (
RELOCATE_JOINT_SPEED_LIMITS
if in_relocation
else PUSH_JOINT_SPEED_LIMITS
)
ratio = np.max(np.abs(qdot) / q_limits)
if ratio > 1.0:
qdot /= ratio
target_qpos += qdot * dt
# 遵守 XML 中执行器定义的控制范围。
for i in range(6):
if ctrl_limited[i]:
target_qpos[i] = np.clip(
target_qpos[i], ctrl_ranges[i, 0], ctrl_ranges[i, 1]
)
data.ctrl[:6] = target_qpos
mujoco.mj_step(model, data)
enforce_planar_box(model, data, box_body_id, box_joint_id)
viewer.sync()
time.sleep(0.002)
plt.ioff()
plt.show()
if __name__ == "__main__":
main()