環境：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