机器人学习之项目- 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 RLLPlanningProjectClientdef 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 valuesmap_width = 1.0map_length = 1.4start_pose, goal_pose = client.get_start_goal()# printing input valuesrospy.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 grippermotions = [# 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 Truerospy.loginfo("No path to goal found")return Falsedef main():rospy.init_node('path_planner')# Inititalize planning client with plan_to_goal as function to execute# Set verbose=True for more debugging outputclient = 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" />

（未完待续）