用Python和ROS 2搞定一个简易机械臂：从URDF建模到MoveIt2轨迹规划实战

用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 version1.0? robot namesimple_arm !-- 基座 -- link namebase_link visual geometrybox size0.2 0.2 0.05//geometry material namesilvercolor rgba0.8 0.8 0.8 1//material /visual /link !-- 第一关节 -- joint namejoint1 typerevolute parent linkbase_link/ child linklink1/ axis xyz0 0 1/ limit lower-3.14 upper3.14 effort10 velocity1.0/ /joint link namelink1 visual geometrycylinder length0.4 radius0.03//geometry material namebluecolor rgba0 0 1 1//material /visual /link !-- 第二关节 -- joint namejoint2 typerevolute parent linklink1/ child linklink2/ origin xyz0.2 0 0 rpy0 0 0/ axis xyz0 1 0/ limit lower-1.57 upper1.57 effort10 velocity1.0/ /joint link namelink2 visual geometrycylinder length0.3 radius0.025//geometry material nameredcolor rgba1 0 0 1//material /visual /link !-- 末端执行器 -- joint nameee_joint typefixed parent linklink2/ child linkee_link/ origin xyz0.15 0 0 rpy0 0 0/ /joint link nameee_link visual geometrybox size0.05 0.05 0.05//geometry material namegreencolor rgba0 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(theta1theta2), -L2*math.sin(theta1theta2)], [L1*math.cos(theta1)L2*math.cos(theta1theta2), L2*math.cos(theta1theta2)] ]) 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[i1] 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_sec1.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.pyimport 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()