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節點時進行加載
<param name="move_group/constraint_approximations_path" value="<path_to_database>"/>
- - 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
加載：<rosparam command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" /> #或sensors_kinect_depthmap.yaml
- - Octomap Configuration
configure the Octomap by adding the following lines into the sensor_manager.launch:
<param name="octomap_frame" type="string" value="odom_combined" />
<param name="octomap_resolution" type="double" value="0.05" />
<param name="max_range" type="double" value="5.0" />
- 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進行優化
<arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints
default_planner_request_adapters/CHOMPOptimizerAdapter" />
<rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
<rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
在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進行優化
<arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints
default_planner_request_adapters/CHOMPOptimizationAdapter" />
<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml"/>
<rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
在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