用Python和ROS 2构建简易机械臂：从零实现运动控制全流程
想象一下，你桌上摆放着一个由3D打印部件组装的小型机械臂，通过几行Python代码就能让它精准地抓取物体——这种成就感正是驱动许多机器人开发者的原动力。本文将带你用ROS 2和Python实现一个精简但完整的机械臂控制流程，避开复杂的理论推导，专注于可运行的代码和直观的视觉反馈。我们会从URDF建模开始，逐步实现正运动学计算、MoveIt2集成，最终完成点到点的轨迹规划。
1. 环境配置与基础模型搭建
1.1 ROS 2 Humble环境准备
首先确保已安装ROS 2 Humble版本，这是目前最新的LTS版本，对MoveIt2支持最为完善。推荐使用Ubuntu 22.04系统，通过以下命令安装基础环境：
sudo apt update sudo apt install ros-humble-desktop接着安装MoveIt2和相关依赖：
sudo apt install ros-humble-moveit ros-humble-moveit-visual-tools验证安装是否成功：
ros2 pkg list | grep moveit1.2 创建机械臂URDF模型
我们设计一个3自由度的简化机械臂，包含基座、两个旋转关节和一个末端执行器。创建simple_arm/urdf/arm.urdf文件：
<?xml version="1.0"?> <robot name="simple_arm"> <!-- 基座 --> <link name="base_link"> <visual> <geometry><box size="0.2 0.2 0.05"/></geometry> <material name="silver"><color rgba="0.8 0.8 0.8 1"/></material> </visual> </link> <!-- 第一关节 --> <joint name="joint1" type="revolute"> <parent link="base_link"/> <child link="link1"/> <axis xyz="0 0 1"/> <limit lower="-3.14" upper="3.14" effort="10" velocity="1.0"/> </joint> <link name="link1"> <visual> <geometry><cylinder length="0.4" radius="0.03"/></geometry> <material name="blue"><color rgba="0 0 1 1"/></material> </visual> </link> <!-- 第二关节 --> <joint name="joint2" type="revolute"> <parent link="link1"/> <child link="link2"/> <origin xyz="0.2 0 0" rpy="0 0 0"/> <axis xyz="0 1 0"/> <limit lower="-1.57" upper="1.57" effort="10" velocity="1.0"/> </joint> <link name="link2"> <visual> <geometry><cylinder length="0.3" radius="0.025"/></geometry> <material name="red"><color rgba="1 0 0 1"/></material> </visual> </link> <!-- 末端执行器 --> <joint name="ee_joint" type="fixed"> <parent link="link2"/> <child link="ee_link"/> <origin xyz="0.15 0 0" rpy="0 0 0"/> </joint> <link name="ee_link"> <visual> <geometry><box size="0.05 0.05 0.05"/></geometry> <material name="green"><color rgba="0 1 0 1"/></material> </visual> </link> </robot>使用以下命令检查URDF模型是否正确：
check_urdf arm.urdf2. 正运动学实现与可视化
2.1 Python正运动学计算
创建kinematics.py实现正运动学计算：
import numpy as np import math from geometry_msgs.msg import Pose class ArmKinematics: def __init__(self): self.link_lengths = [0.4, 0.3] # 连杆长度 def forward_kinematics(self, joint_angles): """计算末端执行器位姿""" theta1, theta2 = joint_angles L1, L2 = self.link_lengths # 计算各关节位置 x1 = L1/2 * math.cos(theta1) y1 = L1/2 * math.sin(theta1) x2 = L1 * math.cos(theta1) + L2/2 * math.cos(theta1 + theta2) y2 = L1 * math.sin(theta1) + L2/2 * math.sin(theta1 + theta2) # 末端位置 ee_x = L1 * math.cos(theta1) + L2 * math.cos(theta1 + theta2) ee_y = L1 * math.sin(theta1) + L2 * math.sin(theta1 + theta2) # 转换为Pose消息 pose = Pose() pose.position.x = ee_x pose.position.y = ee_y pose.position.z = 0.0 return pose def jacobian(self, joint_angles): """计算雅可比矩阵""" theta1, theta2 = joint_angles L1, L2 = self.link_lengths J = np.array([ [-L1*math.sin(theta1)-L2*math.sin(theta1+theta2), -L2*math.sin(theta1+theta2)], [L1*math.cos(theta1)+L2*math.cos(theta1+theta2), L2*math.cos(theta1+theta2)] ]) return J2.2 RViz可视化实现
创建visualizer.py用于实时显示机械臂状态：
import rclpy from rclpy.node import Node from tf2_ros import TransformBroadcaster from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import JointState class ArmVisualizer(Node): def __init__(self): super().__init__('arm_visualizer') self.joint_pub = self.create_publisher(JointState, 'joint_states', 10) self.tf_broadcaster = TransformBroadcaster(self) timer_period = 0.1 # 100ms self.timer = self.create_timer(timer_period, self.update_visualization) self.joint_angles = [0.0, 0.0] # 初始关节角度 def update_joint_angles(self, new_angles): self.joint_angles = new_angles def update_visualization(self): # 发布关节状态 joint_state = JointState() joint_state.header.stamp = self.get_clock().now().to_msg() joint_state.name = ['joint1', 'joint2'] joint_state.position = self.joint_angles self.joint_pub.publish(joint_state) # 广播TF变换 links = ['base_link', 'link1', 'link2', 'ee_link'] for i in range(len(links)-1): transform = TransformStamped() transform.header.stamp = joint_state.header.stamp transform.header.frame_id = links[i] transform.child_frame_id = links[i+1] self.tf_broadcaster.sendTransform(transform)3. MoveIt2集成与配置
3.1 创建MoveIt配置包
使用MoveIt Setup Assistant生成配置包：
ros2 launch moveit_setup_assistant setup_assistant.launch.py按照向导完成以下步骤：
- 加载之前创建的URDF文件
- 配置自碰撞矩阵
- 定义虚拟关节（不需要物理连接）
- 设置规划组（planning group）命名为"arm_group"
- 添加末端执行器
- 配置默认姿态（如"home"位置）
- 生成配置文件
3.2 轨迹规划Python接口
创建moveit_controller.py实现与MoveIt的交互：
import rclpy from rclpy.node import Node from moveit_msgs.srv import GetPositionIK from moveit_msgs.msg import MotionPlanRequest from geometry_msgs.msg import PoseStamped class MoveItController(Node): def __init__(self): super().__init__('moveit_controller') # 创建IK服务客户端 self.ik_client = self.create_client( GetPositionIK, '/compute_ik') while not self.ik_client.wait_for_service(timeout_sec=1.0): self.get_logger().info('等待IK服务...') def compute_ik(self, target_pose): """计算逆运动学""" request = GetPositionIK.Request() request.ik_request.group_name = "arm_group" request.ik_request.pose_stamped = target_pose request.ik_request.timeout.sec = 2 future = self.ik_client.call_async(request) rclpy.spin_until_future_complete(self, future) if future.result() is not None: return future.result().solution.joint_state.position else: self.get_logger().error('IK计算失败') return None def plan_cartesian_path(self, start_pose, end_pose): """笛卡尔空间路径规划""" # 实现笛卡尔路径规划逻辑 pass4. 完整轨迹规划实现
4.1 从点到点运动实现
结合前面模块，创建完整控制节点arm_controller.py：
import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose from moveit_msgs.msg import MotionPlanResponse from .kinematics import ArmKinematics from .moveit_controller import MoveItController class ArmController(Node): def __init__(self): super().__init__('arm_controller') self.kinematics = ArmKinematics() self.moveit = MoveItController() # 示例目标位置序列 self.targets = [ [0.5, 0.2], # 目标1 [0.3, 0.4], # 目标2 [0.6, 0.1] # 目标3 ] self.current_target = 0 timer_period = 5.0 # 每5秒移动到下一个目标 self.timer = self.create_timer(timer_period, self.move_to_next_target) def move_to_next_target(self): if self.current_target >= len(self.targets): self.get_logger().info("已完成所有目标点") self.timer.cancel() return target = self.targets[self.current_target] self.get_logger().info(f"移动到目标: {target}") # 创建目标位姿 target_pose = Pose() target_pose.position.x = target[0] target_pose.position.y = target[1] # 计算逆运动学 joint_positions = self.moveit.compute_ik(target_pose) if joint_positions: self.get_logger().info( f"关节角度: {[f'{x:.2f}' for x in joint_positions]}" ) # 实际控制机械臂移动的代码... self.current_target += 14.2 轨迹优化与碰撞检测
在MoveIt配置中启用碰撞检测：
# moveit_config/config/sensors_3d.yaml sensors: - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater point_cloud_topic: /points max_range: 5.0 padding: 0.1 resolution: 0.02在规划请求中添加碰撞检查：
def plan_safe_path(self, start, goal): request = MotionPlanRequest() request.group_name = "arm_group" request.allowed_planning_time = 5.0 request.planner_id = "RRTConnect" request.avoid_collisions = True # 设置起始和目标状态... return self.plan(request)5. 进阶功能扩展
5.1 添加力反馈模拟
创建虚拟力传感器：
class ForceSensorSimulator: def __init__(self): self.force_threshold = 5.0 # 5N阈值 def check_force(self, joint_efforts): """检查是否超过力阈值""" return any(effort > self.force_threshold for effort in joint_efforts) def get_force_direction(self, joint_angles): """计算受力方向""" jacobian = self.kinematics.jacobian(joint_angles) # 将关节力矩转换为末端力 wrench = np.linalg.pinv(jacobian.T).dot(joint_efforts) return wrench[:2] # 只考虑xy平面5.2 实现拾放动作
添加简单的夹爪控制：
class GripperController: def __init__(self): self.gripper_state = 'open' # 或 'closed' def control_gripper(self, command): if command == 'open': self.gripper_state = 'open' # 发送打开命令... elif command == 'close': self.gripper_state = 'closed' # 发送关闭命令...整合到主控制流程中：
def pick_and_place(self, pick_pose, place_pose): # 移动到拾取位置上方 self.move_to(pick_pose.x, pick_pose.y + 0.1) # 下降 self.move_to(pick_pose.x, pick_pose.y) # 抓取 self.gripper.control_gripper('close') # 抬起 self.move_to(pick_pose.x, pick_pose.y + 0.1) # 移动到放置位置上方 self.move_to(place_pose.x, place_pose.y + 0.1) # 放置 self.move_to(place_pose.x, place_pose.y) self.gripper.control_gripper('open') # 返回安全位置 self.move_to_home()
