在复杂的机器人操作中，需要确保机器人在关节、连杆和末端执行器（TCP）三个层面上不进入不安全区域：

1. 关节角度限制：设置每个关节的旋转角度范围，防止机器人因关节角度过大或过小导致机械结构进入危险区域。
2. 连杆位置限制：根据关节角度，通过机器人运动学模型实时计算连杆的位置，确保连杆不会进入不安全区域。
3. 末端执行器（TCP）位置限制：限制末端执行器在指定的工作区域中，确保机器人 TCP 的位置不会超出安全范围。

逐步实现这些限制，通过检查任意一项超出设定范围时立即停止机器人。

设置关节角度限制

关节角度限制可以确保每个关节不会旋转超出设定范围。通过 rtde_receive 接口可以获取当前关节角度，并判断是否超出限制。

• 目的：设置每个关节的旋转角度范围。
• 方法：使用一个列表 joint_limits 定义每个关节的最小和最大角度，并实时监控。

# 定义每个关节的安全角度范围 (单位：弧度)
joint_limits = [(-2.0, 2.0),  # 关节 1 的安全范围(-2.5, 2.5),  # 关节 2 的安全范围(-2.5, 2.5),  # 关节 3 的安全范围(-2.8, 2.8),  # 关节 4 的安全范围(-2.0, 2.0),  # 关节 5 的安全范围(-3.1, 3.1)   # 关节 6 的安全范围
]# 检查当前关节角度是否在安全范围内
def is_joint_within_limits(joint_angles):for i, angle in enumerate(joint_angles):min_angle, max_angle = joint_limits[i]if not (min_angle <= angle <= max_angle):print(f"关节 {i+1} 超出限制范围！当前角度：{angle}，限制：[{min_angle}, {max_angle}]")return Falsereturn True

计算并检查连杆位置

机器人连杆的位置依赖于关节角度，通过机器人运动学模型可以实时计算出每个连杆的末端位置。然后，通过判断连杆位置是否在指定安全区域内，确保机器人结构不接触危险区域。

• 目的：确保机器人连杆不会进入不安全区域。
• 方法：根据机器人的 D-H 参数或其它运动学模型计算连杆位置，然后实时检查是否在安全范围内。

# 定义不安全区域的边界
unsafe_zone = {"x_min": -1.0, "x_max": 1.0,"y_min": -1.0, "y_max": 1.0,"z_min": 0.0, "z_max": 1.5
}# 检查连杆位置是否在安全范围内
def is_link_within_safe_zone(link_positions):for pos in link_positions:x, y, z = posif not (unsafe_zone["x_min"] <= x <= unsafe_zone["x_max"] andunsafe_zone["y_min"] <= y <= unsafe_zone["y_max"] andunsafe_zone["z_min"] <= z <= unsafe_zone["z_max"]):print(f"连杆位置超出限制！当前连杆末端位置：({x}, {y}, {z})")return Falsereturn True

末端执行器（TCP）位置限制

末端执行器（TCP）位置限制用于控制 TCP 的运动范围。使用 rtde_receive 的 getActualTCPPose() 可以获取当前 TCP 位置。

• 目的：确保 TCP 不超出安全的工作区域。
• 方法：定义 TCP 的安全范围，通过实时检查判断 TCP 是否在范围内。

# 定义末端执行器的安全区域 (单位：米)
tcp_safe_zone = {"x_min": 0.2, "x_max": 0.8,"y_min": -0.5, "y_max": 0.5,"z_min": 0.1, "z_max": 0.6
}# 检查末端执行器是否在安全区域内
def is_tcp_within_safe_zone(tcp_position):x, y, z = tcp_position[:3]if not (tcp_safe_zone["x_min"] <= x <= tcp_safe_zone["x_max"] andtcp_safe_zone["y_min"] <= y <= tcp_safe_zone["y_max"] andtcp_safe_zone["z_min"] <= z <= tcp_safe_zone["z_max"]):print(f"TCP 超出限制！当前位置：({x}, {y}, {z})")return Falsereturn True

实现多重保护控制循环

在控制循环中，逐步检查关节角度、连杆位置和 TCP 位置。一旦检测到任何一项超出限制，立即停止机器人。

import rtde_control
import rtde_receive
import time# 创建 RTDE 控制和接收接口实例
rtde_c = rtde_control.RTDEControlInterface("机器人IP地址")
rtde_r = rtde_receive.RTDEReceiveInterface("机器人IP地址")# 设置速度控制参数
velocity = [0.1, 0.0, 0.0, 0.0, 0.0, 0.0]  # TCP 速度
acceleration = 0.5  # 加速度限制
cycle_time = 0.1  # 控制循环周期try:while True:# 获取当前关节角度joint_angles = rtde_r.getActualQ()if not is_joint_within_limits(joint_angles):print("关节角度超出安全范围，停止机器人！")rtde_c.stopL(0.5)break# 计算并检查连杆位置link_positions = calculate_link_positions(joint_angles)if not is_link_within_safe_zone(link_positions):print("连杆位置超出安全范围，停止机器人！")rtde_c.stopL(0.5)break# 获取并检查末端执行器（TCP）位置tcp_position = rtde_r.getActualTCPPose()if not is_tcp_within_safe_zone(tcp_position):print("TCP 超出安全范围，停止机器人！")rtde_c.stopL(0.5)break# 控制机器人速度rtde_c.speedL(velocity, acceleration, time=0)# 保持控制周期的稳定性loop_start = time.time()while (time.time() - loop_start) < cycle_time:passfinally:# 停止并断开连接rtde_c.stopL(0.5)rtde_c.disconnect()rtde_r.disconnect()print("程序结束，已断开连接。")