Skip to content

Commit 10c91d7

Browse files
rhaschkerhaschke
committed
Merge MSA fixups and addons
2 parents 9c35c36 + b351e8a commit 10c91d7

13 files changed

+238
-3
lines changed

‎config/chomp_planning.yaml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,12 @@ learning_rate: 0.01
77
smoothness_cost_velocity: 0.0
88
smoothness_cost_acceleration: 1.0
99
smoothness_cost_jerk: 0.0
10-
ridge_factor: 0.01
10+
ridge_factor: 0.0
1111
use_pseudo_inverse: false
1212
pseudo_inverse_ridge_factor: 1e-4
1313
joint_update_limit: 0.1
1414
collision_clearance: 0.2
1515
collision_threshold: 0.07
1616
use_stochastic_descent: true
17-
enable_failure_recovery: true
18-
max_recovery_attempts: 5
17+
enable_failure_recovery: false
18+
max_recovery_attempts: 5

‎config/lerp_planning.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
num_steps: 40

‎config/stomp_planning.yaml‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
2+
stomp/panda_arm:
3+
group_name: panda_arm
4+
optimization:
5+
num_timesteps: 60
6+
num_iterations: 40
7+
num_iterations_after_valid: 0
8+
num_rollouts: 30
9+
max_rollouts: 30
10+
initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]
11+
control_cost_weight: 0.0
12+
task:
13+
noise_generator:
14+
- class: stomp_moveit/NormalDistributionSampling
15+
stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4, 0.4]
16+
cost_functions:
17+
- class: stomp_moveit/CollisionCheck
18+
collision_penalty: 1.0
19+
cost_weight: 1.0
20+
kernel_window_percentage: 0.2
21+
longest_valid_joint_move: 0.05
22+
noisy_filters:
23+
- class: stomp_moveit/JointLimits
24+
lock_start: True
25+
lock_goal: True
26+
- class: stomp_moveit/MultiTrajectoryVisualization
27+
line_width: 0.02
28+
rgb: [255, 255, 0]
29+
marker_array_topic: stomp_trajectories
30+
marker_namespace: noisy
31+
update_filters:
32+
- class: stomp_moveit/PolynomialSmoother
33+
poly_order: 6
34+
- class: stomp_moveit/TrajectoryVisualization
35+
line_width: 0.05
36+
rgb: [0, 191, 255]
37+
error_rgb: [255, 0, 0]
38+
publish_intermediate: True
39+
marker_topic: stomp_trajectory
40+
marker_namespace: optimized

‎config/trajopt_planning.yaml‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
trajopt_param:
2+
improve_ratio_threshold: 0.25 # minimum ratio true_improve/approx_improve to accept step. [Research Paper] TrueImprove/ModelImprove > c
3+
min_trust_box_size: 1e-4 # if trust region gets any smaller, exit and report convergence. [Research Paper] xtol
4+
min_approx_improve: 1e-4 # if model improves less than this, exit and report convergence
5+
min_approx_improve_frac: -.Inf # if model improves less than this, exit and report convergence
6+
max_iter: 100 # The max number of iterations
7+
trust_shrink_ratio: 0.1 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao-
8+
trust_expand_ratio: 1.5 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao+
9+
cnt_tolerance: 1e-4 # after convergence of penalty subproblem, if constraint violation is less than this, we're done. [Research Paper] ctol
10+
max_merit_coeff_increases: 5 # number of times that we jack up penalty coefficient. [Reasearch Paper] Max iteration in PenaltyIteration loop
11+
merit_coeff_increase_ratio: 10 # ratio that we increate coeff each time. [Researcy Paper] Penalty scaling factory (italic kappa)
12+
max_time: .inf # not yet implemented
13+
merit_error_coeff: 10 # initial penalty coefficient. [Research Paper] mu_0
14+
trust_box_size: 1e-1 # current size of trust region (component-wise). [Research Paper] s
15+
16+
problem_info:
17+
basic_info:
18+
n_steps: 20 # 2 * steps_per_phase
19+
dt_upper_lim: 2.0 # The upper limit of 1/dt values allowed in the optimization
20+
dt_lower_lim: 100.0 # The lower limit of 1/dt values allowed in the optimization
21+
start_fixed: true # if true, start pose is the current pose at timestep = 0
22+
# if false, the start pose is the one given by req.start_state
23+
use_time: false # if false, it means the timestep is unit, meaning x1-x0 is the velocity for example
24+
# if true, then cost_infos ro cnt_infos should have TT_USE_TIME for their term_type
25+
convex_solver: 1 # which convex solver to use
26+
# 1 = AUTO_SOLVER, 2 = BPMPD, 3 = OSQP, 4 = QPOASES, 5 = GUROBI
27+
28+
init_info:
29+
type: 3 # 1 = STATIONARY, 2 = JOINT_INTERPOLATED, 3 = GIVEN_TRAJ
30+
# request.start_state should be provided for JOINT_INTERPOLATED and GIVEN_TRAJ
31+
dt: 0.5
32+
33+
joint_pos_term_info:
34+
start_pos: # from req.start_state
35+
name: start_pos
36+
first_timestep: 10 # First time step to which the term is applied.
37+
last_timestep: 10 # Last time step to which the term is applied.
38+
# if start_fixed is trure then we can not set the first_timestep to 0 for start_pos as it is going
39+
# to be ignored and only the current pose would be the constraint at timestep = 0.
40+
term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME
41+
middle_pos:
42+
name: middle_pos
43+
first_timestep: 15
44+
last_timestep: 15
45+
term_type: 2
46+
goal_pos:
47+
name: goal_pos
48+
first_timestep: 19
49+
last_timestep: 19
50+
term_type: 2
51+
goal_tmp: # If the constraint from request does not have any name, for example when using MotionPlanning Display in Rviz,
52+
# goal_tmp is assigned as the name of the constraint.
53+
# In this case: start_fixed = false and start_pos should be applied at timestep 0
54+
# OR: start_fixed = true and start_pos can be applies at any timestep
55+
name: goal_tmp
56+
first_timestep: 19 # n_steps - 1
57+
last_timestep: 19 # n_steps - 1
58+
term_type: 2

‎launch/demo_chomp.launch‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
<launch>
2+
<include file="$(dirname)/demo.launch">
3+
<arg name="pipeline" value="chomp"/>
4+
</include>
5+
</launch>

‎launch/demo_lerp.launch‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
2+
<launch>
3+
<include file="$(dirname)/demo.launch">
4+
<arg name="pipeline" value="lerp"/>
5+
</include>
6+
</launch>

‎launch/demo_stomp.launch‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
<launch>
2+
<include file="$(dirname)/demo.launch">
3+
<arg name="pipeline" value="stomp"/>
4+
</include>
5+
</launch>

‎launch/lerp_planning_pipeline.launch.xml‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<launch>
2+
3+
<!-- LERP Plugin for MoveIt! -->
4+
<arg name="planning_plugin" value="lerp_interface/LERPPlanner" />
5+
6+
<!-- The request adapters (plugins) used when planning with TrajOpt.
7+
ORDER MATTERS -->
8+
<arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
9+
default_planner_request_adapters/FixWorkspaceBounds
10+
default_planner_request_adapters/FixStartStateBounds
11+
default_planner_request_adapters/FixStartStateCollision
12+
default_planner_request_adapters/FixStartStatePathConstraints" />
13+
14+
<arg name="start_state_max_bounds_error" value="0.1" />
15+
16+
<param name="planning_plugin" value="$(arg planning_plugin)" />
17+
<param name="request_adapters" value="$(arg planning_adapters)" />
18+
<param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
19+
20+
<rosparam command="load" file="$(find panda_moveit_config)/config/lerp_planning.yaml"/>
21+
22+
</launch>

‎launch/ompl-chomp_planning_pipeline.launch.xml‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
<launch>
2+
<!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
3+
<include file="$(find panda_moveit_config)/launch/ompl_planning_pipeline.launch.xml">
4+
<arg name="planning_adapters" value="
5+
default_planner_request_adapters/AddTimeParameterization
6+
default_planner_request_adapters/FixWorkspaceBounds
7+
default_planner_request_adapters/FixStartStateBounds
8+
default_planner_request_adapters/FixStartStateCollision
9+
default_planner_request_adapters/FixStartStatePathConstraints
10+
chomp/OptimizerAdapter"
11+
/>
12+
</include>
13+
14+
<!-- load chomp config -->
15+
<rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
16+
17+
<!-- override trajectory_initialization_method -->
18+
<param name="trajectory_initialization_method" value="fillTrajectory"/>
19+
</launch>

‎launch/run_benchmark_trajopt.launch‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<launch>
2+
3+
<!-- This argument must specify the list of .cfg files to process for benchmarking -->
4+
<arg name="cfg" />
5+
6+
<!-- Load URDF -->
7+
<include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
8+
<arg name="load_robot_description" value="true"/>
9+
</include>
10+
11+
<!-- Start the database -->
12+
<include file="$(find moveit_resources_panda_moveit_config)/launch/warehouse.launch">
13+
<arg name="moveit_warehouse_database_path" value="moveit_trajopt_benchmark_warehouse"/>
14+
</include>
15+
16+
<!-- Start Benchmark Executable -->
17+
<node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
18+
<rosparam command="load" file="$(find moveit_resources_panda_moveit_config)/config/trajopt_planning.yaml"/>
19+
</node>
20+
21+
</launch>

0 commit comments

Comments
 (0)