ROS进阶——笛卡尔轨迹规划descartes

一、配置

在kinetic版本无法直接通过apt-get安装descartes，因此直接下载源码到工作空间内编译使用(可去掉descartes_tests)。

git clone https://github.com/ros-industrial-consortium/descartes.git

二、解析

2.1 descartes简介

1. 官方说明

• Cartesian Planning: While MoveIt plans free space motion (i.e. move from A to B), Descartes plans robot joint motion for semi-constrained Cartesian paths (i.e. ones whose waypoints may have less than a fully specified 6DOF pose).
• Efficient, Repeatable, Scale-able Planning: The paths that result from Descartes appear very similar to human generated paths, but without all the effort. The plans are also repeatable and scale-able to the complexity of the problem (easy paths are planned very quickly, hard paths take time but are still solved).
• Dynamic Re-planning: Path planning structures are maintained in memory after planning is complete. When changes are made to the desired path, an updated robot joint trajectory can be generated nearly instantaneously.
• Offline Planning: Similar to MoveIt, but different than other planners, Descartes is primarily focus逆解ed on offline or sense/plan/act applications. Real-time planning is not a feature of Descartes.

2. 使用场景

• 可以设置以任何位置为起点进行规划，使用Moveit!规划的话只能以机械臂当前位姿为起点；
• 对多段轨迹结合规划，当调整其中一段轨迹时，会动态规划而不是全部重新规划；
• 对复杂轨迹的规划效果较Moveit!默认规划器更好；

3. 测试说明

• 规划耗时较Moveit!默认规划器会长很多，大部分超过1s，无法用在实时驱动下；
• 规划时必须设置机械臂初始状态（以关节角度的形式而不是末端位姿），否则会因逆解多解产生规划起点与机械臂起点不一致的问题；
• 规划过程中会偶然出现某个点大幅度偏移规划轨迹（发生概率不高，可通过计算轨迹点间距离验证轨迹的有效性）；
• descartes可用在离线复杂轨迹规划下，完成轨迹的规划和优化后保存成bag或者其他文件，后续工作的时候加载发布即可；

2.2 接口说明

1. descartes_core

接口类	说明
descartes_core::TrajectoryPt	笛卡尔轨迹点
descartes_core::RobotModel	笛卡尔规划机器人模型
descartes_core::PathPlannerBase	笛卡尔规划器

2. descartes_moveit

接口类	说明
descartes_moveit::MoveitStateAdapter	结合descartes和moveit

3. descartes_planner

接口类	说明
descartes_planner::DensePlanner dense_planner	稠密求解器，DensePlanner takes a brute force approach to finding a sequence of joint solutions that defines an “optimum” path through a sequence of input descartes_core::TrajectoryPt’s. Optimum is defined as the path with minimal joint changes. It works by expanding all of the valid inverse kinematic solutions for every trajectory point passed in. Edges are added between every possible pair of solutions from contigous user trajectory points. Dijkstra’s algorithm is then used to find a path with minimal joint motion through the graph.
descartes_planner::SparsePlanner sparse_planner	稀疏规划器，SparsePlanner attempts to optimize the process used by DensePlanner by not expanding every point. Instead a subset of the points are expanded and the planner attempts to use joint interpolation to find a satisfactory solution to the not-expanded points (thereby reducing the number of IK solutions required).

4. descartes_trajectory

接口类	说明
descartes_trajectory::JointTrajectoryPt	Represents a robot joint pose. It can accept tolerances for each joint
descartes_trajectory::CartTrajectoryPt	Defines the position and orientation of the tool relative to a world coordinate frame. It can also apply tolerances for the relevant variables that determine the tool pose
descartes_trajectory::AxialSymmetricP	Extends the CartTrajectoryPt by specifying a free axis of rotation for the tool. Useful whenever the orientation about the tool’s approach vector doesn’t have to be defined

5. descartes_utilities

接口类	说明
descartes_utilities::toRosJointPoints	笛卡尔规划结果转化为标准ROS关节信息

三、应用

3.1 实现流程

3.2 相关函数

• 生成笛卡尔运动轨迹数据

descartes_core::TrajectoryPtPtr makeCartesianPoint(const Eigen::Isometry3d &pose) {
  using namespace descartes_core;
  using namespace descartes_trajectory;
  return TrajectoryPtPtr(new CartTrajectoryPt(TolerancedFrame(pose)));
}

• 转换规划数据到ROS格式

trajectory_msgs::JointTrajectory toROSJointTrajectory(
    const TrajectoryVec &trajectory, const descartes_core::RobotModel &model,
    const std::vector<std::string> &joint_names, double time_delay) {

  // Fill out information about our trajectory
  trajectory_msgs::JointTrajectory result;
  result.header.stamp = ros::Time::now();
  result.header.frame_id = "base_link";
  result.joint_names = joint_names;

  // For keeping track of time-so-far in the trajectory
  double time_offset = 0.0;
  // Loop through the trajectory
  int len = trajectory.size();
  for (int i = 0; i < len; i++) {
    // Find nominal joint solution at this point
    std::vector<double> joints;
    trajectory[i].get()->getNominalJointPose(std::vector<double>(), model,
                                             joints);

    // Fill out a ROS trajectory point
    trajectory_msgs::JointTrajectoryPoint pt;
    pt.positions = joints;
    // velocity, acceleration, and effort are given dummy values
    // we'll let the controller figure them out
    pt.velocities.resize(joints.size(), 0.0);
    pt.accelerations.resize(joints.size(), 0.0);
    pt.effort.resize(joints.size(), 0.0);
    // set the time into the trajectory
    pt.time_from_start = ros::Duration(time_offset);
    // increment time
    time_offset += time_delay;

    result.points.push_back(pt);
  }

  return result;
}

参考

ros-descartes

ros-industrial-consortium-descartes

descartes_node

ROS 教程6 工业机器人实战 UR5机械臂 movieit仿真 轨迹规划 Descartes 笛卡尔 轨迹规划

descartes_tutorials

moveit_cartesian_plan_plugin（基于descartes构建的rviz规划插件）