Merge MSA fixups and addons

config/chomp_planning.yaml

@@ -7,12 +7,12 @@ learning_rate: 0.01
 smoothness_cost_velocity: 0.0
 smoothness_cost_acceleration: 1.0
 smoothness_cost_jerk: 0.0
-ridge_factor: 0.01
+ridge_factor: 0.0
 use_pseudo_inverse: false
 pseudo_inverse_ridge_factor: 1e-4
 joint_update_limit: 0.1
 collision_clearance: 0.2
 collision_threshold: 0.07
 use_stochastic_descent: true
-enable_failure_recovery: true
-max_recovery_attempts: 5
\ No newline at end of file
+enable_failure_recovery: false
+max_recovery_attempts: 5

config/lerp_planning.yaml

+num_steps: 40

config/stomp_planning.yaml

+
+stomp/panda_arm:
+  group_name: panda_arm
+  optimization:
+    num_timesteps: 60
+    num_iterations: 40
+    num_iterations_after_valid: 0
+    num_rollouts: 30
+    max_rollouts: 30
+    initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]
+    control_cost_weight: 0.0
+  task:
+    noise_generator:
+      - class: stomp_moveit/NormalDistributionSampling
+        stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4, 0.4]
+    cost_functions:
+      - class: stomp_moveit/CollisionCheck
+        collision_penalty: 1.0
+        cost_weight: 1.0
+        kernel_window_percentage: 0.2
+        longest_valid_joint_move: 0.05
+    noisy_filters:
+      - class: stomp_moveit/JointLimits
+        lock_start: True
+        lock_goal: True
+      - class: stomp_moveit/MultiTrajectoryVisualization
+        line_width: 0.02
+        rgb: [255, 255, 0]
+        marker_array_topic: stomp_trajectories
+        marker_namespace: noisy
+    update_filters:
+      - class: stomp_moveit/PolynomialSmoother
+        poly_order: 6
+      - class: stomp_moveit/TrajectoryVisualization
+        line_width: 0.05
+        rgb: [0, 191, 255]
+        error_rgb: [255, 0, 0]
+        publish_intermediate: True
+        marker_topic: stomp_trajectory
+        marker_namespace: optimized
\ No newline at end of file

config/trajopt_planning.yaml

+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

launch/demo_chomp.launch

+<launch>
+  <include file="$(dirname)/demo.launch">
+    <arg name="pipeline" value="chomp"/>
+  </include>
+</launch>

launch/demo_lerp.launch

+
+<launch>
+  <include file="$(dirname)/demo.launch">
+    <arg name="pipeline" value="lerp"/>
+  </include>
+</launch>
\ No newline at end of file

launch/demo_stomp.launch

+<launch>
+  <include file="$(dirname)/demo.launch">
+    <arg name="pipeline" value="stomp"/>
+  </include>
+</launch>

launch/lerp_planning_pipeline.launch.xml

+<launch>
+
+  <!-- LERP Plugin for MoveIt! -->
+  <arg name="planning_plugin" value="lerp_interface/LERPPlanner" />
+
+  <!-- The request adapters (plugins) used when planning with TrajOpt.
+       ORDER MATTERS -->
+  <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" />
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+
+  <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)" />
+
+  <rosparam command="load" file="$(find panda_moveit_config)/config/lerp_planning.yaml"/>
+
+</launch>

launch/ompl-chomp_planning_pipeline.launch.xml

+<launch>
+  <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
+  <include file="$(find panda_moveit_config)/launch/ompl_planning_pipeline.launch.xml">
+    <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
+       chomp/OptimizerAdapter"
+       />
+  </include>
+
+  <!-- load chomp config -->
+  <rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
+
+  <!-- override trajectory_initialization_method -->
+  <param name="trajectory_initialization_method" value="fillTrajectory"/>
+</launch>

launch/run_benchmark_trajopt.launch

+<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/trajopt_planning.yaml"/>
+  </node>
+
+</launch>

launch/simple_moveit_controller_manager.launch.xml

@@ -3,5 +3,6 @@
   <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
 
   <!-- Load controller list to the parameter server -->
+  <rosparam file="$(find panda_moveit_config)/config/simple_moveit_controllers.yaml" />
   <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml" />
 </launch>

launch/stomp_planning_pipeline.launch.xml

+<launch>
+  <!-- Stomp Plugin for MoveIt! -->
+  <arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+  <arg name="jiggle_fraction" value="0.05" />
+  <!-- The request adapters (plugins) used when planning.
+       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" />
+
+
+  <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)" />
+
+  <!-- Add MoveGroup capabilities specific to this pipeline -->
+  <!-- <param name="capabilities" value="" /> -->
+
+  <rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml"/>
+</launch>

launch/trajopt_planning_pipeline.launch.xml

+<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>