软件接口
get_a1z_robot()
工厂函数，创建配置好的 ArmRobot 实例：
get_a1z_robot(
can_channel="can0", # CAN 通道名
gravity_comp_factor=1.0, # 重力补偿比例 (0=关闭, 1=全补偿)
zero_gravity_mode=True, # True=零力漂浮, False=位置保持
control_freq_hz=250, # 控制回路频率 (Hz)
urdf_path=None, # 覆盖 URDF 路径
default_kp=None, # 覆盖默认位置增益
default_kd=None, # 覆盖默认速度增益
) -> ArmRobot
ArmRobot 主要方法
| 方法 | 说明 |
|---|---|
| start(initial_kp, initial_kd) | 使能电机，启动控制回路 |
| stop() | 平滑停机（0.3s 衰减），失能电机 |
| get_joint_pos() -> np.ndarray | 获取当前关节角 (rad) |
| get_joint_state() -> dict | 获取 {pos, vel, eff} |
| command_joint_pos(pos) | 设置目标关节角（使用默认 PD 增益） |
| command_joint_state(joint_state) | 设置目标关节角 + 自定义增益 |
| move_joints(target, speed, kp, kd) | 线性插值移动到目标位置（阻塞） |
| is_running | 控制回路是否在运行 |
Kinematics 运动学
from a1z.robots.kinematics import Kinematics
kin = Kinematics("/path/to/urdf")
# 正运动学 → 4x4 齐次变换矩阵
T = kin.fk(q)
# 逆运动学 (阻尼最小二乘)
converged, q_sol = kin.ik(target_pose, init_q=q0)
关节限位
| 关节 | 名称 | 机械限位 (°) | 机械限位 (rad) | 软限位 (°) | 软限位 (rad) |
|---|---|---|---|---|---|
| 0 | arm_joint1 | [-130°, 130°] | [-2.269, 2.269] | [-120°, 120°] | [-2.094, 2.094] |
| 1 | arm_joint2 | [-1.94°, 192.78°] | [-0.034, 3.365] | [0°, 180°] | [0.000, 3.142] |
| 2 | arm_joint3 | [-200.38°, 0°] | [-3.497, 0.000] | [-180°, 0°] | [-3.142, 0] |
| 3 | arm_joint4 | [-91.88°, 110.38°] | [-1.604, 1.926] | [-85°, 85°] | [-1.484, 1.484] |
| 4 | arm_joint5 | [-90°, 90°] | [-1.571, 1.571] | [-90°, 90°] | [-1.484, 1.484] |
| 5 | arm_joint6 | [-120°, 120°] | [-2.094, 2.094] | [-115°, 115°] | [-2.007, 2.007] |
默认控制参数
| 控制项 | 参数值 |
|---|---|
| 默认 KP | [30, 30, 30, 20, 5, 5] |
| 默认 KD | [1, 1, 1, 0.5, 0.5, 0.5] |
| 关节坐标系符号 | [1, 1, -1, 1, 1, 1] (关节3与URDF方向相反) |
| 重力扭矩缩放 | [1, 1, 1, 1, 1, 1] |
| 最大重力扭矩 | [50, 50, 50, 24, 10, 10] Nm |
| 扭矩限幅 | [70, 70, 70, 27, 10, 10] Nm |
| MotorA KT | 2.8 (电流→扭矩转换系数) |
| 控制频率 | 250 Hz |
控制原理
1. MIT 力位混控
电机固件执行：
τ_motor = kp × (pos_target − pos_actual) + kd × (vel_target − vel_actual) + τ_ff
SDK在每个控制周期（默认 250 Hz）执行：
- 从 CAN 总线读取所有电机反馈
- 通过 Pinocchio RNEA 计算当前姿态下的重力补偿扭矩 τ_g(q)
- 安全检查：|τ_g| 超过阈值则紧急停止
- 合成最终扭矩：τ_motor = (user_torque + τ_g × scale × factor) × joint_sign
- 裁剪到安全范围后下发
2. 零力漂浮模式
kp=0, kd=较小值，仅靠重力补偿扭矩抵消重力，机械臂可自由拖拽。
3. 位置保持模式
kp=默认增益, kd=默认增益，PD 控制 + 重力补偿。
注意事项
- 首次使用请将
gravity_comp_factor设为较小值（如 0.3），确认补偿方向正确后再逐步增大 - 重力扭矩超过每关节安全阈值时会自动紧急停止
- 停机时会在 0.3s 内平滑衰减重力补偿并增加阻尼，避免突然失能导致机械臂下落
- 所有目标关节角会被裁剪到 URDF 限位范围内