ROS Industrial教程(七)_笛卡尔规划和执行1

本文主要是介绍ROS Industrial教程(七)_笛卡尔规划和执行1，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

笛卡尔规划和执行

引言

目标

接下来将介绍如何使用笛卡尔库中的各种组件来从半约束点轨迹规划和执行机器人路径。

包括：

·熟悉笛卡尔工作流程。

·了解如何加载自定义的笛卡尔RobotModel。

·了解如何从6DOF（Six degrees of freedom六自由度）工具姿势创建半约束轨迹。

·使用笛卡尔规划器规划机器人路径。

·将笛卡尔路径转换为MoveIt！消息以执行运动。

·在机器人上执行路径。

 

应用架构

接下来将配置练习完成过程中使用的所有软件包和文件。

 

获取并初始化工作区

cd ~/industrial_training/exercises/Descartes_Planning_and_Execution

cp -r template_ws ~/descartes_ws

cd ~/descartes_ws

source /opt/ros/melodic/setup.bash

catkin init

 

下载源依赖项

使用wstool命令下载src/.rosinstall文件中列出的存储库。

cd ~/descartes_ws/src/

wstool update

 

下载Debian依赖项

确保已安装并配置了rosdep工具。 然后，从工作区的src目录运行以下命令。

rosdep install --from-paths . --ignore-src -y

 

构建工作区

catkin build --cmake-args -G 'CodeBlocks - Unix Makefiles'

如果构建失败，请重新访问前面的两个步骤以确保下载了所有依赖项。

 

Source工作区

source ~/descartes_ws/devel/setup.bash

 

列出应用程序中的所有软件包

cd ~/descartes_ws/src

ls -la

plan_and_run：包含plan_and_run应用程序的源代码。 将通过编辑此程序包中的源文件来完成练习

ur5_demo_moveit_config：包含用于通过Moveit计划和执行机器人运动的支持文件。 该软件包是由Moveit设置助手生成的

ur5_demo_support：提供机器人定义为URDF文件。 该URDF在运行时由plan_and_run应用程序加载。

ur5_demo_descartes：为UR5手臂提供定制的笛卡尔机器人模型。 它使用逆运动学封闭形式解决方案； 这比MoveitStateAdapter使用的数值方法要快得多。

 

plan_and_run软件包

roscd plan_and_run

ls -la

src：应用程序源文件。

src/demo_application.cpp：包含应用程序实现代码的类源文件。

src/plan_and_run.cpp：应用程序主访问点。 它调用应用程序中的所有任务，并将它们包装在“ main”例程中。

src/tasks：一个目录，其中包含您在练习中取得进展时将要编辑或完成的所有源文件。

include：头文件

include/plan_and_run/demo_application.h：定义应用程序框架，并提供许多全局变量，以便在练习中的各个点传递数据。

launch：启动运行应用程序所需的文件

launch/demo_setup.launch：加载roscore，moveit和应用程序所需的运行时资源。

launch/demo_run.launch：作为ROS节点启动我们的应用程序主要可执行文件。

config：包含非关键配置文件的目录。

 

主应用程序源文件

在“ plan_and_run/src/plan_and_run_node.cpp”中,有如下代码：

int main(int argc,char** argv)

{

  ros::init(argc,argv,"plan_and_run");

  ros::AsyncSpinner spinner(2);

  spinner.start();

 

  // creating application

  plan_and_run::DemoApplication application;

 

  // loading parameters

  application.loadParameters();

 

  // initializing ros components

  application.initRos();

 

  // initializing descartes

  application.initDescartes();

 

  // moving to home position

  application.moveHome();

 

  // generating trajectory

  plan_and_run::DescartesTrajectory traj;

  application.generateTrajectory(traj);

 

 

  // planning robot path

  plan_and_run::DescartesTrajectory output_path;

  application.planPath(traj,output_path);

 

  // running robot path

  application.runPath(output_path);

 

  // exiting ros node

  spinner.stop();

 

  return 0;

}

简而言之，该程序将通过从application对象调用相应的函数来运行每个练习。 例如，为了初始化笛卡尔，程序调用application.initDescartes()。 因此，每个练习都包括编辑执行该练习的源文件，因此对于application.initDescartes()，您将编辑plan_and_run/src/tasks/init_descartes.src源文件。

 

DemoApplication类

在头文件“plan_and_run/include/plan_and_run/demo_application.h”中，您将找到应用程序主类的定义以及一些支持结构。 需要注意的一些重要组件如下：

       ·程序变量：包含在应用程序的各个位置使用的固定值。

const std::string ROBOT_DESCRIPTION_PARAM = "robot_description";

const std::string EXECUTE_TRAJECTORY_ACTION = "execute_trajectory";

const std::string VISUALIZE_TRAJECTORY_TOPIC = "visualize_trajectory_curve";

const double SERVER_TIMEOUT = 5.0f; // seconds

const double ORIENTATION_INCREMENT = 0.5f;

const double EPSILON = 0.0001f;

const double AXIS_LINE_LENGHT = 0.01;

const double AXIS_LINE_WIDTH = 0.001;

const std::string PLANNER_ID = "RRTConnectkConfigDefault";

const std::string HOME_POSITION_NAME = "home";

·轨迹类型：方便表示一系列笛卡尔轨迹点的类型。

typedef std::vector<descartes_core::TrajectoryPtPtr> DescartesTrajectory;

       ·DemoConfiguration数据结构：提供变量，其值在运行时根据相应的ros参数进行初始化。

struct DemoConfiguration

{

  std::string group_name;                 /* Name of the manipulation group containing the relevant links in the robot */

  std::string tip_link;                   /* Usually the last link in the kinematic chain of the robot */

  std::string base_link;                  /* The name of the base link of the robot */

  std::string world_frame;                /* The name of the world link in the URDF file */

  std::vector<std::string> joint_names;   /* A list with the names of the mobile joints in the robot */

 

 

  /* Trajectory Generation Members:

   *  Used to control the attributes (points, shape, size, etc) of the robot trajectory.

   *  */

  double time_delay;              /* Time step between consecutive points in the robot path */

  double foci_distance;           /* Controls the size of the curve */

  double radius;                  /* Controls the radius of the sphere on which the curve is projected */

  int num_points;                 /* Number of points per curve */

  int num_lemniscates;            /* Number of curves*/

  std::vector<double> center;     /* Location of the center of all the lemniscate curves */

  std::vector<double> seed_pose;  /* Joint values close to the desired start of the robot path */

 

  /*

   * Visualization Members

   * Used to control the attributes of the visualization artifacts

   */

  double min_point_distance;      /* Minimum distance between consecutive trajectory points. */

};

 

·DemoApplication类：应用程序的主要组件，为我们程序的每个步骤提供功能。 它还包含将此应用程序转换为ROS节点的几种构造。

class DemoApplication

{

public:

  /*  Constructor

   *    Creates an instance of the application class

   */

  DemoApplication();

  virtual ~DemoApplication();

 

  /* Main Application Functions

   *  Functions that allow carrying out the various steps needed to run a

   *  plan and run application.  All these functions will be invoked from within

   *  the main routine.

   */

 

  void loadParameters();

  void initRos();

  void initDescartes();

  void moveHome();

  void generateTrajectory(DescartesTrajectory& traj);

  void planPath(DescartesTrajectory& input_traj,DescartesTrajectory& output_path);

  void runPath(const DescartesTrajectory& path);

 

protected:

 

  /* Support methods

   *  Called from within the main application functions in order to perform convenient tasks.

   */

 

  static bool createLemniscateCurve(double foci_distance, double sphere_radius,

                                    int num_points, int num_lemniscates,

                                    const Eigen::Vector3d& sphere_center,

                                    EigenSTL::vector_Affine3d& poses);

 

  void fromDescartesToMoveitTrajectory(const DescartesTrajectory& in_traj,

                                              trajectory_msgs::JointTrajectory& out_traj);

 

  void publishPosesMarkers(const EigenSTL::vector_Affine3d& poses);

 

 

protected:

 

  /* Application Data

   *  Holds the data used by the various functions in the application.

   */

  DemoConfiguration config_;

 

 

 

  /* Application ROS Constructs

   *  Components needed to successfully run a ros-node and perform other important

   *  ros-related tasks

   */

  ros::NodeHandle nh_;                        /* Object used for creating and managing ros application resources*/

  ros::Publisher marker_publisher_;           /* Publishes visualization message to Rviz */

  std::shared_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction>>   moveit_run_path_client_ptr_; /* Sends a robot trajectory to moveit for execution */

 

 

 

  /* Application Descartes Constructs

   *  Components accessing the path planning capabilities in the Descartes library

   */

  descartes_core::RobotModelPtr robot_model_ptr_; /* Performs tasks specific to the Robot

                                                     such IK, FK and collision detection*/

  descartes_planner::SparsePlanner planner_;      /* Plans a smooth robot path given a trajectory of points */

 

};

 

应用启动文件

该文件将应用程序作为ROS节点启动，并将必要的参数加载到ROS参数服务器中。 通过打开“ plan_and_run/launch/demo_run.launch”文件来观察此操作：

<launch>

  <node name="plan_and_run_node" type="plan_and_run_node" pkg="plan_and_run" output="screen">

    <param name="group_name" value="manipulator"/>

    <param name="tip_link" value="tool"/>

    <param name="base_link" value="base_link"/>

    <param name="world_frame" value="world"/>

    <param name="trajectory/time_delay" value="0.1"/>

    <param name="trajectory/foci_distance" value="0.07"/>

    <param name="trajectory/radius" value="0.08"/>

    <param name="trajectory/num_points" value="200"/>

    <param name="trajectory/num_lemniscates" value="4"/>

    <rosparam param="trajectory/center">[0.36, 0.2, 0.1]</rosparam>

    <rosparam param="trajectory/seed_pose">[0.0, -1.03, 1.57 , -0.21, 0.0, 0.0]</rosparam>

    <param name="visualization/min_point_distance" value="0.02"/>

  </node>

</launch>

一些重要的参数解释如下：

·group_name：一个命名空间，指向机械臂的运动链中包含的机器人链接列表（从基础到工具末端的链接base to end-of-tooling links）。该列表在ur5_demo_moveit_config包中定义。

·tip_link：运动链中最后一个链接的名称，通常是工具链接。

·base_link：机械手基座链接的名称。

·world_frame：规划环境中定义的所有对象的绝对坐标参考系。

·“trajectory”命名空间下的参数用于生成馈入笛卡尔计划器的轨迹。

·trajectory / seed_pose：这点特别重要，因为在规划路径时，它用于指示机器人的首选开始和结束关节配置。如果未指定“ seed_pose”，则计划将需要更长的时间，因为必须考虑多个起点和终点关节配置，从而导致通过组合多个起点和终点姿势而产生多路径解决方案。

 

 

一般说明

接下来，将展示如何在练习中逐步进行演示。 此外，还将展示如何在仿真模式和真实机器人上运行系统。

 

主要目标

通常，您将逐步实施plan_and_run节点。 这意味着在每个练习中，您将添加完成完整的应用程序演示所需的各个部分。 因此，完成练习后，请在模拟模式下运行演示以验证您的结果。 只有完成所有练习后，才可以在真实的机器人上运行它。

 

完成练习

要完成练习，请打开src/plan_and_run/src/tasks/目录下的相应源文件。 例如，在练习1中，需打开load_parameters.cpp。

花一点时间阅读标题评论，以获取有关如何完成此特定练习的特定说明。 例如，load_parameters.cpp文件包含以下说明和提示：

/* LOAD PARAMETERS

  Goal:

    - Load missing application parameters into the node from the ros parameter server.

    - Use a private NodeHandle in order to load parameters defined in the node's namespace.

  Hints:

    - Look at how the 'config_' structure is used to save the parameters.

    - A private NodeHandle can be created by passing the "~" string to its constructor.

*/

不要忘了注释掉这一行：

ROS_ERROR_STREAM("Task '"<<__FUNCTION__ <<"' is incomplete. Exiting"); exit(-1);

该行通常位于每个功能的开头。 省略此步骤将导致程序在到达此点时立即退出。

当遇到以/* Fill Code:开头的注释块时，这意味着后面的一行（或几行）代码是错误的，注释掉或完成代码。 阅读Fill Code后面的说明，并按照说明完成该任务。 指令注释块的示例如下：

  /*  Fill Code:

   * Goal:

   * - Create a private handle by passing the "~" string to its constructor

   * Hint:

   * - Replace the string in ph below with "~" to make it a private node.

   */

[ COMPLETE HERE ]条目意在由适当的code entry代替。 正确的代码条目（code entries）可以是程序变量，字符串或数字常量。 下面是一个示例：

ros::NodeHandle ph("[ COMPLETE HERE ]: ?? ");

在这种情况下，正确的替换将是字符串“〜”，因此此行应如下所示：

ros::NodeHandle ph("~");

 

 

在完成本练习中的每个任务时，可以运行演示（请参阅以下部分）以验证它是否已正确完成。

在仿真模式下运行演示

在终端中，运行设置程序启动文件，如下所示：

roslaunch plan_and_run demo_setup.launch

虚拟机器人准备就绪后，Rviz应该已启动并正在运行，并且UR5臂处于原始位置，可在终端中看到以下消息：

      .

      .

      .

********************************************************

* MoveGroup using:

*     - CartesianPathService

*     - ExecutePathService

*     - KinematicsService

*     - MoveAction

*     - PickPlaceAction

*     - MotionPlanService

*     - QueryPlannersService

*     - StateValidationService

*     - GetPlanningSceneService

*     - ExecutePathService

********************************************************

 

[ INFO] [1430359645.694537917]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner

[ INFO] [1430359645.694700640]: MoveGroup context initialization complete

 

All is well! Everyone is happy! You can start planning now!

该设置启动文件只需要运行一次。

在另一个终端中，运行应用程序启动文件：

roslaunch plan_and_run demo_run.launch

               在Rviz窗口中查看，机械臂应开始移动。

 

在真实机器人上运行演示

注：确保可以ping通机器人，并且附近没有任何障碍物。

在终端中，运行安装程序启动文件，如下所示：

roslaunch plan_and_run demo_setup.launch sim:=false robot_ip:=000.000.0.00

 

注：在robot_ip参数中输入机器人的实际IP地址。 Rviz中的机器人模型应与真实机器人处于大致相同的位置。

在另一个终端中，运行应用程序启动文件：

roslaunch plan_and_run demo_run.launch

此时，真正的机器人应该开始运动。

 

 

这篇关于ROS Industrial教程(七)_笛卡尔规划和执行1的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---