机器人学习之项目- Project5:KUKA Robot Challenge（一）

1.KUKA（库卡）路径规划项目

在这个项目中，给定一个迷宫的开始和目标状态,实现一个路径规划算法来搜索路径，然后在迷宫中的6自由度工业机器人的帮助下，将物体从起点移动到目标状态。

库卡路径规划项目由库卡机器人和卡尔斯鲁厄理工学院(KIT)机器人学习实验室(RLL)合作推出。KUKA Robotics是一家领先的工业机器人制造商，为KIT实验室提供了真实世界的6自由度工业机械臂。

KIT实验室提供了真实世界的设置以及项目SDK(软件开发工具包)，这是gazebo模拟中物理实验室设置的副本。他们还构建了web界面，我们将使用它来提交代码到硬件，从那里可以查看代码在硬件上执行情况!

项目概述

在这个项目中，通过迷宫导航实现路径规划算法。一般来说，通过迷宫的路径规划问题被简化为两个维度:物体的x和y位置。然而，这个项目将把搜索空间扩展到第三个维度，即围绕垂直z轴的夹持器的方向。

执行周期

在每次运行中，机器人将遵循以下循环:

首先，KUKA夹持器将移动到物体的起始位置，相对于物体定向，抓住物体，并将其举起。

在此阶段，执行路径规划代码：搜索路径，通过命令2D位置和方向角度来引导机器人通过迷宫，以便绕过迷宫中的角落

有8分钟的时间来搜索路径并朝着目标姿势移动。一旦到达，长方体物体将被放置在目标姿势。

最后，机器人将从目标姿势中拾起物体，将其返回到开始姿势，放下它，并为下一次运行做好准备。

项目规范

通常，路径规划算法应用于已知地图。然而，在这个项目中，将不得不几乎盲目地进行搜索，这就产生了一个具有挑战性的问题。以下是对迷宫的了解:

只有它的宽度和长度!

除了迷宫的尺寸(1.2米x 1.6米)，开始和目标状态也会作为算法的输入。通往目标状态的路径就是输出。在启动文件中，将编辑对象的开始姿势和目标姿势。

由于地图是未知的，物体也没有配备任何传感器，你可能想知道……我们如何用机械臂探测障碍物并移动长方体物体?

项目地址： GitHub - KITrobotics/rll_path_planning_project

我们包含了两个可以在python脚本中调用的服务; Move和CheckPath:

• Move: 通过发送2D姿势来命令机器人。机器人将沿着线性路径移动物体到这个位置。移动点应该距离起始点至少5毫米。如果要旋转手臂，那么确保至少旋转0.35弧度。
• CheckPath: 验证两个2D姿态之间的线性路径是否有效。

注意:每次机器人开始迷宫问题时，它应该完全不知道墙在哪里，也不知道迷宫地图是什么样子的!

总而言之，在这个项目中，有两个主要文件要编辑:一个可以指定开始/目标姿势的启动文件，以及一个编写路径规划算法的python脚本。

附：path_planner.py

#! /usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Pose2D
from rll_planning_project_iface.client import RLLPlanningProjectClient


def plan_to_goal(client):
    # type: (RLLPlanningProjectClient) -> bool

    """ Plan a path from Start to Goal """

    rospy.loginfo("Got a planning request")

    # Input: map dimensions, start pose, and goal pose
    # retrieving input values
    map_width = 1.0
    map_length = 1.4
    start_pose, goal_pose = client.get_start_goal()

    # printing input values
    rospy.loginfo("map dimensions: width=%1.2fm, length=%1.2fm",
                  map_width, map_length)
    rospy.loginfo("start pose: x %f, y %f, theta %f",
                  start_pose.x, start_pose.y, start_pose.theta)
    rospy.loginfo("goal pose: x %f, y %f, theta %f",
                  goal_pose.x, goal_pose.y, goal_pose.theta)

    ###############################################
    # Implement your path planning algorithm here #
    ###############################################

    path = []

    # example motions for the gripper
    motions = [
        # movement by 0.1m in positive or negative x-direction
        [0.1, 0, 0], [-0.1, 0, 0],
        # movement by 0.1m in positive or negative y-direction
        [0, 0.1, 0], [0, -0.1, 0],
        # rotation on the spot by 90°, clockwise or counterclockwise
        [0, 0, 1.57], [0, 0, 1.57],
        # rotation by 90° and movement into y or x direction (grinding curves)
        [0, 0.1, 1.57], [0.1, 0, -1.57]]

    ###############################################
    # Example on how to use check_path functionality
    ################################################
    for motion in motions:

        newx = start_pose.x + motion[0]
        newy = start_pose.y + motion[1]
        newtheta = start_pose.theta + motion[2]
        new_pose = Pose2D(newx, newy, newtheta)
        if client.check_path(start_pose, new_pose):
            path.append(new_pose)

    if path is not None:
        rospy.loginfo("A path was found, now trying to execute it")
        for point in path:
            client.move(point)

        return True

    rospy.loginfo("No path to goal found")
    return False


def main():
    rospy.init_node('path_planner')

    # Inititalize planning client with plan_to_goal as function to execute
    # Set verbose=True for more debugging output
    client = RLLPlanningProjectClient(plan_to_goal, verbose=False)
    client.spin()


if __name__ == "__main__":
    main()

该项目合共有4个启动文件，稍后启动这些文件来启动Gazebo和RVIZ模拟，以及所有其他节点: 

• moveit_planning_execution.launch(rll_planning_project package): 启动Gazebo和Rviz中的仿真设置---

• <?xml version="1.0"?>
  <launch>
    <arg name="headless" default="false"/>
    <arg name="gazebo_gui" default="false"/>
    <arg name="use_sim" default="true" />
  
    <!--  This loads the whole Moveit! setup -->
    <include file="$(find rll_moveit_config)/launch/moveit_planning_execution.launch">
      <arg name="description_file" value="$(find rll_planning_project)/urdf/planning_env.urdf.xacro" />
      <arg name="semantic_description_file" value="$(find rll_planning_project)/config/planning_env.srdf.xacro" />
      <arg name="headless" value="$(arg headless)" />
      <arg name="use_sim" value="$(arg use_sim)" />
      <arg name="gazebo_gui" value="$(arg gazebo_gui)" />
      <arg name="rviz_config" value="$(find rll_planning_project)/launch/moveit.rviz" />
    </include>
  
  </launch>

• planning_iface.launch(rll_planning_project package): 启动规划接口---

• <?xml version="1.0"?>
  <launch>
    <arg name="robot" default="iiwa" />
    <arg name="headless" default="false"/>
    <!-- call the path planner three times and take the median as duration -->
    <arg name="run_three_times" default="false"/>
    <arg name="grasp_object_dim_x" default="0.06" />
    <arg name="grasp_object_dim_y" default="0.07" />
    <arg name="grasp_object_dim_z" default="0.04" />
    
    <!-- Start 2D pose  -->
    <arg name="start_pos_x" default="-0.37" />
    <arg name="start_pos_y" default="-0.5" />
    <arg name="start_pos_theta" default="0.0" />
    <!-- Goal 2D pose  -->
    <arg name="goal_pos_x" default="-0.15" />
    <arg name="goal_pos_y" default="0.44" />
    <arg name="goal_pos_theta" default="1.57" />
  
    <remap from="/gazebo/spawn_urdf_model" to="/$(arg robot)/gazebo/spawn_urdf_model" />
  
    <!-- spawn grasp object in Gazebo -->
    <param name="grasp_object_description" command="$(find xacro)/xacro --inorder $(find rll_planning_project)/urdf/grasp_object.urdf.xacro grasp_object_dim_x:=$(arg grasp_object_dim_x) grasp_object_dim_y:=$(arg grasp_object_dim_y) grasp_object_dim_z:=$(arg grasp_object_dim_z)" />
    <!-- TODO: start orientation of the grasp object is not set here -->
    <node name="grasp_object_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -param grasp_object_description -x $(arg start_pos_x) -y $(arg start_pos_y) -model grasp_object" />
  
    <node ns="$(arg robot)" name="planning_iface" pkg="rll_planning_project" type="planning_iface" respawn="false" output="screen">
      <remap from="/use_sim_time" to="/$(arg robot)/use_sim_time" />
      <remap from="/clock" to="/$(arg robot)/clock" />
      <param name="eef_type" value="egl90"/>
      <param name="headless" value="$(arg headless)"/>
      <param name="run_three_times" value="$(arg run_three_times)"/>
      <param name="start_pos_x" value="$(arg start_pos_x)"/>
      <param name="start_pos_y" value="$(arg start_pos_y)"/>
      <param name="start_pos_theta" value="$(arg start_pos_theta)"/>
      <param name="grasp_object_dim_x" value="$(arg grasp_object_dim_x)"/>
      <param name="grasp_object_dim_y" value="$(arg grasp_object_dim_y)"/>
      <param name="grasp_object_dim_z" value="$(arg grasp_object_dim_z)"/>
      <param name="goal_pos_x" value="$(arg goal_pos_x)"/>
      <param name="goal_pos_y" value="$(arg goal_pos_y)"/>
      <param name="goal_pos_theta" value="$(arg goal_pos_theta)"/>
      <param name="collision_link" value="maze" />
    </node>
  
  </launch>

• path_planner.launch(rll_planning_project package): 启动路径规划算法代码---

• <?xml version="1.0"?>
  <launch>
    <arg name="robot" default="iiwa" />
  
    <node ns="$(arg robot)" name="path_planner" pkg="rll_planning_project"
          type="path_planner.py" respawn="false" output="screen"/>
  
  </launch>

• run_project.launch(rll_project_runner package): 启动单个规划和路径执行---

• <?xml version="1.0"?>
  <launch>
    <arg name="robot" default="iiwa" />
  
     <include file="$(find rll_planning_project)/launch/path_planner.launch" >
         <arg name="robot" value="$(arg robot)" />
     </include>
  
     <include file="$(find rll_tools)/launch/run_project.launch" >
       <arg name="robot" value="$(arg robot)" />
     </include>
  
  </launch>

在不同的终端中分别启动这些文件，或者使用提供的shell脚本文件在xterm终端的单独实例中启动所有节点。对于调试来说，第一种方法更可取，因为可以很容易地识别错误。

2.开始/示例运行
首先,导航到项目工作空间。在home/workspace/catkin-ws/src下，将看到项目SDK(rll_sdk)和规划(rll_planning_project)包。主要使用规划包，在其中编辑启动文件以更改对象的开始/目标姿态，并编辑python脚本来编写路径规划算法。

Python脚本:path_planner.py
让我们首先看一下Python路径规划脚本。导航到/home/workspace/catkin_ws/src/rll_planning_project/scripts并打开path_planner.py脚本。在这里，有一个示例解决方案：首先检索输入值，打印它们，调用CheckPath服务，并检查是否有可能直接从开始姿势移动到目标姿势。如果它有效，则调用Move服务将对象线性移动到目标姿态。 

启动文件:planning_iface.launch
接下来，让我们看一下需要修改的启动文件。导航到/home/workspace/catkin_ws/src/rll_planning_project/launch并打开planning_iface.launch。这个项目需要很多参数。现在只需要编辑开始2D姿态和目标2D姿态参数。

运行示例解决方案
现在使用示例解决方案试运行一下项目。

1. 更新/升级系统

   apt-get update -y
   apt-get upgrade -y
   

2. 安装所有软件包依赖项

   cd /home/workspace/catkin_ws/
   rosdep install --from-paths src --ignore-src -r -y
   

3. 构建并引导catkin_ws 

   cd /home/workspace/catkin_ws/
   catkin_make
   source devel/setup.bash
   

4. 启动所有项目节点

   # We’ve combined all the launch files in a `start_project.sh` script. 
   # Note: This script will only run if you are in the visual desktop, don't try to run it inside the workspace terminal!
   cd /home/workspace/catkin_ws/src/rll_planning_project/scripts/
   ./start_project.sh
   

   需要在每次重新启动后重复这些步骤!

停止运行
在主终端中按Enter键立即终止项目执行和所有ROS节点。需要等待至少30秒才能开始新的运行，因为节点将需要一些时间才能完全终止。如果遇到错误，不要担心—只需手动或使用提供的shell脚本文件重新启动节点。
示例回顾

 仔细看示例方案，识别前文提到的4个循环: 

1. 首先，KUKA夹持器将移动到物体的起始位置，相对于物体定向，抓住物体，并将其举起。
2. 在此阶段，执行路径规划代码，搜索路径，并通过命令2D位置和方向角度来引导机器人通过迷宫，以便绕过迷宫中的角落
3. 有8分钟的时间来搜索路径并朝着目标姿势移动。一旦到达，长方体物体将被放置在目标姿势。
4. 最后，机器人将从目标姿势中拾起物体，将其返回到开始姿势，放下它，并为下一次运行做好准备。

Gazebo和Rviz

Gazebo:视觉迷宫(红色)，它是位于德国KIT实验室的真实迷宫的复制品。

Rviz:碰撞迷宫(黄色)，与视觉迷宫相同，但有更厚的墙壁和一些额外的墙壁。在碰撞迷宫内部，仍然可以看到具有较薄墙壁的视觉迷宫。红色块表示对象的目标姿态。

作为一种安全措施，在视觉迷宫中增加了一个公差值，以防止在现实世界中物体或机器人与迷宫墙壁发生碰撞。

3.路径规划
前面，启动了示例解决方案，该解决方案执行了从开始到目标的路径。现在，通过改变对象的开始和结束姿态值来使这个问题更具挑战性。
导航到/home/workspace/catkin_ws/src/rll_planning_project/launch目录，打开planning_iface. launch启动文件，并改变对象的开始和2D姿态。尝试使用不同的开始姿势和目标姿势配置代码。示例如下：

  <?xml version="1.0"?>
<launch>
  <arg name="robot" default="iiwa" />
  <arg name="headless" default="false"/>
  <!-- call the path planner three times and take the median as duration -->
  <arg name="run_three_times" default="false"/>
  <arg name="grasp_object_dim_x" default="0.06" />
  <arg name="grasp_object_dim_y" default="0.07" />
  <arg name="grasp_object_dim_z" default="0.04" />
  
  <!-- Start 2D pose  -->
  <arg name="start_pos_x" default="-0.372" />
  <arg name="start_pos_y" default="0.515" />
  <arg name="start_pos_theta" default="0.00" />
  <!-- Goal 2D pose  -->
  <arg name="goal_pos_x" default="0.0" />
  <arg name="goal_pos_y" default="-0.505" />
  <arg name="goal_pos_theta" default="0.0" />


  <!-- spawn grasp object in Gazebo -->
  <param name="grasp_object_description" command="$(find xacro)/xacro --inorder $(find rll_planning_project)/urdf/grasp_object.urdf.xacro grasp_object_dim_x:=$(arg grasp_object_dim_x) grasp_object_dim_y:=$(arg grasp_object_dim_y) grasp_object_dim_z:=$(arg grasp_object_dim_z)" />
  <!-- TODO: start orientation of the grasp object is not set here -->
  <node name="grasp_object_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -param grasp_object_description -x $(arg start_pos_x) -y $(arg start_pos_y) -model grasp_object" />

  <node ns="$(arg robot)" name="planning_iface" pkg="rll_planning_project" type="planning_iface" respawn="false" output="screen">
    <param name="headless" value="$(arg headless)"/>
    <param name="run_three_times" value="$(arg run_three_times)"/>
    <param name="start_pos_x" value="$(arg start_pos_x)"/>
    <param name="start_pos_y" value="$(arg start_pos_y)"/>
    <param name="start_pos_theta" value="$(arg start_pos_theta)"/>
    <param name="grasp_object_dim_x" value="$(arg grasp_object_dim_x)"/>
    <param name="grasp_object_dim_y" value="$(arg grasp_object_dim_y)"/>
    <param name="grasp_object_dim_z" value="$(arg grasp_object_dim_z)"/>
    <param name="goal_pos_x" value="$(arg goal_pos_x)"/>
    <param name="goal_pos_y" value="$(arg goal_pos_y)"/>
    <param name="goal_pos_theta" value="$(arg goal_pos_theta)"/>
  </node>

</launch>

Contest Maze(竞赛迷宫)试运行结束，现在我们回到Practice Phase Maze 1(练习阶段迷宫1)。
Practice Phase Maze 1开始/目标配置:

  <!-- Start 2D pose  -->
  <arg name="start_pos_x" default="0.38" />
  <arg name="start_pos_y" default="0.0" />
  <arg name="start_pos_theta" default="0.0" />
  <!-- Goal 2D pose  -->
  <arg name="goal_pos_x" default="-0.37" />
  <arg name="goal_pos_y" default="0.5" />
  <arg name="goal_pos_theta" default="0.0" />

（未完待续）