双臂控制¶

Robot 类¶

Robot 类是控制双臂机器人的核心接口，管理左右两条机械臂，提供统一的控制接口。

功能概览¶

Robot
├── 初始化与连接
│   ├── __init__()          # 创建双臂机器人实例
│   └── close()             # 关闭连接
├── 电机使能控制
│   ├── enable_motor()      # 使能指定臂电机
│   ├── enable_all()        # 使能双臂所有电机
│   ├── disable_motor()     # 禁用指定臂电机
│   └── disable_all()       # 禁用双臂所有电机
├── 运动模式设置
│   ├── set_mode_mit()      # 设置指定臂MIT模式
│   ├── set_mode_csp()      # 设置指定臂CSP模式
│   ├── set_mode_mit_all()  # 双臂批量设置MIT模式
│   └── set_mode_csp_all()  # 双臂批量设置CSP模式
├── MIT模式控制
│   ├── move_joint_mit()    # 指定臂单关节MIT控制
│   └── move_joints_mit()   # 指定臂多关节MIT控制
├── CSP模式控制
│   ├── move_joint_csp()    # 指定臂单关节CSP控制
│   └── move_joints_csp()   # 指定臂多关节CSP控制
├── 状态获取
│   ├── get_telemetry()     # 获取双臂遥测数据
│   └── get_status()        # 获取双臂状态数据
└── 零点设置
    ├── set_zero_point()    # 设置指定臂电机零点
    └── set_all_zero_points() # 设置双臂所有零点

类概述¶

属性	说明
`left_arm`	左臂控制对象 (Arm实例)
`right_arm`	右臂控制对象 (Arm实例)
`arms`	机械臂列表 `[right_arm, left_arm]`

构造函数¶

Robot¶

Robot(right_can_channel='can0', left_can_channel='can1', motor_ids=None,
      auto_enable_can=True, bitrate=1000000, password=None, log=None, **kwargs)

初始化双臂机器人控制器。

参数名	类型	默认值	说明
`right_can_channel`	`str`	`'can0'`	右臂CAN通道名称
`left_can_channel`	`str`	`'can1'`	左臂CAN通道名称
`motor_ids`	`List[int]`	`[1-8]`	要控制的电机ID列表
`auto_enable_can`	`bool`	`True`	是否自动启用CAN接口
`bitrate`	`int`	`1000000`	CAN总线波特率（1Mbps）
`password`	`str`	`None`	sudo密码，用于自动启用CAN接口
`log`	`callable`	`None`	日志函数

from openarmx_arm_driver import Robot

# 基本用法
robot = Robot(right_can_channel='can0', left_can_channel='can1')

# 使用密码自动启用CAN接口
robot = Robot(right_can_channel='can0', left_can_channel='can1', password='your_password')

# 自定义电机ID列表
robot = Robot(motor_ids=[1, 2, 3, 4, 5, 6, 7])

注意："CAN通道配置"

默认配置：右臂使用 can0，左臂使用 can1
右臂电机方向正常，左臂电机方向自动反转
如果提供 password 参数，将自动输入sudo密码启用CAN接口

快速开始¶

from openarmx_arm_driver import Robot

# 创建双臂机器人实例
robot = Robot(right_can_channel='can0', left_can_channel='can1')

# 使能所有电机
robot.enable_all()

# 设置控制模式
robot.set_mode_all('mit')

# 控制双臂运动
robot.move_joints_mit(
    left_positions=[0.1, 0.2, 0.3, 0.4, 0, 0, 0, 0],
    right_positions=[0.1, 0.2, 0.3, 0.4, 0, 0, 0, 0],
    kp=10.0, kd=1.0
)

# 获取状态
status = robot.get_all_status()

# 停止所有电机
robot.disable_all()

# 关闭连接
robot.shutdown()

使用with语句（推荐）：

with Robot('can0', 'can1') as robot:
    robot.enable_all()
    robot.set_mode_all('mit')
    robot.move_all_to_zero(kp=5.0, kd=0.5)
    # 自动调用shutdown()

使能/失能控制¶

enable_all¶

enable_all(motor_ids=None, verbose=False, timeout=1.0, progress_callback=None) -> dict

使能所有机械臂的所有电机。

参数名	类型	默认值	说明
`motor_ids`	`List[int]`	`None`	指定电机ID列表，默认为所有电机
`verbose`	`bool`	`False`	是否打印详细信息
`timeout`	`float`	`1.0`	每个电机的超时时间（秒）
`progress_callback`	`callable`	`None`	进度回调函数

返回值结构：

{
    'summary': {
        'total': 16,        # 总电机数
        'success': 16,      # 成功数
        'failed': 0,        # 失败数
        'total_time': 1.5   # 总耗时（秒）
    },
    'details': {
        'right': {1: {'success': True, 'error': None, 'time': 0.1}, ...},
        'left': {1: {'success': True, 'error': None, 'time': 0.1}, ...}
    }
}

回调函数格式：

def callback(arm_name, motor_id, success, error_msg, exec_time):
    """
    arm_name: 'right' 或 'left'
    motor_id: 电机ID
    success: True/False
    error_msg: 错误信息（成功时为None）
    exec_time: 执行时间（秒）
    """
    print(f"{arm_name} - Motor {motor_id}: {'✓' if success else '✗'} ({exec_time:.3f}s)")

# 基本用法
results = robot.enable_all()
print(f"成功: {results['summary']['success']}/{results['summary']['total']}")

# 使用回调函数实时监控
def callback(arm, mid, success, error, time):
    print(f"{arm} - Motor {mid}: {'✓' if success else '✗'}")

results = robot.enable_all(progress_callback=callback)

disable_all¶

disable_all(motor_ids=None, verbose=False, timeout=1.0, progress_callback=None) -> dict

失能所有机械臂的所有电机。

参数名	类型	默认值	说明
`motor_ids`	`List[int]`	`None`	指定电机ID列表
`verbose`	`bool`	`False`	是否打印详细信息
`timeout`	`float`	`1.0`	每个电机的超时时间
`progress_callback`	`callable`	`None`	进度回调函数

返回值： 同 enable_all()

results = robot.disable_all()
if results['summary']['failed'] == 0:
    print("所有电机已安全停止")

enable_left / enable_right¶

enable_left(motor_ids=None, verbose=False, timeout=1.0, progress_callback=None) -> dict
enable_right(motor_ids=None, verbose=False, timeout=1.0, progress_callback=None) -> dict

使能单侧机械臂的所有电机。

# 只使能左臂
robot.enable_left()

# 只使能右臂
robot.enable_right()

# 使能左臂指定电机
robot.enable_left(motor_ids=[1, 2, 3, 4])

disable_left / disable_right¶

disable_left(motor_ids=None, verbose=False, timeout=1.0, progress_callback=None) -> dict
disable_right(motor_ids=None, verbose=False, timeout=1.0, progress_callback=None) -> dict

失能单侧机械臂的所有电机。

robot.disable_left()
robot.disable_right()

模式设置¶

set_mode_all¶

set_mode_all(mode, motor_ids=None, verbose=False, timeout=1.0, progress_callback=None) -> dict

设置所有机械臂的控制模式。

参数名	类型	默认值	说明
`mode`	`str`	-	控制模式
`motor_ids`	`List[int]`	`None`	指定电机ID列表
`verbose`	`bool`	`False`	是否打印详细信息
`timeout`	`float`	`1.0`	每个电机的超时时间
`progress_callback`	`callable`	`None`	进度回调函数

支持的模式：

模式名	代码	说明
`'mit'`	`0`	MIT运控模式
`'pp'`	`1`	PP位置模式
`'speed'`	`2`	速度模式
`'current'`	`3`	电流模式
`'csp'`	`5`	CSP位置模式

# 设置所有电机为MIT模式
robot.set_mode_all('mit')

# 设置所有电机为CSP模式
robot.set_mode_all('csp')

set_mode_left / set_mode_right¶

set_mode_left(mode, motor_ids=None)
set_mode_right(mode, motor_ids=None)

设置单侧机械臂的控制模式。

robot.set_mode_left('mit')
robot.set_mode_right('csp')

MIT模式控制¶

move_joints_mit¶

move_joints_mit(left_positions=None, right_positions=None, kp=5.0, kd=0.5)

同时控制左右臂关节位置（MIT模式）。

参数名	类型	默认值	说明
`left_positions`	`List[float]`	`None`	左臂各关节位置（rad）
`right_positions`	`List[float]`	`None`	右臂各关节位置（rad）
`kp`	`float` 或 `List[float]`	`5.0`	位置增益
`kd`	`float` 或 `List[float]`	`0.5`	速度增益

# 左右臂对称运动，统一增益
robot.move_joints_mit(
    left_positions=[0.1, 0.2, 0.3, 0.4, 0, 0, 0, 0],
    right_positions=[0.1, 0.2, 0.3, 0.4, 0, 0, 0, 0],
    kp=10.0, kd=1.0
)

# 每个电机使用不同的增益
robot.move_joints_mit(
    left_positions=[0.1, 0.2, 0.3, 0.4, 0, 0, 0, 0],
    right_positions=[0.1, 0.2, 0.3, 0.4, 0, 0, 0, 0],
    kp=[5.0, 6.0, 7.0, 8.0, 5.0, 5.0, 5.0, 5.0],
    kd=[0.5, 0.6, 0.7, 0.8, 0.5, 0.5, 0.5, 0.5]
)

# 只控制右臂
robot.move_joints_mit(right_positions=[0.1, 0.2, 0.3, 0.4, 0, 0, 0, 0], kp=10.0)

注意："增益参数建议"

电机型号	KP范围	KD范围	典型值
RS04（关节1-2）	0-5000	0-100	KP=1000, KD=10
RS03（关节3-4）	0-5000	0-100	KP=800, KD=8
RS00（关节5-8）	0-500	0-5	KP=50, KD=0.5

move_one_joint_mit¶

move_one_joint_mit(arm, motor_id, position=0.0, velocity=0.0, torque=0.0,
                   kp=0.0, kd=0.0, wait_response=False, timeout=1.0, verbose=False) -> int

控制单臂单个电机（MIT模式）。

参数名	类型	默认值	说明
`arm`	`str`	-	机械臂选择：`'left'` 或 `'right'`
`motor_id`	`int`	-	电机ID (1-8)
`position`	`float`	`0.0`	目标位置（rad）
`velocity`	`float`	`0.0`	目标速度（rad/s）
`torque`	`float`	`0.0`	前馈扭矩（Nm）
`kp`	`float`	`0.0`	位置增益
`kd`	`float`	`0.0`	速度增益
`wait_response`	`bool`	`False`	是否等待响应
`timeout`	`float`	`1.0`	超时时间（秒）

返回值： int

0 表示成功
1 表示失败

# 控制右臂的电机5移动到位置1.0
robot.move_one_joint_mit('right', motor_id=5, position=1.0, kp=10.0, kd=1.0)

# 控制左臂的电机3
robot.move_one_joint_mit('left', motor_id=3, position=0.5, kp=8.0, kd=0.8)

test_motor_one_by_one¶

test_motor_one_by_one(left_positions=None, right_positions=None, kp=5.0, kd=0.5)

逐个测试电机运动（MIT模式）- 每个电机移动到指定位置后再回零。

参数名	类型	默认值	说明
`left_positions`	`List[float]`	`None`	左臂各关节测试位置
`right_positions`	`List[float]`	`None`	右臂各关节测试位置
`kp`	`float` 或 `List[float]`	`5.0`	位置增益
`kd`	`float` 或 `List[float]`	`0.5`	速度增益

# 逐个测试右臂电机
robot.test_motor_one_by_one(
    right_positions=[0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3],
    kp=10.0, kd=1.0
)

测试流程：每个电机依次执行：移动到目标位置 → 等待0.5秒 → 回到零位 → 等待0.5秒 → 下一个电机

CSP模式控制¶

move_joints_csp¶

move_joints_csp(left_positions=None, right_positions=None)

同时控制左右臂关节位置（CSP模式）。

参数名	类型	默认值	说明
`left_positions`	`List[float]`	`None`	左臂各关节位置（rad）
`right_positions`	`List[float]`	`None`	右臂各关节位置（rad）

robot.move_joints_csp(
    left_positions=[0.1, 0.2, 0.3, 0.4, 0, 0, 0, 0],
    right_positions=[0.1, 0.2, 0.3, 0.4, 0, 0, 0, 0]
)

move_one_joint_csp¶

move_one_joint_csp(arm, motor_id, position, wait_response=False, timeout=0.2, verbose=False) -> int

控制单臂单个电机（CSP模式）。

参数名	类型	默认值	说明
`arm`	`str`	-	机械臂选择：`'left'` 或 `'right'`
`motor_id`	`int`	-	电机ID (1-8)
`position`	`float`	-	目标位置（rad）
`wait_response`	`bool`	`False`	是否等待位置到达
`timeout`	`float`	`0.2`	超时时间（秒）

返回值： int

0 表示成功
1 表示失败

robot.move_one_joint_csp('right', motor_id=5, position=1.0)
robot.move_one_joint_csp('left', motor_id=3, position=0.5)

set_csp_limits_all¶

set_csp_limits_all(speed_limit=None, current_limit=None, motor_ids=None)

设置所有机械臂的CSP速度/电流限制。

参数名	类型	默认值	说明
`speed_limit`	`float`	`None`	速度限制（rad/s）
`current_limit`	`float`	`None`	电流限制（A）
`motor_ids`	`List[int]`	`None`	指定电机ID列表

限制参数建议：

电机型号	速度限制	电流限制
RS04（关节1-2）	5-15 rad/s	8-15 A
RS03（关节3-4）	10-20 rad/s	4-8 A
RS00（关节5-8）	20-30 rad/s	2-5 A

# 设置所有电机速度限制
robot.set_csp_limits_all(speed_limit=10.0)

# 设置速度和电流限制
robot.set_csp_limits_all(speed_limit=10.0, current_limit=5.0)

set_csp_limits_left / set_csp_limits_right¶

set_csp_limits_left(speed_limit=None, current_limit=None, motor_ids=None)
set_csp_limits_right(speed_limit=None, current_limit=None, motor_ids=None)

设置单侧机械臂的CSP速度/电流限制。

robot.set_csp_limits_left(speed_limit=10.0)
robot.set_csp_limits_right(speed_limit=15.0, current_limit=8.0)

状态查询¶

get_all_status¶

get_all_status() -> Dict[str, Dict]

获取所有机械臂的状态。

返回值结构：

{
    'left': {
        1: {'angle': 0.0, 'velocity': 0.0, 'torque': 0.0, 'temperature': 25.0, ...},
        2: {...},
        ...
    },
    'right': {
        1: {'angle': 0.0, 'velocity': 0.0, 'torque': 0.0, 'temperature': 25.0, ...},
        2: {...},
        ...
    }
}

status = robot.get_all_status()

# 获取右臂电机5的角度
angle = status['right'][5]['angle']
print(f"右臂电机5角度: {angle:.3f} rad")

get_left_status / get_right_status¶

get_left_status(motor_id=None) -> Dict
get_right_status(motor_id=None) -> Dict

获取单侧机械臂状态。

参数名	类型	默认值	说明
`motor_id`	`int`	`None`	电机ID，`None`返回所有电机

# 获取左臂所有电机状态
left_status = robot.get_left_status()

# 获取右臂电机5状态
motor5_status = robot.get_right_status(motor_id=5)

show_all_status¶

show_all_status() -> None

显示双臂机器人的整体状态（统一表格）。

robot.show_all_status()

输出示例：

==================================================================
机器人状态 (双臂)
==================================================================
臂   | ID | 角度(rad) | 速度(rad/s) | 力矩(Nm) |  温度    | 模式              | 状态
------------------------------------------------------------------
右臂 |  1 |     0.000 |       0.000 |    0.000 |  25.0°C | 🟢 Motor模式      | ✓ 正常
右臂 |  2 |     0.123 |       0.010 |    0.100 |  26.5°C | 🟢 Motor模式      | ✓ 正常
...
------------------------------------------------------------------
左臂 |  1 |     0.000 |       0.000 |    0.000 |  25.0°C | 🟢 Motor模式      | ✓ 正常
左臂 |  2 |     0.123 |       0.010 |    0.100 |  26.5°C | 🟢 Motor模式      | ✓ 正常
...
==================================================================

show_left_status / show_right_status¶

show_left_status() -> None
show_right_status() -> None

显示单侧机械臂状态表格。

robot.show_left_status()
robot.show_right_status()

零点设置¶

set_zero_all¶

set_zero_all(motor_ids=None, verbose=False, timeout=1.0, progress_callback=None) -> dict

设置所有机械臂的零点。

参数名	类型	默认值	说明
`motor_ids`	`List[int]`	`None`	指定电机ID列表
`verbose`	`bool`	`False`	是否打印详细信息
`timeout`	`float`	`1.0`	每个电机的超时时间
`progress_callback`	`callable`	`None`	进度回调函数

返回值： 同 enable_all()

results = robot.set_zero_all()
if results['summary']['failed'] == 0:
    print("所有电机零点设置成功")

注意：零点设置会保存在电机控制器中，断电后仍然有效

set_zero_left / set_zero_right¶

set_zero_left(motor_ids=None)
set_zero_right(motor_ids=None)

设置单侧机械臂零点。

robot.set_zero_left()
robot.set_zero_right()

set_zero_range_all¶

set_zero_range_all(zero_sta=1, motor_ids=None)

设置所有机械臂的零点表示范围。

参数名	类型	默认值	说明
`zero_sta`	`int`	`1`	`0`: 0~2π，`1`: -π~π
`motor_ids`	`List[int]`	`None`	指定电机ID列表

# 设置所有电机零点范围为 -π~π
robot.set_zero_range_all(zero_sta=1)

# 设置所有电机零点范围为 0~2π
robot.set_zero_range_all(zero_sta=0)

set_zero_range_left / set_zero_range_right¶

set_zero_range_left(zero_sta=1, motor_ids=None)
set_zero_range_right(zero_sta=1, motor_ids=None)

设置单侧机械臂的零点表示范围。

robot.set_zero_range_left(zero_sta=1)
robot.set_zero_range_right(zero_sta=1)

归零控制¶

move_all_to_zero¶

move_all_to_zero(kp=5.0, kd=0.5, motor_ids=None, verbose=False, progress_callback=None) -> dict

所有机械臂回到零位（MIT模式）。

参数名	类型	默认值	说明
`kp`	`float` 或 `List[float]`	`5.0`	位置增益
`kd`	`float` 或 `List[float]`	`0.5`	速度增益
`motor_ids`	`List[int]`	`None`	指定电机ID列表
`verbose`	`bool`	`False`	是否打印详细信息
`progress_callback`	`callable`	`None`	进度回调函数

返回值： 同 enable_all()

# 所有电机使用相同增益归零
results = robot.move_all_to_zero(kp=5.0, kd=0.5)

# 每个电机使用不同增益归零
results = robot.move_all_to_zero(
    kp=[5.0, 6.0, 7.0, 8.0, 5.0, 5.0, 5.0, 5.0],
    kd=[0.5, 0.6, 0.7, 0.8, 0.5, 0.5, 0.5, 0.5]
)

# 使用回调函数实时监控
def callback(arm, mid, success, error, time):
    print(f"{arm} - Motor {mid}: {'✓' if success else '✗'}")

results = robot.move_all_to_zero(progress_callback=callback)

资源管理¶

shutdown¶

shutdown() -> None

关闭所有CAN总线连接。

robot.shutdown()

注意：必须调用 shutdown() 释放CAN总线资源，否则可能导致资源泄漏。推荐使用 with 语句自动管理。

完整示例¶

基本控制流程¶

from openarmx_arm_driver import Robot

# 创建双臂机器人
robot = Robot(right_can_channel='can0', left_can_channel='can1')

try:
    # 1. 使能所有电机
    results = robot.enable_all()
    print(f"使能结果: {results['summary']['success']}/{results['summary']['total']}")

    # 2. 设置MIT模式
    robot.set_mode_all('mit')

    # 3. 归零
    robot.move_all_to_zero(kp=5.0, kd=0.5)

    # 4. 控制运动
    robot.move_joints_mit(
        left_positions=[0.5, 0.3, 0.2, 0.1, 0, 0, 0, 0],
        right_positions=[0.5, 0.3, 0.2, 0.1, 0, 0, 0, 0],
        kp=10.0, kd=1.0
    )

    # 5. 显示状态
    robot.show_all_status()

    # 6. 停止电机
    robot.disable_all()

finally:
    robot.shutdown()

带进度回调的控制¶

from openarmx_arm_driver import Robot

def progress_callback(arm_name, motor_id, success, error_msg, exec_time):
    status = "✓" if success else "✗"
    print(f"  {arm_name:5s} Motor {motor_id}: {status} ({exec_time:.3f}s)")
    if error_msg:
        print(f"    Error: {error_msg}")

with Robot('can0', 'can1') as robot:
    print("正在使能电机...")
    results = robot.enable_all(progress_callback=progress_callback)

    print(f"\n总结: {results['summary']['success']}/{results['summary']['total']} 成功")
    print(f"总耗时: {results['summary']['total_time']:.2f}s")

CSP模式高精度控制¶

from openarmx_arm_driver import Robot
import time

with Robot('can0', 'can1') as robot:
    robot.enable_all()

    # 设置CSP模式
    robot.set_mode_all('csp')

    # 设置速度限制
    robot.set_csp_limits_all(speed_limit=10.0, current_limit=5.0)

    # 轨迹控制
    for i in range(100):
        pos = 0.5 * (1 - math.cos(i * 0.1))  # 正弦轨迹
        robot.move_joints_csp(
            right_positions=[pos, pos, pos, pos, 0, 0, 0, 0],
            left_positions=[pos, pos, pos, pos, 0, 0, 0, 0]
        )
        time.sleep(0.01)

    robot.disable_all()

方法速查表¶

使能/失能¶

方法	功能	返回类型
`enable_all()`	使能所有电机	`dict`
`disable_all()`	失能所有电机	`dict`
`enable_left()`	使能左臂电机	`dict`
`enable_right()`	使能右臂电机	`dict`
`disable_left()`	失能左臂电机	`dict`
`disable_right()`	失能右臂电机	`dict`

模式设置¶

方法	功能	返回类型
`set_mode_all(mode)`	设置所有电机模式	`dict`
`set_mode_left(mode)`	设置左臂电机模式	`None`
`set_mode_right(mode)`	设置右臂电机模式	`None`

MIT模式控制¶

方法	功能	返回类型
`move_joints_mit()`	批量位置控制	`None`
`move_one_joint_mit()`	单关节控制	`int`
`test_motor_one_by_one()`	逐个测试电机	`None`
`move_all_to_zero()`	所有电机归零	`dict`

CSP模式控制¶

方法	功能	返回类型
`move_joints_csp()`	批量位置控制	`None`
`move_one_joint_csp()`	单关节控制	`int`
`set_csp_limits_all()`	设置所有电机限制	`None`
`set_csp_limits_left()`	设置左臂限制	`None`
`set_csp_limits_right()`	设置右臂限制	`None`

状态查询¶

方法	功能	返回类型
`get_all_status()`	获取所有电机状态	`Dict[str, Dict]`
`get_left_status()`	获取左臂状态	`Dict`
`get_right_status()`	获取右臂状态	`Dict`
`show_all_status()`	显示状态表格	`None`
`show_left_status()`	显示左臂状态	`None`
`show_right_status()`	显示右臂状态	`None`

零点设置¶

方法	功能	返回类型
`set_zero_all()`	设置所有电机零点	`dict`
`set_zero_left()`	设置左臂零点	`None`
`set_zero_right()`	设置右臂零点	`None`
`set_zero_range_all()`	设置零点范围	`None`
`set_zero_range_left()`	设置左臂零点范围	`None`
`set_zero_range_right()`	设置右臂零点范围	`None`

资源管理¶

方法	功能	返回类型
`shutdown()`	关闭所有连接	`None`