目录
2. move_group_interface_tutorial启动文件介绍
0.前言
在前述章节的基础下,解释本章move_group案例代码的各部分功能,并执行程序,显示机械臂规划运动结果。
1. move_group.launch.py启动文件介绍
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description():
# 创建MoveIt配置对象,指定了机器人描述文件和控制器配置文件
moveit_config = (
MoveItConfigsBuilder("moveit_resources_panda")
.robot_description(file_path="config/panda.urdf.xacro") # 机器人模型描述文件
.trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") # 运动控制器配置
.to_moveit_configs() # 生成MoveIt配置对象
)
# 定义启动move_group节点,负责处理MoveIt的动作服务
run_move_group_node = Node(
package="moveit_ros_move_group", # 包名
executable="move_group", # 节点名
output="screen", # 输出到屏幕
parameters=[moveit_config.to_dict()], # 参数传递MoveIt配置
)
# 定义RViz节点,负责可视化
rviz_config_file = (
get_package_share_directory("moveit2_tutorials") + "/launch/move_group.rviz" # RViz配置文件路径
)
rviz_node = Node(
package="rviz2", # 包名
executable="rviz2", # 节点名
name="rviz2", # 节点名称
output="log", # 输出日志
arguments=["-d", rviz_config_file], # 传递RViz配置文件
parameters=[
moveit_config.robot_description, # 机器人描述
moveit_config.robot_description_semantic, # 语义化机器人描述
moveit_config.robot_description_kinematics, # 运动学参数
moveit_config.planning_pipelines, # 规划管道
moveit_config.joint_limits, # 关节限制
],
)
# 定义静态TF节点,用于发布静态坐标变换
static_tf = Node(
package="tf2_ros", # 包名
executable="static_transform_publisher", # 节点名
name="static_transform_publisher", # 节点名称
output="log", # 输出日志
arguments=["--frame-id", "world", "--child-frame-id", "panda_link0"], # 参数设置坐标变换
)
# 定义机器人状态发布节点
robot_state_publisher = Node(
package="robot_state_publisher", # 包名
executable="robot_state_publisher", # 节点名
name="robot_state_publisher", # 节点名称
output="both", # 同时输出到屏幕和日志
parameters=[moveit_config.robot_description], # 传递机器人描述
)
# 定义ros2_control节点,使用FakeSystem作为硬件接口
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"ros2_controllers.yaml", # 控制器配置文件路径
)
ros2_control_node = Node(
package="controller_manager", # 包名
executable="ros2_control_node", # 节点名
parameters=[ros2_controllers_path], # 传递控制器配置文件
remappings=[
("/controller_manager/robot_description", "/robot_description"), # 话题重映射
],
output="both", # 同时输出到屏幕和日志
)
# 启动控制器
load_controllers = []
for controller in [
"panda_arm_controller", # 控制器名称
"panda_hand_controller", # 控制器名称
"joint_state_broadcaster", # 控制器名称
]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner {}".format(controller)], # 执行控制器启动命令
shell=True, # 使用shell运行命令
output="screen", # 输出到屏幕
)
]
# 返回LaunchDescription对象,包含所有定义的节点和控制器
return LaunchDescription(
[
rviz_node, # RViz可视化节点
static_tf, # 静态TF发布节点
robot_state_publisher, # 机器人状态发布节点
run_move_group_node, # move_group节点
ros2_control_node, # ros2_control节点
]
+ load_controllers # 加载控制器
)
这段代码定义了一个 ROS 2 启动文件,包含了机器人系统的所有必要组件和配置,以支持机器人运动规划和控制。它设置了MoveIt2、可视化工具RViz、机器人状态发布器、静态坐标变换以及控制器管理器。通过这个启动文件,用户可以一次性启动所有相关节点,使机器人系统能够进行运动规划、控制和可视化。
2. move_group_interface_tutorial启动文件介绍
from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description():
# 创建MoveIt配置对象
moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()
# MoveItConfigsBuilder用于构建MoveIt的配置,这里使用了一个名为"moveit_resources_panda"的资源
# 调用to_moveit_configs()方法来生成所需的MoveIt配置对象
# 定义MoveGroupInterface示例节点
move_group_demo = Node(
name="move_group_interface_tutorial", # 节点的名称
package="moveit2_tutorials", # 包名:包含MoveGroupInterface教程的ROS 2包
executable="move_group_interface_tutorial", # 可执行文件名:要运行的节点的名称
output="screen", # 输出类型:将日志输出到屏幕
parameters=[
moveit_config.robot_description, # 机器人描述参数:提供机器人的URDF模型信息
moveit_config.robot_description_semantic, # 语义机器人描述参数:提供机器人的语义信息
moveit_config.robot_description_kinematics, # 运动学描述参数:提供机器人的运动学信息
],
)
# 返回启动描述,包含所有定义的节点
return LaunchDescription([move_group_demo])
# LaunchDescription对象用于描述要启动的所有节点
# 在这个例子中,只包含了一个节点,即MoveGroupInterface节点
用于执行move_group_interface程序。
3.move_group_interface主程序
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/attached_collision_object.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit_visual_tools/moveit_visual_tools.h>
// 所有使用ROS日志记录的源文件都应在文件顶部定义一个文件特定的
// 静态常量rclcpp::Logger,位于最小范围的命名空间中(如果有的话)
static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo");
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);
// 我们为当前状态监视器启动一个单线程执行器,以获取有关机器人状态的信息
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_node);
std::thread([&executor]() { executor.spin(); }).detach();
// 开始教程
//
// 设置
// ^^^^^
//
// MoveIt操作于名为“规划组”的关节集合,并将其存储在一个名为
// ``JointModelGroup``的对象中。在MoveIt中,“规划组”和“关节模型组”
// 这两个术语是可以互换使用的。
static const std::string PLANNING_GROUP = "panda_arm";
// 可以使用规划组的名称轻松设置
// :moveit_codedir:`MoveGroupInterface<moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h>`
// 类来控制和规划该组的动作。
moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
// 我们将使用
// :moveit_codedir:`PlanningSceneInterface<moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h>`
// 类在我们的“虚拟世界”场景中添加和移除碰撞对象
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// 原始指针通常用于引用规划组以提高性能。
const moveit::core::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// 可视化
// ^^^^^^^^^^^^^
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "panda_link0", "move_group_tutorial",
move_group.getRobotModel());
visual_tools.deleteAllMarkers();
/* 远程控制是一个自省工具,允许用户通过按钮和键盘快捷键在RViz中逐步执行高级脚本 */
visual_tools.loadRemoteControl();
// RViz提供了许多类型的标记,在这个演示中,我们将使用文本、圆柱体和球体
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.0;
visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE);
// 批量发布用于减少发送到RViz的消息数量,以便进行大规模可视化
visual_tools.trigger();
// 获取基本信息
// ^^^^^^^^^^^^^^^^^
//
// 我们可以打印机器人的参考框架名称。
RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str());
// 我们还可以打印该组的末端执行器链路名称。
RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str());
// 我们可以获得机器人中的所有组的列表:
RCLCPP_INFO(LOGGER, "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
// 开始演示
// ^^^^^^^^^^^^^^^^^
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
// .. _move_group_interface-planning-to-pose-goal:
//
// 规划到姿势目标
// ^^^^^^^^^^^^^^^^^^^^^^^
// 我们可以为此组规划到末端执行器的期望姿势。
geometry_msgs::msg::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.5;
move_group.setPoseTarget(target_pose1);
// 现在,我们调用规划器来计算计划并进行可视化。
// 注意,我们只是规划,并没有要求move_group
// 实际移动机器人。
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
// 可视化计划
// ^^^^^^^^^^^^^^^^^
// 我们还可以将计划可视化为RViz中的一条线。
RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line");
visual_tools.publishAxisLabeled(target_pose1, "pose1");
visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// 移动到姿势目标
// ^^^^^^^^^^^^^^^^^^^^^
//
// 移动到姿势目标类似于上面的步骤
// 除了现在我们使用``move()``函数。注意
// 我们之前设置的姿势目标仍然是有效的
// 因此,机器人将尝试移动到该目标。我们将
// 在此教程中不使用该函数,因为它是
// 阻塞函数,需要控制器处于活动状态
// 并在执行轨迹时报告成功。
/* 当使用实际机器人时取消注释下面的行 */
/* move_group.move(); */
// 规划到关节空间目标
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
//
// 让我们设置一个关节空间目标并朝着它移动。这将替换
// 我们之前设置的姿势目标。
//
// 首先,我们将创建一个指针来引用当前机器人的状态。
// RobotState是包含所有当前位置/速度/加速度数据的对象。
moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10);
//
// 接下来获取该组的当前关节值集合。
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
// 现在,让我们修改其中一个关节,规划到新的关节空间目标,并可视化计划。
joint_group_positions[0] = -1.0; // 弧度
bool within_bounds = move_group.setJointValueTarget(joint_group_positions);
if (!within_bounds)
{
RCLCPP_WARN(LOGGER, "Target joint position(s) were outside of limits, but we will plan and clamp to the limits ");
}
// 我们将允许的最大速度和加速度降低到最大值的5%。
// 默认值是10%(0.1)。
// 在机器人的moveit_config的joint_limits.yaml文件中设置您的首选默认值
// 或在代码中设置显式因子,如果需要您的机器人移动更快。
move_group.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.05);
success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
// 在RViz中可视化计划:
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// 使用路径约束进行规划
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// 可以轻松地为机器人上的一个链路指定路径约束。
// 首先定义路径约束。
moveit_msgs::msg::OrientationConstraint ocm;
ocm.link_name = "panda_link7"; // 约束应用于链路 "panda_link7"
ocm.header.frame_id = "panda_link0"; // 参考坐标系为 "panda_link0"
ocm.orientation.w = 1.0; // 目标方向的四元数分量
ocm.absolute_x_axis_tolerance = 0.1; // x 轴方向的绝对容差
ocm.absolute_y_axis_tolerance = 0.1; // y 轴方向的绝对容差
ocm.absolute_z_axis_tolerance = 0.1; // z 轴方向的绝对容差
ocm.weight = 1.0; // 约束的权重
// 现在,将其设置为组的路径约束。
moveit_msgs::msg::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);
// 强制在关节空间进行规划
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// 根据规划问题,MoveIt 在关节空间和笛卡尔空间之间进行选择。
// 在 ompl_planning.yaml 文件中设置组参数 “enforce_joint_model_state_space:true”
// 强制在所有计划中使用 “关节空间”。
// 默认情况下,带有方向路径约束的规划请求会在“笛卡尔空间”中进行采样
// 从而使逆运动学作为生成采样器。
// 强制使用“关节空间”时,规划过程将使用拒绝采样来找到有效请求。
// 请注意,这可能会显著增加规划时间。
// 我们将重用之前的目标,并计划到它。
// 注意,这只有在当前状态已经满足路径约束时才有效。
// 因此,我们需要将起始状态设置为新姿势。
moveit::core::RobotState start_state(*move_group.getCurrentState());
geometry_msgs::msg::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;
start_pose2.position.y = -0.05;
start_pose2.position.z = 0.8;
start_state.setFromIK(joint_model_group, start_pose2); // 使用逆运动学设置起始状态
move_group.setStartState(start_state);
// 现在,我们将从刚刚创建的新起始状态中规划到先前的姿势目标。
move_group.setPoseTarget(target_pose1);
// 带有约束的规划可能会很慢,因为每个样本都必须调用逆运动学求解器。
// 让我们将规划时间从默认的 5 秒增加,以确保规划器有足够的时间来成功。
move_group.setPlanningTime(10.0);
success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
// 在 RViz 中可视化计划:
visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(start_pose2, "start");
visual_tools.publishAxisLabeled(target_pose1, "goal");
visual_tools.publishText(text_pose, "Constrained_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// 使用完路径约束后,请务必清除它。
move_group.clearPathConstraints();
// 笛卡尔路径
// ^^^^^^^^^
// 你可以通过指定一系列目标点来直接规划笛卡尔路径。
// 请注意,我们从上述新的起始状态开始。
// 初始姿势(起始状态)不需要添加到路径点列表中,但添加它可以帮助可视化。
std::vector<geometry_msgs::msg::Pose> waypoints;
waypoints.push_back(start_pose2);
geometry_msgs::msg::Pose target_pose3 = start_pose2;
target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3); // 向下
target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3); // 向右
target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3); // 向上和向左
// 我们希望笛卡尔路径以 1 cm 的分辨率插值
// 这就是为什么我们将笛卡尔平移的最大步长指定为 0.01。
// 我们将跳跃阈值指定为 0.0,有效地禁用它。
// 警告 - 禁用跳跃阈值可能会导致冗余关节的大幅度不可预测运动,这可能是一个安全问题
moveit_msgs::msg::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);
// 在 RViz 中可视化计划
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Cartesian_Path", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// 笛卡尔运动通常应该较慢,例如在接近物体时。
// 笛卡尔计划的速度目前无法通过 maxVelocityScalingFactor 设置,而是需要手动调整轨迹时间,如 `这里 <https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4>`_ 中所述。
// 欢迎提交拉取请求。
//
// 你可以像这样执行轨迹。
// /* move_group.execute(trajectory); */
// 向环境中添加对象
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// 首先,让我们规划到另一个简单的目标,没有障碍物阻挡。
move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::msg::Pose another_pose;
another_pose.orientation.w = 0;
another_pose.orientation.x = -1.0;
another_pose.position.x = 0.7;
another_pose.position.y = 0.0;
another_pose.position.z = 0.59;
move_group.setPoseTarget(another_pose);
success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED");
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(another_pose, "goal");
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
// 结果可能如下所示:
//
// .. image:: ./move_group_interface_tutorial_clear_path.gif
// :alt: 动画显示机器人臂相对直接地移动到目标位置
//
// 现在,我们定义一个碰撞对象的 ROS 消息,以便机器人避开。
moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
// 对象的 ID 用于识别它。
collision_object.id = "box1";
// 定义一个盒子以添加到世界中。
shape_msgs::msg::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.1;
primitive.dimensions[primitive.BOX_Y] = 1.5;
primitive.dimensions[primitive.BOX_Z] = 0.5;
// 为盒子定义一个姿势(相对于 frame_id)。
geometry_msgs::msg::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.48;
box_pose.position.y = 0.0;
box_pose.position.z = 0.25;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
// 现在,将碰撞对象添加到世界中
// (使用一个可以包含其他对象的向量)
RCLCPP_INFO(LOGGER, "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);
// 在RViz中显示状态文本,并等待MoveGroup接收和处理碰撞物体消息
visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到碰撞物体出现在RViz中");
// 现在,当我们规划轨迹时,它将避开障碍物。
success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "可视化计划 6(姿态目标绕立方体移动) %s", success ? "" : "失败");
visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到计划完成");
// 结果可能如下所示:
//
// .. image:: ./move_group_interface_tutorial_avoid_path.gif
// :alt: 显示手臂避开新障碍物的动画
//
// 将物体附加到机器人
// ^^^^^^^^^^^^^^^^^^^^
//
// 您可以将一个物体附加到机器人,使其随机器人几何体一起移动。
// 这模拟了拾取物体以便进行操作的过程。
// 运动规划也应该避免物体之间的碰撞。
moveit_msgs::msg::CollisionObject object_to_attach;
object_to_attach.id = "cylinder1";
shape_msgs::msg::SolidPrimitive cylinder_primitive;
cylinder_primitive.type = primitive.CYLINDER;
cylinder_primitive.dimensions.resize(2);
cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20;
cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;
// 我们定义这个圆柱体的坐标系/姿态,使其出现在夹爪中。
object_to_attach.header.frame_id = move_group.getEndEffectorLink();
geometry_msgs::msg::Pose grab_pose;
grab_pose.orientation.w = 1.0;
grab_pose.position.z = 0.2;
// 首先,我们将物体添加到世界中(不使用向量)。
object_to_attach.primitives.push_back(cylinder_primitive);
object_to_attach.primitive_poses.push_back(grab_pose);
object_to_attach.operation = object_to_attach.ADD;
planning_scene_interface.applyCollisionObject(object_to_attach);
// 然后,我们将物体“附加”到机器人上。它使用 frame_id 来确定附加到哪个机器人连杆。
// 我们还需要告诉MoveIt允许物体与夹爪的指尖连杆发生碰撞。
// 您也可以使用 applyAttachedCollisionObject 直接将物体附加到机器人上。
RCLCPP_INFO(LOGGER, "将物体附加到机器人上");
std::vector<std::string> touch_links;
touch_links.push_back("panda_rightfinger");
touch_links.push_back("panda_leftfinger");
move_group.attachObject(object_to_attach.id, "panda_hand", touch_links);
visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* 等待MoveGroup接收和处理附加碰撞物体消息 */
visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到新物体附加到机器人上");
// 重新规划,但现在手中有物体。
move_group.setStartStateToCurrentState();
success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "可视化计划 7(与圆柱体一起移动绕立方体) %s", success ? "" : "失败");
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到计划完成");
// 结果可能如下所示:
//
// .. image:: ./move_group_interface_tutorial_attached_object.gif
// :alt: 显示物体附加后手臂运动的不同动画
//
// 分离和移除物体
// ^^^^^^^^^^^^^^^
//
// 现在,让我们将圆柱体从机器人的夹爪中分离。
RCLCPP_INFO(LOGGER, "将物体从机器人上分离");
move_group.detachObject(object_to_attach.id);
// 在RViz中显示状态文本
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* 等待MoveGroup接收和处理已分离的碰撞物体消息 */
visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到新物体从机器人上分离");
// 现在,让我们从世界中移除物体。
RCLCPP_INFO(LOGGER, "从世界中移除物体");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
object_ids.push_back(object_to_attach.id);
planning_scene_interface.removeCollisionObjects(object_ids);
// 在RViz中显示状态文本
visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
/* 等待MoveGroup接收和处理已移除的碰撞物体消息 */
visual_tools.prompt("在RvizVisualToolsGui窗口中按‘next’,直到碰撞物体消失");
// 教程代码结束
visual_tools.deleteAllMarkers();
visual_tools.trigger();
rclcpp::shutdown();
return 0;
}
4.任务分解并执行
4.1 规划到姿态目标
- 使用
move_group.setPoseTarget(target_pose1)
设置目标姿态。 - 调用
move_group.plan(my_plan)
计算规划。 - 通过 RViz 进行可视化。
4.2 规划到关节空间目标
- 获取当前的机器人状态并修改关节值。
- 使用
move_group.setJointValueTarget(joint_group_positions)
设置关节目标。 - 调整最大速度和加速度。
- 进行规划并在 RViz 中可视化。
4.3 带路径约束的规划
- 定义并设置路径约束(如方向约束)。
- 修改起始状态以满足路径约束。
- 设置新的目标姿态并规划。
- 通过 RViz 进行可视化。
4.4 笛卡尔路径规划
- 指定一系列路径点。
- 使用
move_group.computeCartesianPath()
计算笛卡尔路径。 - 在 RViz 中可视化路径。
4.5 环境中的障碍物
- 先进行没有障碍物的规划。
- 定义并添加碰撞对象到环境中。
- 再次进行规划,并确保机器人避开障碍物。
4.6 将物体附加到机器人
- 定义一个要附加到机器人的物体,并将其添加到世界中。
- 使用
move_group.attachObject()
将物体附加到机器人上。 - 进行新的规划并在 RViz 中可视化。
4.7 从机器人上分离和移除物体
- 使用
move_group.detachObject()
从机器人上分离物体。 - 使用
planning_scene_interface.removeCollisionObjects()
从环境中移除物体。 - 在 RViz 中显示更新状态。
5.总结
本文演示了如何使用MoveIt2在ROS 2中进行机器人运动规划。这包括了如何启动必要的节点,如何设置MoveGroupInterface进行运动规划,以及如何在主程序中实现不同的运动规划策略。