[WIP] Ensure ROS 2 Compatibility

.setup_assistant

@@ -1,7 +1,7 @@
 moveit_setup_assistant_config:
  URDF:
  package: franka_description
- relative_path: robots/panda/panda.urdf.xacro
+ relative_path: robots/panda/panda_arm.urdf.xacro
  xacro_args: hand:=true
  SRDF:
  relative_path: config/panda.srdf.xacro

CMakeLists.txt

@@ -1,10 +1,7 @@
-cmake_minimum_required(VERSION 3.1.3)
+cmake_minimum_required(VERSION 3.10.2)
 project(panda_moveit_config)
+find_package(ament_cmake REQUIRED)
 
-find_package(catkin REQUIRED)
+ament_package()
 
-catkin_package()
-
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
- PATTERN "setup_assistant.launch" EXCLUDE)
-install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})

config/chomp_planning.yaml

@@ -1,18 +1,36 @@
+planning_plugin: chomp_interface/CHOMPPlanner
+request_adapters: >-
+ default_planner_request_adapters/AddTimeOptimalParameterization
+ default_planner_request_adapters/FixWorkspaceBounds
+ default_planner_request_adapters/FixStartStateBounds
+ default_planner_request_adapters/FixStartStateCollision
+ default_planner_request_adapters/FixStartStatePathConstraints
+start_state_max_bounds_error: 0.1
 planning_time_limit: 10.0
 max_iterations: 200
 max_iterations_after_collision_free: 5
 smoothness_cost_weight: 0.1
 obstacle_cost_weight: 1.0
 learning_rate: 0.01
+animate_path: true
+add_randomness: false
 smoothness_cost_velocity: 0.0
 smoothness_cost_acceleration: 1.0
 smoothness_cost_jerk: 0.0
+hmc_discretization: 0.01
+hmc_stochasticity: 0.01
+hmc_annealing_factor: 0.99
+use_hamiltonian_monte_carlo: false
 ridge_factor: 0.0
 use_pseudo_inverse: false
 pseudo_inverse_ridge_factor: 1e-4
+animate_endeffector: false
+animate_endeffector_segment: "panda_rightfinger"
 joint_update_limit: 0.1
 collision_clearance: 0.2
 collision_threshold: 0.07
+random_jump_amount: 1.0
 use_stochastic_descent: true
 enable_failure_recovery: false
 max_recovery_attempts: 5
+trajectory_initialization_method: "quintic-spline"

config/joint_limits.yaml

@@ -59,4 +59,13 @@ joint_limits:
  max_velocity: 2.6100
  has_acceleration_limits: true
  max_acceleration: 5
-
+ panda_finger_joint1:
+ has_velocity_limits: true
+ max_velocity: 0.1
+ has_acceleration_limits: false
+ max_acceleration: 0.0
+ panda_finger_joint2:
+ has_velocity_limits: true
+ max_velocity: 0.1
+ has_acceleration_limits: false
+ max_acceleration: 0.0

config/lerp_planning.yaml

@@ -1 +1,9 @@
+planning_plugin: lerp_interface/LERPPlanner
+request_adapters: >-
+ default_planner_request_adapters/AddTimeOptimalParameterization
+ default_planner_request_adapters/FixWorkspaceBounds
+ default_planner_request_adapters/FixStartStateBounds
+ default_planner_request_adapters/FixStartStateCollision
+ default_planner_request_adapters/FixStartStatePathConstraints
+start_state_max_bounds_error: 0.1
 num_steps: 40

config/moveit_controllers.yaml

@@ -0,0 +1,32 @@
+trajectory_execution:
+ allowed_execution_duration_scaling: 1.2
+ allowed_goal_duration_margin: 0.5
+ allowed_start_tolerance: 0.01
+
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+ controller_names:
+ - panda_arm_controller
+ - panda_hand_controller
+
+ 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
+
+ panda_hand_controller:
+ action_ns: gripper_cmd
+ type: GripperCommand
+ default: true
+ joints:
+ - panda_finger_joint1
+

config/ompl_planning.yaml

@@ -1,49 +1,42 @@
 planner_configs:
- AnytimePathShortening:
- type: geometric::AnytimePathShortening
- shortcut: true # Attempt to shortcut all new solution paths
- hybridize: true # Compute hybrid solution trajectories
- max_hybrid_paths: 24 # Number of hybrid paths generated per iteration
- num_planners: 4 # The number of default planners to use for planning
- planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
- SBL:
+ SBLkConfigDefault:
  type: geometric::SBL
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
- EST:
+ ESTkConfigDefault:
  type: geometric::EST
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
  goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
- LBKPIECE:
+ LBKPIECEkConfigDefault:
  type: geometric::LBKPIECE
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
  min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
- BKPIECE:
+ BKPIECEkConfigDefault:
  type: geometric::BKPIECE
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
- border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
+ border_fraction: 0.9 # Fraction fof time focused on boarder default: 0.9
  failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
  min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
- KPIECE:
+ KPIECEkConfigDefault:
  type: geometric::KPIECE
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
  border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
  failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
  min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
- RRT:
+ RRTkConfigDefault:
  type: geometric::RRT
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
- RRTConnect:
+ RRTConnectkConfigDefault:
  type: geometric::RRTConnect
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
- RRTstar:
+ RRTstarkConfigDefault:
  type: geometric::RRTstar
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
  delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
- TRRT:
+ TRRTkConfigDefault:
  type: geometric::TRRT
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
@@ -54,20 +47,20 @@ planner_configs:
  frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
  frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
  k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup()
- PRM:
+ PRMkConfigDefault:
  type: geometric::PRM
  max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
- PRMstar:
+ PRMstarkConfigDefault:
  type: geometric::PRMstar
- FMT:
+ FMTkConfigDefault:
  type: geometric::FMT
  num_samples: 1000 # number of states that the planner should sample. default: 1000
  radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1
  nearest_k: 1 # use Knearest strategy. default: 1
  cache_cc: 1 # use collision checking cache. default: 1
  heuristics: 0 # activate cost to go heuristics. default: 0
  extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
- BFMT:
+ BFMTkConfigDefault:
  type: geometric::BFMT
  num_samples: 1000 # number of states that the planner should sample. default: 1000
  radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0
@@ -77,9 +70,9 @@ planner_configs:
  heuristics: 1 # activates cost to go heuristics. default: 1
  cache_cc: 1 # use the collision checking cache. default: 1
  extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
- PDST:
+ PDSTkConfigDefault:
  type: geometric::PDST
- STRIDE:
+ STRIDEkConfigDefault:
  type: geometric::STRIDE
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
@@ -90,95 +83,96 @@ planner_configs:
  max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6
  estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0
  min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2
- BiTRRT:
+ BiTRRTkConfigDefault:
  type: geometric::BiTRRT
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1
  init_temperature: 100 # initial temperature. default: 100
  frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
  frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
  cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
- LBTRRT:
+ LBTRRTkConfigDefault:
  type: geometric::LBTRRT
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
  epsilon: 0.4 # optimality approximation factor. default: 0.4
- BiEST:
+ BiESTkConfigDefault:
  type: geometric::BiEST
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
- ProjEST:
+ ProjESTkConfigDefault:
  type: geometric::ProjEST
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
- LazyPRM:
+ LazyPRMkConfigDefault:
  type: geometric::LazyPRM
  range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
- LazyPRMstar:
+ LazyPRMstarkConfigDefault:
  type: geometric::LazyPRMstar
- SPARS:
+ SPARSkConfigDefault:
  type: geometric::SPARS
  stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
  sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
  dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
  max_failures: 1000 # maximum consecutive failure limit. default: 1000
- SPARStwo:
+ SPARStwokConfigDefault:
  type: geometric::SPARStwo
  stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
  sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
  dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
  max_failures: 5000 # maximum consecutive failure limit. default: 5000
-$(arg arm_id)_arm: &arm_config
+ TrajOptDefault:
+ type: geometric::TrajOpt
+
+panda_arm:
  planner_configs:
- - AnytimePathShortening
- - SBL
- - EST
- - LBKPIECE
- - BKPIECE
- - KPIECE
- - RRT
- - RRTConnect
- - RRTstar
- - TRRT
- - PRM
- - PRMstar
- - FMT
- - BFMT
- - PDST
- - STRIDE
- - BiTRRT
- - LBTRRT
- - BiEST
- - ProjEST
- - LazyPRM
- - LazyPRMstar
- - SPARS
- - SPARStwo
- projection_evaluator: joints($(arg arm_id)_joint1,$(arg arm_id)_joint2)
- longest_valid_segment_fraction: 0.005
-$(arg arm_id)_manipulator: *arm_config
-$(arg arm_id)_hand:
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ - FMTkConfigDefault
+ - BFMTkConfigDefault
+ - PDSTkConfigDefault
+ - STRIDEkConfigDefault
+ - BiTRRTkConfigDefault
+ - LBTRRTkConfigDefault
+ - BiESTkConfigDefault
+ - ProjESTkConfigDefault
+ - LazyPRMkConfigDefault
+ - LazyPRMstarkConfigDefault
+ - SPARSkConfigDefault
+ - SPARStwokConfigDefault
+ - TrajOptDefault
+panda_arm_hand:
  planner_configs:
- - AnytimePathShortening
- - SBL
- - EST
- - LBKPIECE
- - BKPIECE
- - KPIECE
- - RRT
- - RRTConnect
- - RRTstar
- - TRRT
- - PRM
- - PRMstar
- - FMT
- - BFMT
- - PDST
- - STRIDE
- - BiTRRT
- - LBTRRT
- - BiEST
- - ProjEST
- - LazyPRM
- - LazyPRMstar
- - SPARS
- - SPARStwo
+ - SBLkConfigDefault
+ - ESTkConfigDefault
+ - LBKPIECEkConfigDefault
+ - BKPIECEkConfigDefault
+ - KPIECEkConfigDefault
+ - RRTkConfigDefault
+ - RRTConnectkConfigDefault
+ - RRTstarkConfigDefault
+ - TRRTkConfigDefault
+ - PRMkConfigDefault
+ - PRMstarkConfigDefault
+ - FMTkConfigDefault
+ - BFMTkConfigDefault
+ - PDSTkConfigDefault
+ - STRIDEkConfigDefault
+ - BiTRRTkConfigDefault
+ - LBTRRTkConfigDefault
+ - BiESTkConfigDefault
+ - ProjESTkConfigDefault
+ - LazyPRMkConfigDefault
+ - LazyPRMstarkConfigDefault
+ - SPARSkConfigDefault
+ - SPARStwokConfigDefault
+ - TrajOptDefault
+

config/pilz_industrial_motion_planner_planning.yaml

@@ -0,0 +1,10 @@
+planning_plugin: pilz_industrial_motion_planner/CommandPlanner
+request_adapters: >-
+ default_planner_request_adapters/FixWorkspaceBounds
+ default_planner_request_adapters/FixStartStateBounds
+ default_planner_request_adapters/FixStartStateCollision
+ default_planner_request_adapters/FixStartStatePathConstraints
+default_planner_config: PTP
+capabilities: >-
+ pilz_industrial_motion_planner/MoveGroupSequenceAction
+ pilz_industrial_motion_planner/MoveGroupSequenceService