ROS2 底层运动参考例程
底层运动开发例程实现了将机器人从任意初始关节位置，复位至预备状态，然后用两种不同模式控制机器人踝关节摆动和手指位置。例程源代码位于 pnd_ros2/example，可从 此处 访问。源码目录按照机器人型号分别对应sample代码例子：
├── adam_lite
│ └── adam_lite_low_level_example.py
├── adam_sp
│ └── adam_sp_low_level_example.py
├── adam_pro
│ └── adam_pro_low_level_example.py
└── adam_u
└── adam_u_low_level_example.py
运行方式
- 确保ros2 msg接口编译完成。用户自定义$ROOT 目录
3. 运行参考例程，以下均以Adam_Pro为例
cd $ROOT/pnd_ros2
# 注册ROS2环境
source /opt/ros/humble/setup.bash
# 注册pnd_adam ros2 消息接口
source install/setup.bash
# 运行例子
python3 example/adam_pro/adam_pro_low_level_example.py
代码解析
以下代码针对Adam Pro 版本机型，其他版本机型的例程结构均类似，只是关节index定义不同。
核心类型与功能介绍
| 类型名称 | 描述 |
|---|---|
| ADAMJointIndex | 所有关节按名称排序索引。也是各个关节在LowCmd.MotorCmd命令数组中的下标 |
| LowCmd | 电机指令相关ROS2消息结构体 |
| LowState | 电机状态相关ROS2消息结构体 |
| HandCmd | 手指命令相关ROS2消息结构体 |
| DemonController | 核心控制逻辑类（ROS2 node） |
例程的主程序定义在 adam_pro_low_level_example.py 中，主程序实例化 DemonController ROS2 node类。
在 DemonController 的构造函数中创建了两个发布器，一个订阅器和定时器，以及对应的ROS2消息结构体：
- 定义12根手指在握拳时的位置参数
DemonController.close_hand，展开双手时的位置参数DemonController.open_hand - 创建
lowcmd主题电机位置发布器，handcmd主题手指位置发布器 - 底层状态接收
lowstate主题回调，调用函数DemonController.getLowState - 以
DemonController.control_dt_定义的时间间隔定时调用Custom.LowCmdWrite
具体如下代码所示：
class DemonController(Node):
def __init__(self):
super().__init__('demon_controller')
self.time_ = 0.0
self.control_dt_ = 0.0025 # [2ms]
self.duration_ = 3.0 # [3 s]
self.counter_ = 0
self.lowcmd_pub_ = self.create_publisher(LowCmd, 'lowcmd', 10)
self.handcmd_pub_ = self.create_publisher(HandCmd, 'handcmd', 10)
self.getstate_flag = False
self.timer = self.create_timer(self.control_dt_, self.LowCmdWrite)
self.mutex = threading.Lock()
self.low_state = LowState()
self.low_state.motor_state = [MotorState() for _ in range(ADAM_PRO_NUM_MOTOR)]
# print("init len(low_state.motor_state)=", len(self.low_state.motor_state))
self.low_cmd = LowCmd()
self.low_cmd.motor_cmd = [MotorCmd() for _ in range(ADAM_PRO_NUM_MOTOR)]
self.hand_cmd = HandCmd()
self.close_hand = np.array([500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500], dtype=int)
self.open_hand = np.array([1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000], dtype=int)
self.lowstate_sub_ = self.create_subscription(
LowState,
"lowstate",
self.getLowState,
10)
底层状态接收回调函数
当接收到话题 lowstate 的底层状态消息时，程序会自动回调 CDemonController.getLowState 函数。在回调函数中提取了电机的状态。底层状态的结构体定义可参考 ROS2消息定义。实现了：
- 记录当前各个关节所在位置，为闭环控制提供数据
- 补齐MotorState对象个数，避免后续越界访问崩溃
- 隔一段时间打印IMU欧拉角
def getLowState(self, msg):
# 获取实际接收到的电机数量
received_len = len(msg.motor_state)
if received_len < ADAM_PRO_NUM_MOTOR:
# 如果收到的数据不足，先复制已有的数据
self.low_state = msg
# 计算缺失的数量
missing_count = ADAM_PRO_NUM_MOTOR - received_len
# 用默认的 MotorState 对象补齐列表
# 这样 self.low_state.motor_state[i].q 就会默认为 0.0
self.low_state.motor_state.extend([MotorState() for _ in range(missing_count)])
# 可选：打印警告信息（仅打印一次或限制频率）
# print(f"Warning: Received only {received_len} motor states. Padding with {missing_count} default states.")
else:
# 如果数量足够或更多，直接赋值
self.low_state = msg
self.getstate_flag = True
self.counter_ +=1
if (self.counter_ % 500 == 0) :
self.counter_ = 0
print(self.low_state.imu_state.ypr)
底层指令发送函数
CDemonController.LowCmdWrite 用于周期性发送底层指令，实现了：
- 阶段 1：将 Adam 机器人从任意初始关节位置，复位至预备状态 (程序启动的前 3秒)；
- 阶段 2：采用踝关节 PR控制模式控制踝关节持续摆动 3秒；
- 阶段 3：把左右手指打开，一直到程序运行终止。
def LowCmdWrite(self):
if(self.getstate_flag):
self.time_ += self.control_dt_
if self.time_ < self.duration_ :
# [Stage 1]: set robot to zero posture
for i in range(ADAM_PRO_NUM_MOTOR):
ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
self.low_cmd.motor_cmd[i].mode = 1 # 1:Enable, 0:Disable
self.low_cmd.motor_cmd[i].tau = 0.
self.low_cmd.motor_cmd[i].q = (1.0 - ratio) * self.low_state.motor_state[i].q
self.low_cmd.motor_cmd[i].dq = 0.
self.low_cmd.motor_cmd[i].kp = Kp[i]
self.low_cmd.motor_cmd[i].kd = Kd[i]
for i in range(12):
self.hand_cmd.position[i] = self.close_hand[i]
elif self.time_ < self.duration_ * 2 :
max_P = np.pi * 30.0 / 180.0
max_R = np.pi * 10.0 / 180.0
t = self.time_ - self.duration_
L_P_des = max_P * np.sin(2.0 * np.pi * t)
L_R_des = max_R * np.sin(2.0 * np.pi * t)
R_P_des = max_P * np.sin(2.0 * np.pi * t)
R_R_des = -max_R * np.sin(2.0 * np.pi * t)
self.low_cmd.motor_cmd[ADAMJointIndex.LeftAnklePitch].q = L_P_des
self.low_cmd.motor_cmd[ADAMJointIndex.LeftAnkleRoll].q = L_R_des
self.low_cmd.motor_cmd[ADAMJointIndex.RightAnklePitch].q = R_P_des
self.low_cmd.motor_cmd[ADAMJointIndex.RightAnkleRoll].q = R_R_des
elif self.time_ < self.duration_ * 3 :
for i in range(ADAM_PRO_NUM_MOTOR):
self.low_cmd.motor_cmd[i].mode = 1 # 1:Enable, 0:Disable
self.low_cmd.motor_cmd[i].tau = 0.
self.low_cmd.motor_cmd[i].q = 0.
self.low_cmd.motor_cmd[i].dq = 0.
self.low_cmd.motor_cmd[i].kp = Kp[i]
self.low_cmd.motor_cmd[i].kd = Kd[i]
for i in range(12):
self.hand_cmd.position[i] = self.open_hand[i]
self.lowcmd_pub_.publish(self.low_cmd)
self.handcmd_pub_.publish(self.hand_cmd)