机器人 / 系统课时

计算预抓取位姿与接近路径

计算预抓取位姿与接近路径

已知物体位姿后,机械臂不能直接冲向夹持点。这里以 ROS 2 Jazzy 和 MoveIt 2 的 Panda 演示配置为例,先算出离物体有安全距离的预抓取目标,再验证从当前位置到该目标、以及随后沿工具轴接近的一小段路径,全程只规划不执行。

学习目标

  • 把物体位姿、工具方向和预抓取距离统一到同一坐标系。
  • 使用 MoveGroupInterface 规划预抓取段和无碰撞的笛卡尔接近段。
  • 根据规划结果、路径完成比例与场景碰撞体判断是否可以继续抓取。

明确物体坐标与接近方向

示例使用规划组 panda_arm,以 panda_link0 为基座参考系,以 panda_link8 为姿态目标链接。演示暂将该链接当作工具参考点;真机若以两指中点为 TCP,必须先利用标定的链接到 TCP 变换换算目标,不能把 TCP 位姿直接当作 panda_link8 位姿。

设物体中心位于基座坐标 (0.45, 0.00, 0.20) 米,物体高 0.10 m,桌面顶部位于 z=0.15 m。定义物体到接近终点的相对平移为 (0, 0, 0.15) 米,并绕工具局部 X 轴转 180°,使工具局部 +Z 指向世界坐标的下方。这样接近终点高度为 0.35 m,仍离物体顶部 0.10 m,尚不发生夹持接触。

来自相机的 PoseStamped 必须先按其 header.frame_id 和时间戳通过 tf2 转到 panda_link0;示例里的数字已经完成这一步,不能把光学坐标系下的数值直接复制进去。

计算预抓取位姿

令 G 为接近终点,P 为预抓取点。沿工具局部 -Z 反向退 d=0.12 m,位置关系为 TBP=TBOTOGTrans⁡(0,0,−d)T_{BP}=T_{BO}T_{OG}\operatorname{Trans}(0,0,-d)。由于工具局部 +Z 向下,反向后退会使世界坐标的 Z 增大:预抓取点为 (0.45, 0.00, 0.47) 米。

预抓取点和接近终点保持同一姿态。先自由空间规划到 P,再从 P 沿固定姿态下移 0.12 m 到 G。这里的 G 是安全停靠点而非实际闭爪位置;真正接触物体时还需处理夹爪几何、允许触碰对象和闭爪动作。

Panda 预抓取坐标系、接近方向与避障空间关系

使用 MoveIt 规划两段路径

前提是已安装或构建 moveit2_tutorials、Panda 配置和支持运动学求解的 MoveIt 2。先启动演示环境,并确认 /joint_states 在更新。第一个终端执行:

bash
source /opt/ros/jazzy/setup.bash
ros2 launch moveit2_tutorials move_group.launch.py

预期结果:move_group 启动,RViz 中可看到 Panda 模型;具体日志时间与输出行数依本机环境而异。另一个终端创建 C++ 包:

bash
source /opt/ros/jazzy/setup.bash
mkdir -p ~/moveit_ws/src
cd ~/moveit_ws/src
ros2 pkg create pregrasp_demo --build-type ament_cmake --dependencies rclcpp geometry_msgs moveit_msgs shape_msgs tf2 moveit_ros_planning_interface
mkdir -p pregrasp_demo/src pregrasp_demo/launch

预期结果:出现 pregrasp_demo/package.xml、pregrasp_demo/CMakeLists.txt 与 src、launch 目录。将以下内容保存为 pregrasp_demo/src/pregrasp_demo.cpp:

cpp
#include <geometry_msgs/msg/pose.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <rclcpp/rclcpp.hpp>
#include <shape_msgs/msg/solid_primitive.hpp>
#include <tf2/LinearMath/Transform.h>
#include <iomanip>
#include <iostream>
#include <string>
#include <thread>

using Pose = geometry_msgs::msg::Pose;

Pose toPose(const tf2::Transform& t) {
  Pose p;
  p.position.x = t.getOrigin().x();
  p.position.y = t.getOrigin().y();
  p.position.z = t.getOrigin().z();
  p.orientation.x = t.getRotation().x();
  p.orientation.y = t.getRotation().y();
  p.orientation.z = t.getRotation().z();
  p.orientation.w = t.getRotation().w();
  return p;
}

moveit_msgs::msg::CollisionObject box(
    const std::string& id, const std::string& frame,
    double x, double y, double z, double sx, double sy, double sz) {
  moveit_msgs::msg::CollisionObject c;
  c.header.frame_id = frame;
  c.id = id;
  c.operation = c.ADD;
  shape_msgs::msg::SolidPrimitive b;
  b.type = b.BOX;
  b.dimensions = {sx, sy, sz};
  Pose p;
  p.position.x = x; p.position.y = y; p.position.z = z;
  p.orientation.w = 1.0;
  c.primitives.push_back(b);
  c.primitive_poses.push_back(p);
  return c;
}

bool checkApproach(const rclcpp::Node::SharedPtr& node) {
  moveit::planning_interface::MoveGroupInterface arm(node, "panda_arm");
  arm.setPoseReferenceFrame("panda_link0");
  if (!arm.setEndEffectorLink("panda_link8")) return false;
  arm.setPlanningTime(8.0);
  arm.setMaxVelocityScalingFactor(0.1);

  // 物体坐标变换、接近终点姿态、沿工具 -Z 退让距离
  tf2::Transform base_obj, obj_goal, goal_pre;
  base_obj.setIdentity();
  base_obj.setOrigin(tf2::Vector3(0.45, 0.0, 0.20));
  obj_goal.setIdentity();
  obj_goal.setOrigin(tf2::Vector3(0.0, 0.0, 0.15));
  tf2::Quaternion q;
  q.setRPY(3.141592653589793, 0.0, 0.0);
  obj_goal.setRotation(q);
  goal_pre.setIdentity();
  goal_pre.setOrigin(tf2::Vector3(0.0, 0.0, -0.12));
  const Pose goal = toPose(base_obj * obj_goal);
  const Pose pre = toPose(base_obj * obj_goal * goal_pre);
  std::cout << std::fixed << std::setprecision(2)
            << "pre.z=" << pre.position.z
            << " stop.z=" << goal.position.z << std::endl;

  // 桌面与目标物体同步进入 Planning Scene;不允许忽略目标物体
  moveit::planning_interface::PlanningSceneInterface scene;
  if (!scene.applyCollisionObjects({
      box("table_demo", "panda_link0", 0.50, 0.0, 0.10, 0.70, 0.60, 0.10),
      box("object_demo", "panda_link0", 0.45, 0.0, 0.20, 0.05, 0.05, 0.10)})) {
    std::cout << "scene_update=fail" << std::endl;
    return false;
  }

  auto state = arm.getCurrentState(5.0);
  if (!state) {
    std::cout << "joint_state=missing" << std::endl;
    return false;
  }
  arm.setStartStateToCurrentState();
  arm.setPoseTarget(pre, "panda_link8");
  moveit::planning_interface::MoveGroupInterface::Plan first;
  const auto result = arm.plan(first);
  const auto& joints = first.trajectory.joint_trajectory;
  if (result != moveit::core::MoveItErrorCode::SUCCESS ||
      joints.points.empty()) {
    std::cout << "pregrasp_plan=fail" << std::endl;
    return false;
  }
  std::cout << "pregrasp_plan=success" << std::endl;

  // 用第一段计划的终点作为第二段起点,不移动真实机械臂
  state->setVariablePositions(joints.joint_names, joints.points.back().positions);
  state->update();
  arm.setStartState(*state);
  arm.clearPoseTargets();
  moveit_msgs::msg::RobotTrajectory approach;
  const double fraction = arm.computeCartesianPath({goal}, 0.01, approach, true);
  std::cout << "approach_fraction=" << fraction << std::endl;
  return fraction >= 0.999 && !approach.joint_trajectory.points.empty();
}

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  options.automatically_declare_parameters_from_overrides(true);
  auto node = rclcpp::Node::make_shared("pregrasp_demo", options);
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  std::thread spin([&executor] { executor.spin(); });
  const bool valid = checkApproach(node);
  executor.cancel();
  spin.join();
  rclcpp::shutdown();
  return valid ? 0 : 1;
}

这是完整规划校验程序,不包含 execute() 或夹爪关闭。plan() 检查当前状态能否绕障到预抓取位姿;第二段使用第一段预测终点的关节状态,避免误从未移动的真实当前位置计算直线接近。computeCartesianPath(..., 0.01, ..., true) 以约 1 cm 的末端插值步长计算路径并启用碰撞检查。

将生成的 CMakeLists.txt 改为:

cmake
cmake_minimum_required(VERSION 3.8)
project(pregrasp_demo)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(shape_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)

add_executable(pregrasp_demo src/pregrasp_demo.cpp)
ament_target_dependencies(pregrasp_demo
  rclcpp geometry_msgs moveit_msgs shape_msgs tf2 moveit_ros_planning_interface)
install(TARGETS pregrasp_demo DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
ament_package()

保存以下启动文件为 pregrasp_demo/launch/check_approach.launch.py,它为 C++ 节点注入 Panda 的 URDF、SRDF 与运动学参数;直接使用 ros2 run 而不传这些参数,可能找不到 robot_description。

python
from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()
    return LaunchDescription([
        Node(
            package="pregrasp_demo",
            executable="pregrasp_demo",
            output="screen",
            parameters=[
                config.robot_description,
                config.robot_description_semantic,
                config.robot_description_kinematics,
            ],
        ),
    ])

预期结果:启动时能为 MoveGroupInterface 提供机器人模型、语义规划组与 IK 配置。编译通过后仍须保持第一个终端的 move_group 和关节状态发布正常运行。

运行与判断路径是否完整

在第二个终端构建并启动程序:

bash
cd ~/moveit_ws
colcon build --packages-select pregrasp_demo
source install/setup.bash
ros2 launch pregrasp_demo check_approach.launch.py

预期输出(环境具备可行初始状态且两段规划均成功时,省略 MoveIt 自身日志):

text
pre.z=0.47 stop.z=0.35
pregrasp_plan=success
approach_fraction=1.00

前两个 Z 值由几何关系固定;pregrasp_plan 和 approach_fraction 取决于当前关节状态、运动学求解器及规划场景,不保证每次成功。程序返回码为 0 仅表示两段路径在当前模型中通过检查,不代表已经抓住物体,也不代表轨迹经过真实硬件安全认证。

定位不可达与碰撞失败

遇到 pregrasp_plan=fail,先确认 /joint_states 更新时间与 panda_arm 组的运动学配置,再调整预抓取位置或姿态;可尝试将物体横向移动 0.05 m 或把姿态旋转 90° 做对照,不能仅把规划时间无限增大。

如果第一段成功但 approach_fraction 小于 1.00,优先排查直线段经过的桌面、物体及机器人自碰撞;也可能是笛卡尔插值沿途逆解失败或关节突跳被限制。用 RViz 的 Planning Scene 核对 table_demo、object_demo 的尺寸与位置,逐步缩短 0.12 m 接近行程定位失败位置。

只有目标帧已标定、规划场景包含实际障碍、预抓取段成功且接近段比例达到 1.00 时,才能把两段轨迹交给后续的轨迹预览与执行安全检查。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

计算预抓取位姿与接近路径

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通