环境：Ubuntu16.04+ROS Kinetic

一、简介

STOMP（随机轨迹优化运动规划算法）是基于PI^2算法的进行优化的运动规划器。它可以完成机器人手臂运动轨迹的平滑规划，同时避开障碍物和优化约束。 该算法不需要梯度，因此可以优化成本函数中的任意项，如电机工作。

注意：STOMP只能用于机械臂关节空间的路径规划，不能用于空间末端运动轨迹的规划。

二、配置

1、编译industrial_moveit源码

cd ~/catkin_ws/src
git clone https://github.com/ros-industrial/industrial_moveit.git

删除下面文件

industrial_collision_detection
constrained_ik
industrial_moveit_benchmarking

2、在config包的launch目录下新建文件stomp_planning_pipeline.launch.xml，内容如下，<robot_moveit_config>需改为config包名字。

<launch>
  <!-- Stomp Plugin for MoveIt! -->
  <arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
 
  <!-- The request adapters (plugins) ORDER MATTERS -->
  <arg name="planning_adapters" value="default_planner_request_adapters/FixWorkspaceBounds
                                      default_planner_request_adapters/FixStartStateBounds
                                      default_planner_request_adapters/FixStartStateCollision
                                      default_planner_request_adapters/FixStartStatePathConstraints" />
  <arg name="start_state_max_bounds_error" value="0.1" />
  <param name="planning_plugin" value="$(arg planning_plugin)" />
  <param name="request_adapters" value="$(arg planning_adapters)" />
  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
  <rosparam command="load" file="$(find <robot_moveit_config>)/config/stomp_planning.yaml"/>
</launch>

3、修改move_group.launch文件

<!-- move_group settings -->
<arg name="allow_trajectory_execution" default="true"/>
<arg name="fake_execution" default="false"/>
<arg name="max_safe_path_cost" default="1"/>
<arg name="jiggle_fraction" default="0.05" />
<arg name="publish_monitored_planning_scene" default="true"/>
 
<!-- Planning Functionality -->
<include ns="move_group" file="$(find myworkcell_moveit_config)/launch/planning_pipeline.launch.xml">
  <arg name="pipeline" value="stomp" />
</include>

4、在config包的config目录下新建文件stomp_planning.yaml，内容如下，groupname为规划组名字，相关参数需根据机械臂进行修改。

stomp/groupname:
  group_name: groupname
  optimization:
    num_timesteps: 60
    num_iterations: 40
    num_iterations_after_valid: 0    
    num_rollouts: 30
    max_rollouts: 30 
    initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST
    control_cost_weight: 0.0
  task:
    noise_generator:
      - class: stomp_moveit/NormalDistributionSampling
        stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4, 0.4]
    cost_functions:
      - class: stomp_moveit/CollisionCheck
        collision_penalty: 1.0
        cost_weight: 1.0
        kernel_window_percentage: 0.2
        longest_valid_joint_move: 0.05 
    noisy_filters:
      - class: stomp_moveit/JointLimits
        lock_start: True
        lock_goal: True
      - class: stomp_moveit/MultiTrajectoryVisualization
        line_width: 0.02
        rgb: [255, 255, 0]
        marker_array_topic: stomp_trajectories
        marker_namespace: noisy
    update_filters:
      - class: stomp_moveit/PolynomialSmoother
        poly_order: 6
      - class: stomp_moveit/TrajectoryVisualization
        line_width: 0.05
        rgb: [0, 191, 255]
        error_rgb: [255, 0, 0]
        publish_intermediate: True
        marker_topic: stomp_trajectory
        marker_namespace: optimized 

部分参数说明

Optimization Parameters:（优化设置）

  num_timesteps: the number of timesteps the optimizer can take to find a solution before terminating.
（求解）找到解的最大迭代次数
  num_iterations: this is the number of iterations that the planner can take to find a good solution while optimization.
（优化）找到最优解的最大迭代次数
  num_iterations_after_valid: maximum iterations to be performed after a valid path has been found.
  找到有效路径后要执行的最大迭代次数
  num_rollouts: this is the number of noisy trajectories.
  噪声轨迹数量
  max_rollouts: the combined number of new and old rollouts during each iteration should not exceed this value.
  每次迭代过程中新旧推出噪声轨迹数量的总数的最大值
  initialization method: this is the initialization method chosen to select the means to initialize the trajectory.
  初始化轨迹的初始化方法（1LINEAR_INTERPOLATION,2CUBIC_POLYNOMIAL,3MININUM_CONTROL_COST）
  control_cost_weight: this is the percentage of the trajectory accelerations cost to be applied in the total cost calculation.
  在总成本计算中应用的轨迹加速成本的百分比

  Noise Generator Parameters:（噪声设置）

  class: this can be set to “NormalDistributionSampling” (default) or “GoalGuidedMultivariateGaussian”. Depending on what class is used specific parameters need to be set. Have a look at this link for setting parameters if using the “GoalGuidedMultivariateGaussian”.

  可以设置为“NormalDistributionSampling”（默认）或“GoalGuidedMultivariateGaussian”。
根据使用的类，需要设置特定参数。如果使用“GoalGuidedMultivariateGaussian”，请参考看参数设置例子设置参数。

  stddev: this is the degree of noise that can be applied to the joints. Each value in this array is the amplitude of the noise applied to the joint at that position in the array. For instace, the leftmost value in the array will be the value used to set the noise of the first joint of the robot (panda_joint1 in our case). The dimensionality of this array should be equal to the number of joints in the planning group name. Larger “stddev” values correspond to larger motions of the joints.

  用于描述不同关节的噪声程度的矩阵。该阵列中的每个值是施加到阵列中该位置处的关节的噪声的幅度。数组中最左边的值用于设置机器人第一个关节的噪声的值。此数组的维度应等于规划组名称中的关节数。较大的“stddev”值对应于关节的较大运动。

Cost Function Parameters:（成本设置）

  class: here you can set the cost function you want to use. You could set this to “CollisionCheck”, “ObstacleDistanceGradient” or “ToolGoalPose”. Depending on what you put here, you need to set the appropriate cost function class’s parameters: For “CollisionCheck”, you need to set the parameters (collision_penalty, cost_weight, kernel_window_percentage, longest_valid_joint_nove); for “ObstacleDistanceGradient”, you should set the parameters (cost_weight, max_distance, longest_valid_joint_move) and for “ToolGoalPose”, you should set the parameters (constrained_dofs, position_error_range, orientation_error_range, position_cost_weight, orientation_cost_weight). Have a look at this link for setting parameters for “ToolGoalPose” class.

  成本函数。设置可以为“CollisionCheck”，“ObstacleDistanceGradient”或“ToolGoalPose”。
  根据选择的内容，需要设置相应的cost函数类的参数：

  “CollisionCheck”：设置参数（collision_penalty，cost_weight，kernel_window_percentage，longest_valid_joint_nove）;
  “ObstacleDistanceGradient”：设置参数（cost_weight，max_distance，longest_valid_joint_move），
  “ToolGoalPose”：设置参数（constrained_dofs，position_error_range，orientation_error_range，position_cost_weight，orientation_cost_weight）。
   collision_penalty: this is the value assigned to a collision state.

   分配给碰撞的权重。
   cost_weight: unused parameter.
   kernel_window_percentage: the multiplicative factor used to compute the window_size for doing kernel smoothing.
   用于计算window_size以进行内核平滑的乘法因子
   longest_valid_joint_move: this parameter indicates how far can a joint move in between consecutive trajectory points.
   此参数代表关节在连续轨迹点之间移动的距离

Update Filter parameters:

class: this can be set to “PolynomialSmoother” or “ConstrainedCartesianGoal”. Specific paramters need to be set depending on the chosen class. For setting parameters for “ConstrainedCartesianGoal”, have a look at this link.
poly_order: this is the order of the polynomial function used for smoothing trajectories.

参数设置例子

 
/**
@page stomp_plugins_examples STOMP Plugins Examples
@tableofcontents
@section stomp_plugins STOMP Plugins
@subsection cost_functions_plugins Cost Function Plugins
  - @ref tool_goal_pose_example
 
@subsection noise_generators Noise Generator Plugins
  - @ref goal_guided_mult_gaussian_example
 
@subsection constrained_cart_goal Update Filter Plugins
  - @ref constrained_cart_goal_example
 
*/
 
/**
@page tool_goal_pose_example Tool Goal Pose 
Evaluates the cost of the goal pose by determining how far it is from the underconstrained task manifold.  The parameters
are as follow:
@code
  cost_functions:
    - class: stomp_moveit/ToolGoalPose
      constrained_dofs: [1, 1, 1, 1, 1, 0]
      position_error_range: [0.01, 0.1]     #[min, max]
      orientation_error_range: [0.01, 0.1]  #[min, max]
      position_cost_weight: 0.5
      orientation_cost_weight: 0.5
@endcode
  - class:                    The class name.
  - constrained_dofs:         Indicates which cartesians DOF are fully constrained (1) or unconstrained (0).  This vector is of the form
                              [x y z rx ry rz] where each entry can only take a value of 0 or 1.
  - position_error_range:     Used in scaling the position error from 0 to 1.  Any error less than this range is set to 
                              a cost of 0 and errors above this range are set to 1.
  - orientation_error_range:  Used in scaling the orientation error from 0 to 1.  Any error less than this range is set to 
                              a cost of 0 and errors above this range are set to 1.
  - position_cost_weight:     Factor applied to the position error cost. The total cost = pos_cost * pos_weight + orient_cost * orient_weight
  - orientation_cost_weight:  Factor applied to the orientation error cost.  The total cost = pos_cost * pos_weight + orient_cost * orient_weight
*/
 
/**
@page goal_guided_mult_gaussian_example Goal Guided Multivariate Gaussian
Generates noise that is applied onto the trajectory while keeping the goal pose within the task manifold.  The parameters are 
as follows:
@code
  noise_generator:
    - class: stomp_moveit/GoalGuidedMultivariateGaussian
      stddev: [0.1, 0.05, 0.1, 0.05, 0.05, 0.05, 0.05] 
      goal_stddev: [0.0, 0.0, 0.0, 0.0, 0.0, 2.0] 
      constrained_dofs: [1, 1, 1, 1, 1, 0]
@endcode
  - class:            The class name.
  - stddev:           The amplitude of the noise applied onto each joint.
  - goal_stddev:      The amplitude of the noise applied onto each cartesian DOF at the goal.  The array has the following
                      form [px, py, pz, rx, ry, rz].
  - constrained_dofs: Indicates which cartesians DOF are fully constrained (1) or unconstrained (0).  This vector is of the form
                      [x y z rx ry rz] where each entry can only take a value of 0 or 1.
*/
 
/**
@page constrained_cart_goal_example Constrained Cartesian Goal
Modifies the trajectory update such that the goal of the updated trajectory is within the task manifold
@code
  update_filters:
      - class: stomp_moveit/ConstrainedCartesianGoal
        constrained_dofs: [1, 1, 1, 1, 1, 0]
        cartesian_convergence: [0.005, 0.005, 0.005, 0.01, 0.01, 1.00]
        joint_update_rates: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
        max_ik_iterations: 100
@endcode
  - class:                  The class name.
  - constrained_dofs:       Indicates which cartesians DOF are fully constrained (1) or unconstrained (0).  This vector is of the form
                            [x y z rx ry rz] where each entry can only take a value of 0 or 1.
  - cartesian_convergence:  A vector of convergence values for each cartesian DOF [vx vy vz wx wy wz].
  - joint_update_rates:     The rates at which to update the joints during numerical ik computations.
  - max_ik_iterations:      Limit on the number of iterations allowed for numerical ik computations.
*/

三、简单应用

因运动学规划算法STOMP只能用于关节空间的运动规划，因此在使用时需先求取目标点的逆解，然后再通过STOMP进行初始状态到目标状态之间的运动规划。

      group.setStartState(*group.getCurrentState());
      // joint space goal
      robot_model_loader::RobotModelLoader robot_model_loader(
          "robot_description");
      robot_model::RobotModelPtr robot_model_ptr =
          robot_model_loader.getModel();
      robot_state::RobotStatePtr robot_state_ptr(
          group.getCurrentState()); // copy state
      const robot_state::JointModelGroup *joint_model_group =
          robot_model_ptr->getJointModelGroup(group.getName());
      bool found_ik =
          robot_state_ptr->setFromIK(joint_model_group, target_pose, 10, 1);
      if (found_ik) {
        std::vector<double> jointValues;
        robot_state_ptr->copyJointGroupPositions("hand", jointValues);
        group.setJointValueTarget(jointValues);
        moveit::planning_interface::MoveGroupInterface::Plan plan;
        success = group.plan(plan);
        ROS_INFO("Visualizing plan %s",
                 success == moveit_msgs::MoveItErrorCodes::SUCCESS ? "SUCCESS"
                                                                   : "FAILED");
      } else
        ROS_INFO("setFromIK: FAILED");

参考

http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/stomp_planner/stomp_planner_tutorial.html

http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/stomp_planner/stomp_planner_tutorial.html

https://github.com/ros-industrial/industrial_moveit/issues/70

https://groups.google.com/forum/#!topic/swri-ros-pkg-dev/sNvFmkQsMtg