MoveIt Tutorial学习笔记 - 二

http://docs.ros.org/kinetic/api/moveit_tutorials/html/index.html 阅读学习笔记 ；保存成.md文件查看

#### Planning with Approximated Constraint Manifolds  使用近似约束流型进行规划
-  reduces planning time.
将用户设定的约束，先转化为Approximated Constraint Manifolds，并保存到一个database；后续直接基于这个database进行运动规划，以减少运动规划的时间；

compute an approximation of the constraint manifold beforehand and perform trajectory planning in that. The OMPL plugin contains the functionality to do that for a given set of constraints and save it in a database. In later instances the database can be loaded to use for constrained planning with any OMPL planner which strongly reduces planning time.

-  Database Construction  数据库的构建
adding the constraints to the ConstraintsLibrary object the database can be constructed by callingsaveApproximationConstraints()

The function requires four parameters:
1.constraints message (moveit_msgs::Constraints)
2.robot description (std::string)
3.planning scene (planning_scene::PlanningScenePtr)
4.construction options (ompl_interface::ConstraintApproximationConstructionOptions)

- Database Loading and Usage
- - Loading：
The constraints database must be loaded at launch of the move group node by setting the ros parameter://必须在启动move group节点时进行加载


- - Usage：
For planning just initialize the constraints message as always and set the messages name to the exact name that was used to construct the database. Also you need to specify the same values and tolerances again since by default the planner just samples over the states but does not necessary follow the constraints during interpolation for path planning. A correctly named constraint message without initialized constraints would use the database but can therefore lead to invalid trajectories anyway.
//初始化constraints message，该message的name要与用于生成database的message name保持一致；不需要对constraints message中的constraints进行初始化；规划时将自动调用先前生成的database；


#### Pick and Place 
In MoveIt!, grasping is done using the MoveGroup interface. 

相关数据类型：主要对这两个数据类型中各功能元素的理解；设定了pick和place相关的动作、姿态等
moveit_msgs::Grasp  //grasp
moveit_msgs::PlaceLocation  //place

pick and place动作接口：
move_group.setSupportSurfaceName("table1");
move_group.pick("object", grasps);

group.setSupportSurfaceName("table2");
group.place("object", place_location);


### Integration with a New Robot

#### MoveIt! Setup Assistant
Step 3: Add Virtual Joints
Virtual joints are used primarily to attach the robot to the world. 

Step 7: Add Passive Joints
This tells the planners that they cannot (kinematically) plan for these joints because they can’t be directly controlled.

Step 8: 3D Perception
 configuring the 3D sensors sensors_3d.yaml.

Step 9: Gazebo Simulation
 help you simulate your robot with Gazebo by generating a new Gazebo compatible urdf if needed.

Step 10: ROS Control
 auto generate simulated controllers to actuate the joints of your robot.
基于ros_control框架


#### URDF and SRDF
Safety Limits
If the “soft_lower_limit” and the “soft_upper_limit” in the safety_controller are set to 0.0, your joint will be unable to move.

#### Low Level Controllers
configuring MoveIt! with the controllers on your robot. We will assume that your robot offers a FollowJointTrajectory action service for the arms on your robot and (optionally) a GripperCommand service for your gripper. If your robot does not offer this we recommend the ROS control framework for easily adding this functionality around your hardware communication layer.
//基于ros_control framework, 增加FollowJointTrajectory action service for the arms；GripperCommand service for your gripper.此处仅介绍MoveIt端的相关配置；

-  controllers.yaml进行MoveIt端 控制器  配置
- -  对FollowJointTrajectory Controller Interface和GripperCommand Controller Interface进行配置
controller_list:
 - name: panda_arm_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
     - panda_joint1
     - panda_joint2
     - panda_joint3
     - panda_joint4
     - panda_joint5
     - panda_joint6
     - panda_joint7
 - name: hand_controller
   action_ns: gripper_action
   type: GripperCommand
   default: true
   parallel: true
   joints:
     - panda_finger_joint1
     - panda_finger_joint2

- - Optional Allowed Trajectory Execution Duration Parameters：
allowed_execution_duration_scaling: 1.2
allowed_goal_duration_margin: 0.5
The parameters are used to compute the allowed trajectory execution duration by scaling the expected execution duration and adding the margin afterwards. If this duration is exceeded the trajectory will be cancelled. The controller-specific parameters can be set as follows


- Create the Controller launch file 加载
robot_moveit_controller_manager.launch.xml

-  Debugging Information  调试
The FollowJointTrajectory or GripperCommand interfaces on your robot must be communicating in the namespace: /name/action_ns. In the above example, you should be able to see the following topics (using rostopic list) on your robot:
/panda_arm_controller/follow_joint_trajectory/goal ### 格式：/contorller_name/action_ns_name.
/panda_arm_controller/follow_joint_trajectory/feedback
/panda_arm_controller/follow_joint_trajectory/result
/hand_controller/gripper_action/goal
/hand_controller/gripper_action/feedback
/hand_controller/gripper_action/result


- Remapping /joint_states topic 重映射
When you run a move group node, you may need to remap the topic /joint_states to /robot/joint_states, otherwise MoveIt! won’t have feedback from the joints. 


- Trajectory Execution Manager Options 轨迹执行管理器 选项
trajectory_execution.launch.xml
1.execution_duration_monitoring: when false, will not throw error is trajectory takes longer than expected to complete at the low-level controller side
2.allowed_goal_duration_margin: Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling)
3.allowed_start_tolerance: Allowed joint-value tolerance for validation that trajectory’s first point matches current robot state. If set to zero will skip waiting for robot to stop after execution


#### Perception Pipeline Tutorial
integration of 3D sensors using Octomap.
配置、集成3D传感器；

The primary component in MoveIt! that deals with 3D perception is the Occupancy Map Updater. The updater uses a plugin architecture to process different types of input. The currently available plugins in MoveIt! are:
1.The PointCloud Occupancy Map Updater: which can take as input point clouds (sensor_msgs/PointCloud2)
2.The Depth Image Occupancy Map Updater: which can take as input Depth Images (sensor_msgs/Image)


- YAML Configuration file (Point Cloud)
“sensors_kinect_pointcloud.yaml”:
sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /camera/depth_registered/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud


-  YAML Configuration file (Depth Map)
“sensors_kinect_depthmap.yaml”:
sensors:
  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
    image_topic: /camera/depth_registered/image_raw
    queue_size: 5
    near_clipping_plane_distance: 0.3
    far_clipping_plane_distance: 5.0
    shadow_threshold: 0.2
    padding_scale: 4.0
    padding_offset: 0.03
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud


-  Update the launch file
 need to update the sensor_manager.launch file
加载： #或sensors_kinect_depthmap.yaml

- -  Octomap Configuration
configure the Octomap by adding the following lines into the sensor_manager.launch:





-  Detecting and Adding Object as Collision Object
 extracting a cylinder from a pointcloud, computing relevant values and adding it as a collision object to the planning scene. 



#### IKFast Kinematics Solver
configuring an IKFast plugin for MoveIt；配置过程较麻烦；解析解；

IKFast， provided within Rosen Diankov’s OpenRAVE motion planning software.
IKFast可以解析地求解任何复杂运动学链的运动学方程，并生成特定于语言的文件（如C ++）供以后使用。 最终结果是极其稳定的解决方案，在最新的处理器上可以运行高达5微秒的速度；

MoveIt! IKFast ：is a tool that generates a IKFast kinematics plugin for MoveIt using OpenRAVE generated cpp files. 

-  基本配置流程：
安装各种依赖软件和库；
安装MoveIt! IKFast 工具；
安装 OpenRAVE；
Create Collada File For Use With OpenRAVE；
Create IKFast Solution CPP File；
用MoveIt! IKFast工具生成 Plugin；
在kinematics.yaml中进行求解器的配置；


#### TRAC-IK Kinematics Solver
trac-ik-kinematics-plugin；数值解
TRAC-IK是由TRACLabs开发的逆运动学求解器，它通过线程结合了两种IK的实现，比常用的开源IK解算器可获得更可靠的解决方案；

KDL的收敛算法是基于牛顿方法的，这种方法在存在 joint limits的情况下效果不佳。 TRAC-IK同时运行两个IK实现。 一种是对KDL基于牛顿的收敛算法的简单扩展，该算法可以通过随机跳跃来检测并缓解由于joint limits而引起的局部最小值。 第二种是SQP（顺序二次规划）非线性优化方法，它使用拟牛顿方法更好地处理joint limits。 默认情况下，当这些算法中的任何一个收敛到答案时，IK搜索都会立即返回。还提供了distance and manipulability的次要约束条件，以便获得“最佳” IK解决方案。

- 安装：sudo apt-get install ros-kinetic-trac-ik-kinematics-plugin
-  配置： kinematics.yaml 



### Configuration

#### Kinematics Configuration
kinematics.yaml 
panda_arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

- KDL kinematics plugin 数值解
- - only works with serial chains；provided by the Orocos KDL package

position_only_ik: True  //仅在使用KDL Kinematics Plugin时，将其加入到kinematics.yaml 中即可；


- LMA (Levenberg-Marquardt) kinematics plugin   数值解
- - only works with serial chains；provided by the Orocos KDL package
- - Usage: kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin


#### Fake Controller Manager
MoveIt! comes with a series of fake trajectory controllers to be used in simulation.

config/fake_controllers.yaml

#### Custom Constraint Samplers
自定义约束取样器， for more difficult planning problems.
Some planning problems require more complex or custom constraint samplers for more difficult planning problems. 

This document explains how to create a custom motion planning constraint sampler for use with MoveIt!.

需要创建一个ROBOT_moveit_plugins package .在新创建的Plugin下，有Constraint Samplers配置文件；


#### Creating MoveIt Plugins
The plugin class inherits from the base class and overrides its virtual functions. 
The main library used for this purpose is pluginlib. 

This tutorials contains three different types of plugins, namely, motion planner, controller manager and constraint sampler.


#### OMPL Planner
- OMPL Settings
ompl_planning.yaml file

- - 碰撞检测分辨率的配置
longest_valid_segment_fraction：定义了用于碰撞检查的机器人运动的离散化，并极大地影响了基于OMPL的解决方案的性能和可靠性。 在这种情况下，运动可以被视为图中两个节点之间的边线，其中节点是沿着轨迹的waypoints 。 OMPL中的默认运动碰撞检测仅将边线离散化为多个子状态以进行碰撞检测。
longest_valid_segment_fraction是机器人状态空间的一部分，在给定机器人当前没有碰撞的情况下，我们假设机器人可以在保持无碰撞的情况下运行。例如，longest_valid_segment_fraction= 0.01，那么我们假设，如果两个节点之间的边小于状态空间的1/100，那么我们不需要显示检查沿该边的任何子状态，只检查这个边连接的两个节点。//控制网格化、离散化密度，影响总体检测时间；x相当于分辨率


maximum_waypoint_distance：为碰撞检查定义了相同的机器人运动离散化，但它是在绝对水平上进行的，而不是使用状态空间的分数（百分比）。例如，如果maximum_waypoint_distance = 0.1，那么如果一条边在状态空间距离上小于0.1，那么我们就不会显示地检查沿着该边的任何子状态。

longest_valid_segment_fraction和maximum_waypoint_distance都可用来控制状态空间的离散化，如果同时设定了，(generate the most states to collision check on a given edge ) is chosen.
这两个值设置得太低，碰撞检测/运动规划将非常慢。设置得太高，在小的或窄的物体周围会错过碰撞。此外，较高的碰撞检测分辨率将导致路径平滑器输出不可理解的运动，因为它们能够“捕捉”无效路径，然后尝试通过在路径周围采样来修复它们，但是不完美。


- - projection_evaluator
projection_evaluator可以采用关节或连杆的列表来近似configuration space的覆盖范围。这些规划器（例如KPIECE，BKPIECE，LBKPIECE和PDST）使用此设置。 有关更多信息，请阅读相应的出版物。

- - Enforce Planning in Joint Space 
enforce_joint_model_state_space ；
根据规划问题的不同，MoveIt在joint space和cartesian space之间进行选择以对问题进行描述。 设置参数force_joint_model_state_space，强制所有规划在joint space内进行。

默认情况下，对具有方向路径约束的规划请求，在cartesian space进行采样，以便调用IK作为generative sampler（生成采样器）。

通过强制joint space，规划过程将使用 rejection sampling来找到有效的请求。 请不要这样，因为这可能会大大增加规划时间。

- - Other Settings
不同的planer都有相关的参数配置，以进行tuning；默认值由MoveIt! Setup Assistant自动生成，在ompl_planning.yaml文件中；建议轻微的调整这些默认值；


- - OMPL Optimization Objectives
优化目标

- - - OMPL规划库的一些规划器，能够对指定的优化目标进行优化，称之为optimal planners。目前具备该功能的规划器有：geometric::RRTstar和geometric::PRMstar

- - - 目前MoveIt中可用的optimization objectives有：
1.PathLengthOptimizationObjective (Default)  //路径长度
2.MechanicalWorkOptimizationObjective        //机械功
MaximizeMinClearanceObjective                //最大最小路径间隙（同碰撞物的距离）
StateCostIntegralObjective                   //状态代价积分
MinimaxObjective

这些optimization objectives（对路径质量进行度量的指标）的配置，可在ompl_planning.yaml中进行；比如：
RRTstarkConfigDefault:
    type: geometric::RRTstar
    optimization_objective: MaximizeMinClearanceObjective
    range: 0.0
    goal_bias: 0.05
    delay_collision_checking: 1

更多的资料信息可阅读： OMPL - Optimal Planning documentation：http://ompl.kavrakilab.org/optimalPlanning.html


#### CHOMP Planner
Covariant Hamiltonian optimization for motion planning (CHOMP) //运动规划的协变哈密顿优化
（CHOMP）是一种新颖的基于梯度的轨迹优化程序，使许多日常运动规划问题既简单又易于训练（Ratliff et al。，2009c）。 

-  CHOMP和OMPL获得的规划之间的差异

优化规划器可以优化代价函数，有时可能会导致令人惊奇的结果：通过细小的障碍物可能比避免所有碰撞的长而曲折的轨迹的成本更低。

OMPL是一个开源库，用于基于采样/随机运动的规划算法。OMPL不包含任何与碰撞检查或可视化相关的代码，因为OMPL的设计人员不想将其绑定到任何特定的碰撞检查器或可视化前端。该库的设计使其可以轻松集成到提供其他组件的系统中。OMPL中的规划器是抽象的，即OMPL没有机器人的概念，相反，MoveIt！直接与OMPL集成，并将OMPL中的运动规划器用作其默认规划器集。

CHOMP：虽然大多数高维运动规划器将轨迹生成划分为不同的规划和优化阶段，但是该算法利用协变梯度和函数梯度方法进入优化阶段，以设计出完全基于轨迹优化的运动计划算法。给定一个infeasible轨迹，CHOMP会对周围环境做出反应，以快速将轨迹拉出碰撞，同时优化诸如关节速度和加速度之类的动力学量。 它迅速收敛到可以在机器人上高效执行的平滑无碰撞轨迹。协变更新规则 可确保CHOMP迅速收敛到局部最优轨迹。

对于包含障碍物的场景，CHOMP通常会通过在代价函数中添加一些噪声（ridge_factor）来生成不平滑轨迹的路径，以实现机器人的动态量（如加速度，速度）。CHOMP在大多数情况下都可以避免障碍，但是如果由于轨迹的初始猜测不正确而陷入局部极小值，它可能会失败。OMPL可用于为CHOMP生成无碰撞的种子轨迹，以缓解此问题。在Planning Adapter中，可将OMPL配置为CHOMP的pre-processor。


- Using CHOMP with Your Robot
概述：
chomp_planning.yaml file
chomp_planning_pipeline.launch.xml file 
Copy the demo.launch file to demo_chomp.launch，并对相关参数进行更改；move_group.launch中的相关参数设置；


- Tweaking some of the parameters for CHOMP 
调整一些参数；
chomp_planning.yaml file中

planning_time_limit：优化程序在终止之前找到解决方案所花费的最长时间
max_iterations：这是规划器在优化时可以找到最佳解决方案的最大迭代次数
max_iterations_after_collision_free：找到无冲突路径后要执行的最大迭代次数。
smoothness_cost_weight：smoothness_cost_weight参数控制其在CHOMP实际上正在优化的最终成本中的权重
obstacle_cost_weight：此控制体重对最终成本的Chomp优化了必须考虑到的障碍。例如，0.0将有障碍被忽略，1.0将是硬约束
learning_rate：这是优化程序用于查找局部/全局最小值同时降低总成本的学习率。
smoothness_cost_velocity，smoothness_cost_acceleration，smoothness_cost_jerk：与速度，加速度和加速度率成本相关的变量。
ridge_factor：添加到目标函数中总二次成本矩阵对角线的噪声。小噪声（例如0.001）的添加使CHOMP能够以轨迹平滑为代价避免障碍。
use_pseudo_inverse：是否启用伪逆计算。
pseudo_inverse_ridge_factor：如果启用了伪逆，则设置岭因子。
joint_update_limit：设置机器人关节的更新限制
crash_clearance：避免障碍物需要保持的最小距离。
碰撞阈值：需要保持碰撞阈值成本以避免碰撞
use_stochastic_descent：如果要在优化成本的同时使用随机下降，则将其设置为true / false。在随机下降中，使用轨迹的随机点，而不是所有轨迹点。这样可以更快并保证收敛，但是在最坏的情况下可能需要更多的迭代。
启用故障恢复：如果将其设置为true，则CHOMP会调整ceratin参数，以期在chomp_planning.yaml文件中指定默认参数不存在的情况下找到解决方案。
max_recovery_attempts：这是在第一次尝试使用默认参数之后，使用各种参数集运行CHOMP的最大次数。
trajectory_initializaiton_method：可这里的Chomp提供轨迹的初始化类型，这可以是quintic-spline，linear，cubic或fillTrajectory。前三个选项是指用于起始状态和目标状态之间的轨迹初始化的插值方法。fillTrajectory提供了一种从现有运动计划器（如OMPL）计算出的路径初始化轨迹的选项。

一些未使用/已注释的参数是hmc_stochasticity，hmc_annealing_factor，hmc_discretization，use_hamiltonian_montecarlo，animate_endeffector，animate_endeffector_segment，animate_path，random_jump_amount，add_randomness。

CHOMP的默认参数在没有障碍的环境中效果很好。但是，在有许多障碍的环境中，默认参数可能会导致CHOMP卡在局部最小值中。通过调整参数，我们可以提高CHOMP生成的计划的质量。


#### STOMP Planner

- STOMP，CHOMP和OMPL获得的规划之间的差异
一些moveIt规划器倾向于产生抖动的轨迹，并可能引入不必要的机器人动作，通常需要后处理平滑步骤。

由于STOMP倾向于在短时间内生成平滑且行为良好的运动计划，因此不需要其他运动规划器所要求的后处理平滑步骤。
CHOMP基于协变和函数梯度方法来优化给定的初始轨迹，完全基于轨迹优化。
OMPL是一个开源库，用于基于采样/随机运动计划算法。基于采样的算法很可能是完整的：如果存在某个解决方案，则最终会找到解决方案，但是无法报告解决方案的不存在。

下面对这些规划器进行了定性分析：
Local Minima Handling局部最小值处理：STOMP可以避免局部最小值，因为它具有随机性。但是，CHOMP倾向于并且经常卡在局部最小值中，从而避开了最佳解决方案。根据STOMP和CHOMP论文，STOMP性能更好。CHOMP由于其基于梯度的性质，陷入局部极小值，并且通常无法找到解或返回次优解。//STOMP优与CHOMP
Time requirements时间需求：尽管CHOMP比STOMP需要更多的迭代才能成功，但执行时间是类似的。这主要是因为STOMP的每次迭代都需要多次轨迹代价评估，但是可以比CHOMP梯度更新规则 以更稳定地方式采用较大的步幅进行迭代。OMPL算法高效，通常可以快速找到解决方案。//OMPL最优；STOMP与CHOMP差不多；
Parameter tuning参数整定：与STOMP相比，CHOMP通常需要其他参数调整才能获得成功的解决方案。OMPL不需要大量参数调整，默认参数在大多数情况下都能很好地完成工作。//OMPL>STOMP>CHOMP
Obstacle Handling障碍物处理：对于包含障碍物的场景，STOMP由于其随机性，通常能够成功避开障碍物。但是，CHOMP会通过在成本函数中添加一些噪声（ridge_factor）来生成不平滑轨迹的路径，以实现机器人的动态量（如加速度，速度）。OMPL还可以在存在障碍物的情况下生成无碰撞的平滑路径。//OMPL>STOMP>CHOM

- -  综合以上定性分析，即可明白，MoveIt为什么采用OMPL作为默认运动规划器；对于规划结果的优化，可采用STOMP作为OMPL的后处理器。


- Using STOMP with Your Robot
概述：
stomp_planning.yaml file
stomp_planning_pipeline.launch.xml file 
Copy the demo.launch file to demo_stomp.launch，并对相关参数进行更改；move_group.launch中的相关参数设置；

在避免障碍方面，STOMP比CHOMP表现更好。这是由于STOMP的随机性会产生非抖动的轨迹，而CHOMP通常会产生抖动的路径来避免障碍。


- 调整STOMP的一些参数
stomp_planning.yaml file中

- -  Optimization Parameters 优化参数
num_timesteps：优化程序在终止之前查找解决方案所花费的时间步数。
num_iterations：这是规划器在优化时可以找到良好解决方案的迭代次数。
num_iterations_after_valid：找到有效路径后要执行的最大迭代次数。
num_rollouts：这是嘈杂的轨迹数。
max_rollouts：每次迭代期间新推出和旧推出的总数量不应超过此值。
nitialization method：这是选择的初始化方法，用于选择初始化轨迹的方式。
control_cost_weight：这是在总代价计算中 应用的 轨迹加速代价的百分比。

- -  Noise Generator Parameters  噪声发生器参数
class：可以设置为“ NormalDistributionSampling”（默认）或“ GoalGuidedMultivariateGaussian”。根据使用的类，需要设置特定的参数。如果使用“ GoalGuidedMultivariateGaussian”，请查看此链接以设置参数。
stddev：这是可以应用于关节的噪音程度。此数组中的每个值都是在数组中该位置处施加到关节的噪声的幅度。对于instace，数组中最左边的值将是用于设置机器人第一个关节的噪音的值（在本例中为panda_joint1）。该数组的维数应等于计划组名称中的关节数。较大的“ stddev”值对应于关节的较大运动。


- -  Cost Function Parameters 代价函数参数
class：您可以在此处设置要使用的成本函数。您可以将其设置为“ CollisionCheck”，“ ObstacleDistanceGradient”或“ ToolGoalPose”。根据您在此处输入的内容，您需要设置适当的成本函数类的参数：对于“ 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）。请看此链接，为“ ToolGoalPose”类设置参数。
crash_penalty：这是分配给碰撞状态的值。
cost_weight：未使用的参数。
kernel_window_percentage：用于计算window_size进行内核平滑的乘法因子。
longest_valid_joint_move：此参数指示关节可以在连续的轨迹点之间移动多远。

- -  Update Filter parameters 更新过滤器参数
class：可以将其设置为“ PolynomialSmoother”或“ ConstrainedCartesianGoal”。需要根据所选类别设置特定的参数。要为“ ConstrainedCartesianGoal”设置参数，请查看此链接。
poly_order：这是用于平滑轨迹的多项式函数的阶数。


为STOMP选择参数比CHOMP需要更少的直觉知识intuition。一个可以具有STOMP的默认参数，这在大多数环境中都能很好地工作。但是，您可以增加num_timesteps，num_rollouts，并使用stddev数组来使STOMP在复杂环境下正常运行，以便STOMP在这些情况下可以找到最佳路径。另外，您可以尝试不同的成本函数，噪声生成器，通过在stomp_config.yaml文件中设置所需的值来更新过滤器类。


- Running STOMP Unit Tests
must have the stomp_core package from ros-industrial/industrial_moveit repository. 

执行：catkin_make run_tests_stomp_moveit stomp_moveit_utest
If these tests run successfully, this implies STOMP is successfully built and running on your system. 

#### Planning Adapter Tutorials 规划适配器
MoveIt中现有规划适配器的一些示例包括AddTimeParameterization，FixWorkspaceBounds，FixStartBounds，FixStartStateCollision，FixStartStatePathConstraints，CHOMPOptimizerAdapter等。 
使用Planning Adapter概念，可以在一个 pipeline中使用多种运动规划算法来生成更加 robust的 motion plans。

OMPL, CHOMP and STOMP as these are the only planners currently supported by MoveIt.
CHOMP and STOMP are not a part of the official release yet；


- Running CHOMP as a post-processor for OMPL
CHOMP可以作为其它规划算法得到的规划的 后处理优化技术。
一些随机规划算法生成CHOMP的初始猜测。然后CHOMP根据这个初始的猜测，进一步优化轨迹。

在ompl_planning_pipeline.launch内进行配置；//规划算法是ompl，采用CHOMP进行优化




在chomp_planning.yaml中，增加以下配置：
trajectory_initialization_method: "fillTrajectory"

 Inside the CHOMP adapter, a call to OMPL is made before invoking the CHOMP optimization solver, so CHOMP takes the initial path computed by OMPL as the starting point to further optimize it.


- Running CHOMP as a post-processor for STOMP
STOMP produces an initial path for CHOMP. CHOMP then takes this initial path and further optimizes this trajectory.

在stomp_planning_pipeline.launch内进行配置；//规划算法是stomp，采用CHOMP进行优化






在chomp_planning.yaml中，增加以下配置：
trajectory_initialization_method: "fillTrajectory"


- Running STOMP as a post-processor for OMPL；
STOMP可以作为其它规划算法得到的规划的 后处理平滑技术。
 STOMP can be used as a post-processing smoothing technique for plans obtained by other planning algorithms；
some randomized planning algorithm produces an initial path for STOMP. STOMP then takes this initial path and further smoothens the trajectory. 

配置过程与以上类似，具体参考教程；
用到的适配器：default_planner_request_adapters/STOMPSmoothingAdapter


- Running STOMP as a post-processor for CHOMP
STOMP can be used as a post-processing smoothing technique for plans obtained by CHOMP. 

配置过程与以上类似，具体参考教程；
用到的适配器：default_planner_request_adapters/STOMPSmoothingAdapter


- when to use which planner and how using certain planning request adapters in a certain pipeline can lead to producing robust paths 

- 单种运动规划算法
OMPL：一个基于采样/随机运动规划算法 的开源代码库。基于采样的算法在概率上是完整的:如果有解的话，最终会找到一个解，但是不能报告不存在解。这些算法很有效，通常能很快找到解决方案。

STOMP：在合理的时间内 生成 平滑且性能良好的无碰撞路径。 该方法用 生成带噪音的轨迹 来探索初始轨迹周围的空间（该初始轨迹可能不可行），然后对该初始轨迹进行组合，以产生代价更低的新轨迹。

CHOMP：是一种优化算法，可优化给定的初始轨迹。 基于环境，CHOMP可迅速地将初始轨迹拉出碰撞。 但是，此处需要注意的重要一点是，为了避免障碍，参数ridge_factor必须大于或等于0.001。 执行此CHOMP可以找到路径，同时避免障碍。 这里应该注意，即使CHOMP可以成功避开障碍物，但是它无法提供平滑的路径，通常会在存在障碍物的情况下会导致 jerky paths。 对于CHOMP，避免碰撞是以牺牲轨迹的速度平滑度为代价的。

-  使用多种运动规划算法来生成更加 robust的 motion plans。
CHOMP as a post-processor for OMPL；// 推荐
CHOMP as a post-processor for STOMP；
STOMP as a post-processor for OMPL；// 推荐
STOMP as a post-processor for CHOMP；



### Miscellaneous 杂项

#### Joystick Control Teleoperation 操纵杆控制遥控
在RViz的 Motion Planning plugin中，在Planning选项卡中启用Allow External commm .复选框。使能Query Goal State选项，其显示在Motion Planning plugin中的 Planning Request部分。//使用Joystick控制RVIZ中的机器人

- Joystick Command Mappings
操纵杆的按键映射；设定目标位姿，plan和execute等。


####  Benchmarking  基准测试：用基准问题进行测试，测试性能
benchmarking package；
To use this benchmarking method, need to download and install the ROS Warehouse plugin. 

benchmarking package提供了一些 使用OMPL Planner Arena 对 运动规划算法和汇总/绘图统计 进行基准测试 的方法。
下面的示例演示了如何 Panda robot arm运行基准测试。


Parameters of the BenchmarkOptions Class 
// 在该类中进行parameters and options的配置；benchmark运行时，会从ROS parameter server.读入这些parameters and options的值；
Parameters of the BenchmarkExecutor Class

执行基准测试，许多参数将被汇总并写入日志文件。 提供了一个脚本（moveit_benchmark_statistics.py）来解析这些数据并绘制统计信息。

对不同运动规划器的基准测试：CHOMP，STOMP和OMPL ；具体配置过程见教程
可以将这些规划器进行相互比较，以获得明确定义的基准（适用于相同的环境，开始状态，查询和目标状态）。 可以报​​告每个规划器的不同指标以获得量化统计数据，这可以帮助在定义的环境中正确选择特定规划器。 为每个规划器报告的统计信息包括：计算路径所需的时间，路径长度，路径时间，是否找到有效路径等。//做一个对比


#### Integration/Unit Tests 集成和单元测试
说明了如何测试 在各种机器人上对MoveIt的更改，包括单元测试和集成测试。

- Integration Test
基于Python的集成测试可用于测试MoveIt中更高级别的move_group功能！
rostest moveit_ros_planning_interface python_move_group.test

-  Unit Tests
要在整个MoveIt 的catkin工作区上，本地运行单元测试，使用catkin-tools；

catkin run_tests -iv
catkin run_tests --no-deps --this -iv