< >
Home » ROS2与MoveIt2教程 » ROS2与MoveIt2入门教程-实时机械臂伺服

ROS2与MoveIt2入门教程-实时机械臂伺服

说明：

  • 介绍如何实时机械臂伺服

概述：

MoveIt Servo允许您将末端执行器（EEF）速度指令数据流传输到您的机械臂，并让其同步执行这些速度指令。这可以通过广泛的输入方案实现远程操作，或者让其它自主软件来控制机器人—例如在视觉伺服或闭环位置控制中。

本教程将会说明如何使用MoveIt Servo向支持ROS的机器人发送实时伺服命令。伺服节点的一些不错的功能包括奇异性处理和碰撞检测，这可以防止操作人员破坏机器人。

启动独立伺服节点

MoveIt Servo提供了一个组件节点moveit_servo::ServoServer允许您通过ROS话题发送指令。指令可以来自任何地方，例如操纵杆、键盘或其它控制器。

本演示是为Xbox 1控制器编写的，但可以通过更改 joystick_servo_example.cpp文件轻松修改以使用任何与Joy软件包兼容的控制器。

要运行本演示，请确保已插上您的控制器并且可以被ros2 run joy joy_node命令检测到。这通常会在插入控制器后自动发生。然后用以下命令来启动本演示：

ros2 launch moveit2_tutorials servo_teleop.launch.py

现在您应该可以使用控制器来控制机械臂了，MoveIt Servo会自动避开奇点和碰撞。

  • 不使用控制器

如果您没有控制器，仍然可以使用键盘来尝试本演示。在演示仍在运行的情况下，在一个新终端中运行以下命令：

ros2 run moveit2_tutorials servo_keyboard_input

这样您将能够使用键盘来伺服机器人。使用箭头键以及“.”键和“;”键来发送笛卡尔指令。使用“W”字母键来更改笛卡尔指令的世界坐标系，使用“E”字母键来更改末端执行器的坐标系。使用数字键1-7来发送关节点动指令（使用字母键R来反转方向）。

预期输出

预期输出应该如下面这个视频所示：https://link.zhihu.com/?target=http%3A//moveit2_tutorials.picknik.ai/doc/realtime_servo/realtime_servo_tutorial.html

请注意，该视频中的控制器覆盖仅仅用于演示目的，实际上并没有包含控制器操作过程在内。

使用C++接口

除了将Servo作为其自身的组件来启动之外，也可以通过C++接口将Servo包含在您自己的节点中。在这两种情况下，向机器人发送的指令非常相似，但是对于C++接口而言，需要对Servo进行一些设置。作为对此项设置工作的补偿，您将能够通过其C++ API直接与Servo进行交互。

这个基本的C++接口演示会以预定的方式移动机器人，且可以使用以下命令来启动该演示：

ros2 launch moveit2_tutorials servo_cpp_interface_demo.launch.py

这样就应该会出现一个Rviz窗口，窗口中会有一个Panda机械臂和一个碰撞物体。在切换到笛卡尔运动之前，机械臂会点动关节几秒钟。当机械臂接近碰撞物体时，它会减慢速度并停下来。

完整代码说明
本演示的完整代码可以在此处获取。

  • 设置

首先来声明节点指针和会向Servo发布指令消息的发布者指针：

rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<control_msgs::msg::JointJog>::Share    dPtr joint_cmd_pub_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
size_t count_ = 0;

接着来设置该节点、planning_scene_monitor和碰撞物体：

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.use_intra_process_comms(true);
  node_ = std::make_shared<rclcpp::Node>    ("servo_demo_node", node_options);

休眠暂停4秒钟以等待Rviz启动出现。这在具有单个启动文件的集成演示中是必需的：

rclcpp::sleep_for(std::chrono::seconds(4));

然后创建planning_scene_monitor。我们需要将其传递给Servo的构造函数，且在初始化任何碰撞物体之前应该要先设置它：

auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
node_, "robot_description", tf_buffer, "planning_scene_monitor");

这里要确保planning_scene_monitor正在从关节状态话题进行实时更新：

if (planning_scene_monitor->getPlanningScene())
{
  planning_scene_monitor->startStateMonitor("/joint_states");
  planning_scene_monitor->setPlanningScenePublishingFrequency(25);
  planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, "/moveit_servo/publish_planning_scene");
  planning_scene_monitor->startSceneMonitor();
}
else
{
  RCLCPP_ERROR(LOGGER, "Planning scene not configured");
  return EXIT_FAILURE;
}

这些是会将指令发送到MoveIt Servo的发布者。支持两种指令类型：会直接在关节空间中点动机器人的JointJog消息，以及会使用笛卡尔速度指令移动指定链接的TwistStamped消息。在本演示中点动了末端执行器链接。

joint_cmd_pub_ = node_->create_publisher<control_msgs::msg::JointJog>("servo_demo_node/delta_joint_cmds", 10);
twist_cmd_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>("servo_demo_node/delta_twist_cmds",

接下来会在机械臂移动的路径上创建一个碰撞物体。当伺服机械臂移向该碰撞物体时，机械臂会在碰撞前减速并停下来：

moveit_msgs::msg::CollisionObject     collision_object;
collision_object.header.frame_id = "panda_link0";

collision_object.id = "box";

制作一个方盒并将其放在机械臂移动的路径上：

shape_msgs::msg::SolidPrimitive box;
box.type = box.BOX;
box.dimensions = { 0.1, 0.4, 0.1 };
geometry_msgs::msg::Pose box_pose;
box_pose.position.x = 0.6;
box_pose.position.y = 0.0;
box_pose.position.z = 0.6;

将该方盒添加为一个碰撞物体：

collision_object.primitives.push_back(box);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

创建会发布该碰撞物体的消息：

moveit_msgs::msg::PlanningSceneWorld psw;
psw.collision_objects.push_back(collision_object);
moveit_msgs::msg::PlanningScene ps;
ps.is_diff = true;
ps.world = psw;

将该碰撞物体消息发布到规划场景中：

auto scene_pub = node_->create_publisher<moveit_msgs::msg::PlanningScene>("planning_scene", 10);
scene_pub->publish(ps);
  • 初始化伺服（Servo）

伺服（Servo）需要许多参数来决定其行为。这些可以通过使用makeServoParameters辅助函数自动读取这些参数：

auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_, LOGGER);
if (!servo_parameters)
{
  RCLCPP_FATAL(LOGGER, "Failed to load the servo parameters");
  return EXIT_FAILURE;
}

通过传递节点指针、参数和PSM来初始化Servo C++接口：

auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_parameters, planning_scene_monitor);

可以使用C++接口直接启动Servo。如果使用替代选项ServoServer来启动，则要使用一个ROS服务来启动Servo。在启动之前，MoveIt Servo不会接受任何指令或者移动机器人：

servo->start();
  • 发送指令

本演示将会使用一个简单的ROS计时器来向机器人发送关节指令和twist指令：

rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);

这是用于发布指令的计时器回调函数。C++接口通过ROS内部话题来发送指令，与使用ServoServer来启动Servo完全一样。

void publishCommands()
{

首先会发布100个关节点动指令。Joint_names字段允许您指定要以相应的velocities字段中的速度移动的单个关节。消息中包含最新的时间戳非常重要，否则Servo会认为该指令已过时且不会移动机器人。

if (count_ < 100)
{
  auto msg = std::make_unique<control_msgs::msg::JointJog>();
  msg->header.stamp = node_->now();
  msg->joint_names.push_back("panda_joint1");
  msg->velocities.push_back(0.3);
  joint_cmd_pub_->publish(std::move(msg));
  ++count_;
}

过一会儿之后，就切换到发布twist指令。提供的坐标系就是定义twist的坐标系，而不是会遵循该指令的机器人坐标系。同样地也需要消息中的最新时间戳：

else
  {
    auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
    msg->header.stamp = node_->now();
    msg->header.frame_id = "panda_link0";
    msg->twist.linear.x = 0.3;
    msg->twist.angular.z = 0.5;
    twist_cmd_pub_->publish(std::move(msg));
  }
}

这里使用了多线程执行器，因为Servo具有用于移动机器人和避免碰撞的并发进程：

auto executor =     std::make_unique<rclcpp::executors::MultiThreadedExec    utor>();
executor->add_node(node_);
executor->spin();

伺服概述

后续两个小节会提供有关MoveIt Servo的一些背景信息，以及描述在您的机器人上设置MoveIt Servo的首要步骤。

Servo包括以下多个不错的功能：

  • 笛卡尔末端执行器twist指令；

  • 关节指令；

  • 碰撞检测；

  • 奇异性检测；

  • 关节位置和速度限制强制执行；

  • 输入是通用的ROS消息。

在新机器人上进行Servo设置

  • 前提条件

让您的机器人运行 MoveIt Servo的最低要求包括：

  • 机器人的有效URDF和SRDF；

  • 一个可以接受来自ROS话题的关节位置或速度指令的控制器；

  • 能提供快速准确的关节位置反馈的关节编码器。

由于运动学是由MoveIt核心部分进行处理的，因此建议为您的机器人准备一个有效的配置软件包，并且可以运行其中包含的演示启动文件。

  • 输入设备

    MoveIt Servo的两个主要输入就是笛卡尔指令和关节指令。它们分别以TwistStamped和JointJog消息传入Servo。这些指令的输入来源几乎可以是以下任何设备：游戏手柄、语音指令、SpaceNav鼠标，或者PID控制器（例如用于视觉伺服的控制器）。

无论输入设备如何，对传入的指令消息要求如下：

  • TwistStamped和JointJog：发布该消息时，需要在消息标头中有该消息的最新时间戳；

  • JointJog：必须在joint_names字段中具有与displacements或velocities字段中给出的指令相对应的有效关节名称；

  • （可选项）TwistStamped：可以在该消息的标头中提供任意的frame_id，该坐标系将会应用于该twist。如果为空，则会使用配置中的默认值。

Servo配置

这个演示配置文件显示了MoveIt Servo需要的参数，并且有详细的说明文档。

可以从该演示文件中开始进行参数设置，但必须针对您的具体设置更改其中一些参数：

  • robot_link_command_frame：将其更新为您机器人中的有效坐标系，推荐设置为规划坐标系或EEF坐标系；

  • command_in_type：如果您的输入来自操纵杆，则设置为“unitless”，如果您的输入是以米/秒或弧度/秒为单位，则设置为“speed_units”；

  • command_out_topic：将此参数值更改成您控制器的输入话题；

  • command_out_type：根据您控制器需要的消息类型更改此参数；

  • publish_joint_positions和publish_joint_velocities：根据您控制器的需要更改这两个参数的值。注意，如果command_out_type == std_msgs/Float64MultiArray，则这两个参数中只能有一个为True；

  • joint_topic：将此参数值更改为您机械臂的joint_state话题，通常是/joint_states；

  • move_group_name：将此参数值更改为您移动组的名称，正如在您的SRDF中所定义的移动组；

  • planning_frame：此参数值应该设置为您移动组的规划坐标系。

英语原文地址：https://moveit2_tutorials.picknik.ai/doc/realtime_servo/realtime_servo_tutorial.html

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2与moveit2入门教程