运动控制¶

Arm 类¶

Arm 类是控制单个机械臂的核心接口，用于控制基于 Robstride 电机的机械臂。每条机械臂有8个电机，通过 CAN 总线进行通信。

功能概览¶

Arm
├── 初始化与连接
│   ├── __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() # 设置所有零点
    └── home_all()          # 回零位并设零点

电机配置映射¶

电机ID	型号	关节类型	扭矩特性
1-2	RS04	大扭矩关节	高扭矩，低速度
3-4	RS03	中扭矩关节	中等扭矩和速度
5-8	RS00	小扭矩关节/夹爪	低扭矩，高速度

构造函数¶

Arm¶

Arm(can_channel, side=None, bustype='socketcan', bitrate=1000000,
    motor_ids=None, direction_multipliers=None, auto_enable_can=True, password=None, log=None)

初始化机械臂控制器，建立CAN总线连接并加载配置。

参数名	类型	默认值	说明
`can_channel`	`str`	-	CAN通道名称，如 `'can0'`、`'can1'`
`side`	`str`	`None`	机械臂方位：`'left'`/`'right'`/`None`
`bustype`	`str`	`'socketcan'`	CAN总线类型
`bitrate`	`int`	`1000000`	CAN总线波特率（1Mbps）
`motor_ids`	`List[int]`	`[1-8]`	要控制的电机ID列表
`direction_multipliers`	`Dict[int, float]`	`None`	自定义方向系数
`auto_enable_can`	`bool`	`True`	是否自动启用CAN接口
`password`	`str`	`None`	sudo密码，用于自动启用CAN接口
`log`	`callable`	`None`	日志函数

from openarmx_arm_driver import Arm

# 创建右臂实例
right_arm = Arm('can0', side='right')

# 创建左臂实例（电机方向自动反转）
left_arm = Arm('can1', side='left')

# 使用密码自动启用CAN接口
arm = Arm('can0', side='right', password='your_password')

# 自定义方向配置
custom_arm = Arm('can0', direction_multipliers={1: -1, 2: -1, 3: 1, 4: 1, 5: 1, 6: 1, 7: 1, 8: 1})

注意：如果提供 password 参数，当CAN接口未启用时，将自动输入sudo密码启用接口；否则需要手动输入密码

快速开始¶

from openarmx_arm_driver import Arm

# 创建机械臂实例
arm = Arm('can0', side='right')

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

# 设置为 MIT 模式
arm.set_mode('mit')

# 移动电机
arm.move_joint_mit(motor_id=5, position=1.0, kp=20, kd=2)

# 获取电机状态
status = arm.get_status(motor_id=5)
print(f"角度: {status['angle']}, 速度: {status['velocity']}")

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

# 关闭连接
arm.close()

基础控制方法¶

enable¶

enable(motor_id, timeout=1.0, verbose=False) -> int

使能指定电机，使其进入可控制状态。

参数名	类型	默认值	说明
`motor_id`	`int`	-	电机ID，取值范围 1-8
`timeout`	`float`	`1.0`	操作超时时间（秒）
`verbose`	`bool`	`False`	是否输出详细信息

返回值: int

0 表示成功
1 表示失败

result = arm.enable(motor_id=5)
if result == 0:
    print("5号电机使能成功")

disable¶

disable(motor_id, timeout=1.0, verbose=False) -> int

停止（失能）指定电机，使其进入安全状态。

参数名	类型	默认值	说明
`motor_id`	`int`	-	电机ID，取值范围 1-8
`timeout`	`float`	`1.0`	操作超时时间（秒）
`verbose`	`bool`	`False`	是否输出详细信息

返回值: int

0 表示成功
1 表示失败

enable_all¶

enable_all(verbose=False) -> Dict[int, int]

使能所有电机（批量操作）。

返回值: Dict[int, int]

每个电机的状态字典（键：电机ID，值：状态码）

status = arm.enable_all()
if all(v == 0 for v in status.values()):
    print("所有电机使能成功")

disable_all¶

disable_all(verbose=False) -> Dict[int, int]

停止所有电机（批量操作）。

返回值: Dict[int, int]

每个电机的状态字典

set_mode¶

set_mode(mode, motor_id=None, timeout=1.0, verbose=False) -> int

设置电机控制模式。

参数名	类型	默认值	说明
`mode`	`str`/`int`	-	控制模式
`motor_id`	`int`	`None`	电机ID，`None`表示所有电机
`timeout`	`float`	`1.0`	操作超时时间
`verbose`	`bool`	`False`	是否输出详细信息

支持的模式:

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

注意：目前只提供 mit 和 csp 两种模式，其他模式后续陆续添加！

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

# 设置单个电机为CSP模式
arm.set_mode('csp', motor_id=5)

状态获取方法¶

get_status¶

get_status(motor_id, timeout=1.0, verbose=False) -> Optional[Dict]

获取电机完整状态信息。

参数名	类型	默认值	说明
`motor_id`	`int`	-	电机ID，取值范围 1-8
`timeout`	`float`	`1.0`	操作超时时间（秒）
`verbose`	`bool`	`False`	是否输出详细信息

返回值: Dict 或 None

字段名	类型	说明	单位
`angle`	`float`	当前角度位置	rad
`velocity`	`float`	当前运动速度	rad/s
`torque`	`float`	当前输出力矩	Nm
`temperature`	`float`	电机温度	°C
`mode_status`	`str`	控制模式状态	-
`fault_status`	`str`	故障状态	-

status = arm.get_status(motor_id=5)
if status:
    print(f"角度: {status['angle']:.3f} rad")
    print(f"速度: {status['velocity']:.3f} rad/s")
    print(f"力矩: {status['torque']:.3f} Nm")
    print(f"温度: {status['temperature']:.1f} °C")
    print(f"模式: {status['mode_status']}")
    print(f"状态: {status['fault_status']}")

方法	返回内容	适用场景
`get_status()`	角度、速度、力矩、温度、模式、故障状态	完整状态监控、故障诊断
`get_telemetry()`	机械位置、机械速度、电流、电压	实时控制、底层数据采集

get_telemetry¶

get_telemetry(motor_id, timeout=1.0, verbose=False) -> Optional[Dict]

获取电机基本遥测数据。

返回值: Dict 或 None

字段名	类型	说明	单位
`mech_pos`	`float`	机械位置	rad
`mech_vel`	`float`	机械速度	rad/s
`iqf`	`float`	电流反馈值	A
`vbus`	`float`	总线电压	V

telemetry = arm.get_telemetry(5)
if telemetry:
    print(f"位置: {telemetry['mech_pos']:.3f} rad")

get_all_status¶

get_all_status(timeout=1.0, verbose=False) -> Dict[int, Optional[Dict]]

获取所有电机的状态（批量查询）。

返回值: Dict[int, Optional[Dict]]

所有电机的状态字典

all_status = arm.get_all_status()
for motor_id, status in all_status.items():
    if status:
        print(f"电机{motor_id}: 角度={status['angle']:.3f} rad")

show_motor_status¶

show_motor_status(motor_id=None, show_header=True) -> None

以表格形式显示电机状态信息。

参数名	类型	默认值	说明
`motor_id`	`int`	`None`	电机ID，`None`显示所有
`show_header`	`bool`	`True`	是否显示表头

MIT模式控制方法¶

MIT模式提供灵活的运动控制接口，支持位置、速度和力矩的复合控制。

move_joint_mit¶

move_joint_mit(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模式运动控制。

参数名	类型	默认值	说明	单位
`motor_id`	`int`	-	电机ID	-
`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`	超时时间	s

增益参数建议:

电机型号	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

# 基本位置控制
arm.move_joint_mit(motor_id=5, position=1.0, kp=20.0, kd=2.0)

# 复合控制
arm.move_joint_mit(motor_id=2, position=1.2, velocity=0.5, torque=1.0, kp=15.0, kd=1.5)

move_joint_position¶

move_joint_position(motor_id, position, kp=5.0, kd=0.5, verbose=False) -> int

MIT模式简化位置控制。

参数名	类型	默认值	说明
`motor_id`	`int`	-	电机ID
`position`	`float`	-	目标位置（rad）
`kp`	`float`	`5.0`	位置增益
`kd`	`float`	`0.5`	速度增益

arm.move_joint_position(motor_id=5, position=1.0)

move_joint_velocity¶

move_joint_velocity(motor_id, velocity, kd=2.0, verbose=False) -> int

MIT模式速度控制。

参数名	类型	默认值	说明
`motor_id`	`int`	-	电机ID
`velocity`	`float`	-	目标速度（rad/s）
`kd`	`float`	`2.0`	速度增益

arm.move_joint_velocity(motor_id=5, velocity=0.5)

move_joint_torque¶

move_joint_torque(motor_id, torque, verbose=False) -> int

MIT模式扭矩控制。

参数名	类型	默认值	说明
`motor_id`	`int`	-	电机ID
`torque`	`float`	-	目标扭矩（Nm）

扭矩范围:

电机型号	扭矩范围
RS04（关节1-2）	±120.0 Nm
RS03（关节3-4）	±60.0 Nm
RS00（关节5-8）	±14.0 Nm

arm.move_joint_torque(motor_id=5, torque=1.0)

home_joint¶

home_joint(motor_id, kp=5.0, kd=0.5, verbose=False) -> int

将指定电机移动到零位（归零操作）。

arm.home_joint(motor_id=5)

home_all¶

home_all(kp=5.0, kd=0.5, verbose=False) -> Dict[int, int]

所有电机归零（批量操作）。

参数名	类型	默认值	说明
`kp`	`float` 或 `List[float]`	`5.0`	位置增益
`kd`	`float` 或 `List[float]`	`0.5`	速度增益
`verbose`	`bool`	`False`	是否打印详细信息

返回值: Dict[int, int]

每个电机的状态字典（键：电机ID，值：状态码）

# 所有电机使用相同的kp和kd值
arm.home_all(kp=5.0, kd=0.5)

# 每个电机使用不同的kp和kd值
arm.home_all(
    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]
)

CSP模式控制方法¶

CSP（Cyclic Synchronous Position）模式提供高精度的位置控制。

move_joint_csp¶

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

CSP模式位置控制。

参数名	类型	默认值	说明
`motor_id`	`int`	-	电机ID
`position`	`float`	-	目标位置（rad）
`wait_response`	`bool`	`False`	是否等待位置到达
`timeout`	`float`	`0.2`	超时时间（秒）

arm.move_joint_csp(motor_id=5, position=1.0)

set_csp_limits¶

set_csp_limits(motor_id=None, speed_limit=None, current_limit=None, timeout=0.2, verbose=False) -> int

设置CSP模式的速度和电流限制。

参数名	类型	默认值	说明
`motor_id`	`int`	`None`	电机ID，`None`表示所有电机
`speed_limit`	`float`	`None`	速度限制（rad/s）
`current_limit`	`float`	`None`	电流限制（A）

限制参数建议:

电机型号	速度限制	电流限制
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

move_to_csp¶

move_to_csp(motor_id, position, speed_limit=None, current_limit=None, timeout=1.0, verbose=False) -> int

CSP模式完整工作流程：自动设置模式、配置限制并执行位置控制。

arm.move_to_csp(motor_id=5, position=1.0, speed_limit=10.0, current_limit=5.0)

零点设置方法¶

set_zero¶

set_zero(motor_id=None, timeout=1.0, verbose=False) -> int

设置电机的当前位置为零点。

参数名	类型	默认值	说明
`motor_id`	`int`	`None`	电机ID，`None`表示所有电机
`timeout`	`float`	`1.0`	操作超时时间

# 设置单个电机零点
arm.set_zero(motor_id=5)

# 设置所有电机零点
arm.set_zero()

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

set_zero_range¶

set_zero_range(motor_id=None, zero_sta=1, timeout=1.0, verbose=False) -> int

设置零点表示范围（角度范围模式）。

参数名	类型	默认值	说明
`zero_sta`	`int`	`1`	`0`: 0~2π，`1`: -π~π

参数读取方法¶

read_parameter¶

read_parameter(motor_id=None, param_index=None, timeout=1.0, verbose=False)

读取电机控制器的内部参数。

常用参数索引:

索引	参数名称	类型	单位
`0x7019`	当前机械角度	`float`	rad
`0x701A`	当前机械速度	`float`	rad/s
`0x701B`	当前电流	`float`	A
`0x701C`	当前温度	`float`	°C

state, angle = arm.read_parameter(motor_id=5, param_index=0x7019)
if state == 0:
    print(f"角度: {angle} rad")

配置查询方法¶

get_motor_config¶

get_motor_config(motor_id=None) -> Union[Dict, Dict[int, Dict]]

获取电机的配置参数。

返回字典结构:

{
    'model': 'RS04',           # 电机型号
    'direction': 1.0,          # 方向系数
    'position_min': -12.57,    # 位置下限
    'position_max': 12.57,     # 位置上限
    'torque_min': -12.0,       # 扭矩下限
    'torque_max': 12.0,        # 扭矩上限
    'kp_max': 500.0,           # KP增益上限
    'kd_max': 5.0              # KD增益上限
}

get_motor_limits¶

get_motor_limits(motor_id=None) -> Union[Dict, Dict[int, Dict]]

获取电机的所有限制参数。

返回字典结构:

{
    'P_MIN': -12.57,    # 位置最小值
    'P_MAX': 12.57,     # 位置最大值
    'V_MIN': -30.0,     # 速度最小值
    'V_MAX': 30.0,      # 速度最大值
    'T_MIN': -12.0,     # 扭矩最小值
    'T_MAX': 12.0,      # 扭矩最大值
}

资源管理方法¶

close¶

close() -> None

关闭CAN总线连接，释放相关资源。

# 标准模式
arm = Arm('can0', side='right')
try:
    arm.enable_all()
    # ... 执行控制操作
finally:
    arm.close()

# 推荐：使用with语句
with Arm('can0', side='right') as arm:
    arm.enable_all()
    arm.move_joint_mit(5, position=1.0)
    # 自动调用close()

注意：必须调用close()释放CAN总线资源，否则可能导致资源泄漏

完整示例¶

基本控制流程¶

from openarmx_arm_driver import Arm

# 创建机械臂实例
arm = Arm('can0', side='right')

try:
    # 1. 使能所有电机
    status = arm.enable_all()
    if all(v == 0 for v in status.values()):
        print("所有电机使能成功")

    # 2. 设置MIT模式
    arm.set_mode('mit')

    # 3. 归零
    arm.home_all(kp=5.0, kd=0.5)

    # 4. 控制运动
    for motor_id in [1, 2, 3, 4]:
        arm.move_joint_mit(motor_id, position=0.5, kp=10.0, kd=1.0)

    # 5. 显示状态
    arm.show_motor_status()

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

finally:
    arm.close()

使用with语句（推荐）¶

from openarmx_arm_driver import Arm

with Arm('can0', side='right') as arm:
    arm.enable_all()
    arm.set_mode('mit')

    # 位置控制
    arm.move_joint_position(motor_id=5, position=1.0, kp=20.0, kd=2.0)

    # 获取状态
    status = arm.get_status(motor_id=5)
    print(f"当前角度: {status['angle']:.3f} rad")

    arm.disable_all()
    # 自动调用close()

带异常处理的完整示例¶

from openarmx_arm_driver import Arm
from openarmx_arm_driver.exceptions import (
    CANInitializationError,
    InvalidMotorIDError,
    CANTimeoutError
)

try:
    arm = Arm('can0', side='right', password='your_password')

    # 使能电机
    arm.enable_all()
    arm.set_mode('mit')

    # 批量控制
    positions = [0.1, 0.2, 0.3, 0.4, 0.0, 0.0, 0.0, 0.0]
    for motor_id, pos in enumerate(positions, start=1):
        result = arm.move_joint_mit(motor_id, position=pos, kp=10.0, kd=1.0)
        if result != 0:
            print(f"电机{motor_id}控制失败")

    # 显示所有状态
    arm.show_motor_status()

except CANInitializationError as e:
    print(f"CAN初始化失败: {e}")
except InvalidMotorIDError as e:
    print(f"无效电机ID: {e}")
except CANTimeoutError as e:
    print(f"通信超时: {e}")
finally:
    if 'arm' in locals():
        arm.disable_all()
        arm.close()

CSP模式轨迹控制¶

from openarmx_arm_driver import Arm
import time
import math

with Arm('can0', side='right') as arm:
    arm.enable_all()

    # 设置CSP模式
    arm.set_mode('csp')

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

    # 正弦轨迹控制
    for i in range(200):
        pos = 0.5 * math.sin(i * 0.05)  # 正弦轨迹
        arm.move_joint_csp(motor_id=5, position=pos)
        time.sleep(0.01)

    # 归零
    arm.move_joint_csp(motor_id=5, position=0.0)

    arm.disable_all()

多电机协调控制¶

from openarmx_arm_driver import Arm
import time

with Arm('can0', side='right') as arm:
    arm.enable_all()
    arm.set_mode('mit')

    # 定义关节轨迹点
    waypoints = [
        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],  # 初始位置
        [0.2, 0.1, 0.1, 0.1, 0.0, 0.0, 0.0, 0.0],  # 位置1
        [0.4, 0.2, 0.2, 0.1, 0.0, 0.0, 0.0, 0.0],  # 位置2
        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],  # 返回初始
    ]

    # 执行轨迹
    for waypoint in waypoints:
        for motor_id, pos in enumerate(waypoint, start=1):
            arm.move_joint_mit(motor_id, position=pos, kp=15.0, kd=1.5)
        time.sleep(1.0)  # 等待到达

    arm.disable_all()

方法速查表¶

基础控制¶

方法	功能	返回类型
`enable(motor_id)`	使能单个电机	`int`
`disable(motor_id)`	失能单个电机	`int`
`enable_all()`	使能所有电机	`Dict[int, int]`
`disable_all()`	失能所有电机	`Dict[int, int]`
`set_mode(mode, motor_id)`	设置控制模式	`int`

状态查询¶

方法	功能	返回类型
`get_status(motor_id)`	获取完整状态	`Optional[Dict]`
`get_telemetry(motor_id)`	获取遥测数据	`Optional[Dict]`
`get_all_status()`	获取所有电机状态	`Dict[int, Optional[Dict]]`
`show_motor_status()`	显示状态表格	`None`

MIT模式控制¶

方法	功能	返回类型
`move_joint_mit()`	MIT模式完整控制	`int`
`move_joint_position()`	MIT简化位置控制	`int`
`move_joint_velocity()`	MIT速度控制	`int`
`move_joint_torque()`	MIT扭矩控制	`int`
`home_joint(motor_id)`	单电机归零	`int`
`home_all()`	所有电机归零	`Dict[int, int]`

CSP模式控制¶

方法	功能	返回类型
`move_joint_csp()`	CSP位置控制	`int`
`set_csp_limits()`	设置速度/电流限制	`int`
`move_to_csp()`	CSP完整工作流程	`int`

零点设置¶

方法	功能	返回类型
`set_zero(motor_id)`	设置零点	`int`
`set_zero_range(motor_id)`	设置零点范围	`int`

参数读取¶

方法	功能	返回类型
`read_parameter()`	读取电机参数	`Tuple` 或 `Dict`
`get_motor_config()`	获取电机配置	`Dict`
`get_motor_limits()`	获取电机限制	`Dict`

资源管理¶

方法	功能	返回类型
`close()`	关闭CAN连接	`None`