ROS Industrial——運動学計画アルゴリズムSTOMP

環境:Ubuntu 16.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、内容は以下の通り、configパッケージの名前に変更する必要があります.

3、move_を修正するgroup.launchファイル

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.

初期化軌跡の初期化方法(1 LINEAR_INTERPOLATION,2 COBLIC_POLYNOMIAL,3 MININUM_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 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