Preventing irregular trajectories in MoveIts Grasping Pipeline#
Task: The MoveIt Grasping Pipeline is used to grasp objects and then place them on the opposite side of the robotic arm (horizontal rotation of 180°).
Problem: MoveIt uses cartesian space trajectory planning inside of its grasping module. This can have 2 effects. Firstly, some planners may not be able to find a trajectory. Or secondly, the trajectory is irregular. This means that it is not possible to execute the trajectory on a real robot due to jumps in the joint positions, or even not correct joint-position. For the OMPL-planner, this can be solved easily.
Solution:
Navigate to the used MoveIt configuration package, e.g. the panda_moveit_config for the Franka Emika Panda
Locate the configuration file of OMPL (ompl_planning.yaml)
Modify the configuration of the corresponding planning group / move group by adding:
enforce_joint_model_state_space: true
This results for example in the following configuration for a planning group:
panda_arm:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
- FMTkConfigDefault
- BFMTkConfigDefault
- PDSTkConfigDefault
- STRIDEkConfigDefault
- BiTRRTkConfigDefault
- LBTRRTkConfigDefault
- BiESTkConfigDefault
- ProjESTkConfigDefault
- LazyPRMkConfigDefault
- LazyPRMstarkConfigDefault
- SPARSkConfigDefault
- SPARStwokConfigDefault
- TrajOptDefault
longest_valid_segment_fraction: 0.005
enforce_joint_model_state_space: true
This solution will enable the usage of joint space trajectories and thus prevents for most of the possible situations irregular trajectories.
Related Tutorial: OMPL Interface Tutorial