Skip to content

Commit

Permalink
Add TrajOpt planner config
Browse files Browse the repository at this point in the history
This commit adds the files needed for running the trajopt planner example that is found in the
[moveit_tutorials](https://ros-planning.github.io/moveit_tutorials/doc/trajopt_planner/trajopt_planner_tutorial.html).
This planner is not included into `move_group.launch` by default but can be invoked via the `pipeline` argument.
This was done since it is not yet officially released (see moveit/moveit#1467).
  • Loading branch information
rickstaa authored Oct 4, 2021
1 parent 36d7c9f commit bc2bbc7
Show file tree
Hide file tree
Showing 4 changed files with 116 additions and 0 deletions.
4 changes: 4 additions & 0 deletions config/ompl_planning.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,8 @@ planner_configs:
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
TrajOpt:
type: geometric::TrajOpt
panda_arm:
planner_configs:
- AnytimePathShortening
Expand All @@ -153,6 +155,7 @@ panda_arm:
- LazyPRMstar
- SPARS
- SPARStwo
- TrajOpt
hand:
planner_configs:
- AnytimePathShortening
Expand All @@ -179,3 +182,4 @@ hand:
- LazyPRMstar
- SPARS
- SPARStwo
- TrajOpt
58 changes: 58 additions & 0 deletions config/trajopt_planning.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
trajopt_param:
improve_ratio_threshold: 0.25 # minimum ratio true_improve/approx_improve to accept step. [Research Paper] TrueImprove/ModelImprove > c
min_trust_box_size: 1e-4 # if trust region gets any smaller, exit and report convergence. [Research Paper] xtol
min_approx_improve: 1e-4 # if model improves less than this, exit and report convergence
min_approx_improve_frac: -.Inf # if model improves less than this, exit and report convergence
max_iter: 100 # The max number of iterations
trust_shrink_ratio: 0.1 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao-
trust_expand_ratio: 1.5 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao+
cnt_tolerance: 1e-4 # after convergence of penalty subproblem, if constraint violation is less than this, we're done. [Research Paper] ctol
max_merit_coeff_increases: 5 # number of times that we jack up penalty coefficient. [Reasearch Paper] Max iteration in PenaltyIteration loop
merit_coeff_increase_ratio: 10 # ratio that we increate coeff each time. [Researcy Paper] Penalty scaling factory (italic kappa)
max_time: .inf # not yet implemented
merit_error_coeff: 10 # initial penalty coefficient. [Research Paper] mu_0
trust_box_size: 1e-1 # current size of trust region (component-wise). [Research Paper] s

problem_info:
basic_info:
n_steps: 20 # 2 * steps_per_phase
dt_upper_lim: 2.0 # The upper limit of 1/dt values allowed in the optimization
dt_lower_lim: 100.0 # The lower limit of 1/dt values allowed in the optimization
start_fixed: true # if true, start pose is the current pose at timestep = 0
# if false, the start pose is the one given by req.start_state
use_time: false # if false, it means the timestep is unit, meaning x1-x0 is the velocity for example
# if true, then cost_infos ro cnt_infos should have TT_USE_TIME for their term_type
convex_solver: 1 # which convex solver to use
# 1 = AUTO_SOLVER, 2 = BPMPD, 3 = OSQP, 4 = QPOASES, 5 = GUROBI

init_info:
type: 3 # 1 = STATIONARY, 2 = JOINT_INTERPOLATED, 3 = GIVEN_TRAJ
# request.start_state should be provided for JOINT_INTERPOLATED and GIVEN_TRAJ
dt: 0.5

joint_pos_term_info:
start_pos: # from req.start_state
name: start_pos
first_timestep: 10 # First time step to which the term is applied.
last_timestep: 10 # Last time step to which the term is applied.
# if start_fixed is trure then we can not set the first_timestep to 0 for start_pos as it is going
# to be ignored and only the current pose would be the constraint at timestep = 0.
term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME
middle_pos:
name: middle_pos
first_timestep: 15
last_timestep: 15
term_type: 2
goal_pos:
name: goal_pos
first_timestep: 19
last_timestep: 19
term_type: 2
goal_tmp: # If the constraint from request does not have any name, for example when using MotionPlanning Display in Rviz,
# goal_tmp is assigned as the name of the constraint.
# In this case: start_fixed = false and start_pos should be applied at timestep 0
# OR: start_fixed = true and start_pos can be applies at any timestep
name: goal_tmp
first_timestep: 19 # n_steps - 1
last_timestep: 19 # n_steps - 1
term_type: 2
22 changes: 22 additions & 0 deletions launch/run_benchmark_trajopt.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
<launch>

<!-- This argument must specify the list of .cfg files to process for benchmarking -->
<arg name="cfg" />

<!-- Load URDF -->
<include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>

<!-- Start the database -->
<include file="$(find moveit_resources_panda_moveit_config)/launch/warehouse.launch">
<arg name="moveit_warehouse_database_path" value="moveit_trajopt_benchmark_warehouse"/>
</include>

<!-- Start Benchmark Executable -->
<node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
<rosparam command="load" file="$(find moveit_resources_panda_moveit_config)/config/kinematics.yaml"/>
<rosparam command="load" file="$(find moveit_resources_panda_moveit_config)/config/trajopt_planning.yaml"/>
</node>

</launch>
32 changes: 32 additions & 0 deletions launch/trajopt_planning_pipeline.launch.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
<launch>

<!-- TrajOpt Plugin for MoveIt -->
<arg name="planning_plugin" value="trajopt_interface/TrajOptPlanner" />

<!-- define capabilites that are loaded on start (space seperated) -->
<arg name="capabilities" default=""/>

<!-- inhibit capabilites (space seperated) -->
<arg name="disable_capabilities" default=""/>

<!-- The request adapters (plugins) used when planning with TrajOpt.
ORDER MATTERS -->
<arg name="planning_adapters" default="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" />

<arg name="start_state_max_bounds_error" default="0.1" />
<arg name="jiggle_fraction" default="0.05" />

<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)" />
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
<param name="capabilities" value="$(arg capabilities)" />
<param name="disable_capabilities" value="$(arg disable_capabilities)" />

<rosparam command="load" file="$(find panda_moveit_config)/config/trajopt_planning.yaml"/>

</launch>

0 comments on commit bc2bbc7

Please sign in to comment.