机器人 / 系统课时
计算预抓取位姿与接近路径
计算预抓取位姿与接近路径
已知物体位姿后,机械臂不能直接冲向夹持点。这里以 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,位置关系为 。由于工具局部 +Z 向下,反向后退会使世界坐标的 Z 增大:预抓取点为 (0.45, 0.00, 0.47) 米。
预抓取点和接近终点保持同一姿态。先自由空间规划到 P,再从 P 沿固定姿态下移 0.12 m 到 G。这里的 G 是安全停靠点而非实际闭爪位置;真正接触物体时还需处理夹爪几何、允许触碰对象和闭爪动作。

使用 MoveIt 规划两段路径
前提是已安装或构建 moveit2_tutorials、Panda 配置和支持运动学求解的 MoveIt 2。先启动演示环境,并确认 /joint_states 在更新。第一个终端执行:
source /opt/ros/jazzy/setup.bash
ros2 launch moveit2_tutorials move_group.launch.py预期结果:move_group 启动,RViz 中可看到 Panda 模型;具体日志时间与输出行数依本机环境而异。另一个终端创建 C++ 包:
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:
#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_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。
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 和关节状态发布正常运行。
运行与判断路径是否完整
在第二个终端构建并启动程序:
cd ~/moveit_ws
colcon build --packages-select pregrasp_demo
source install/setup.bash
ros2 launch pregrasp_demo check_approach.launch.py预期输出(环境具备可行初始状态且两段规划均成功时,省略 MoveIt 自身日志):
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 时,才能把两段轨迹交给后续的轨迹预览与执行安全检查。