diff --git a/config/chomp_planning.yaml b/config/chomp_planning.yaml index c14a673e85a300e2ac5c4c7bc6d8ab389c19d440..eb9c91225b1db9db1f5b5f7e1d8ff51191a311e8 100644 --- a/config/chomp_planning.yaml +++ b/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 diff --git a/config/lerp_planning.yaml b/config/lerp_planning.yaml new file mode 100644 index 0000000000000000000000000000000000000000..9d2eebd53d2cb926337bf6fcee6c80625a6c0b59 --- /dev/null +++ b/config/lerp_planning.yaml @@ -0,0 +1 @@ +num_steps: 40 diff --git a/config/stomp_planning.yaml b/config/stomp_planning.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d6d34365efff05644420676a2f9c79f9cdaf1677 --- /dev/null +++ b/config/stomp_planning.yaml @@ -0,0 +1,40 @@ + +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 diff --git a/config/trajopt_planning.yaml b/config/trajopt_planning.yaml new file mode 100644 index 0000000000000000000000000000000000000000..6c6ea43612ea32b235083282684f52fccf66a44c --- /dev/null +++ b/config/trajopt_planning.yaml @@ -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 diff --git a/launch/demo_chomp.launch b/launch/demo_chomp.launch new file mode 100644 index 0000000000000000000000000000000000000000..a3051a809fd8e46744ee7f9ae765f7d4fa682e97 --- /dev/null +++ b/launch/demo_chomp.launch @@ -0,0 +1,5 @@ +<launch> + <include file="$(dirname)/demo.launch"> + <arg name="pipeline" value="chomp"/> + </include> +</launch> diff --git a/launch/demo_lerp.launch b/launch/demo_lerp.launch new file mode 100644 index 0000000000000000000000000000000000000000..d5d8355417f21e50eb700009f804a61f7d2391a1 --- /dev/null +++ b/launch/demo_lerp.launch @@ -0,0 +1,6 @@ + +<launch> + <include file="$(dirname)/demo.launch"> + <arg name="pipeline" value="lerp"/> + </include> +</launch> \ No newline at end of file diff --git a/launch/demo_stomp.launch b/launch/demo_stomp.launch new file mode 100644 index 0000000000000000000000000000000000000000..0ae05517da11fc70561bed8e6c1a8a29ad0a3dee --- /dev/null +++ b/launch/demo_stomp.launch @@ -0,0 +1,5 @@ +<launch> + <include file="$(dirname)/demo.launch"> + <arg name="pipeline" value="stomp"/> + </include> +</launch> diff --git a/launch/lerp_planning_pipeline.launch.xml b/launch/lerp_planning_pipeline.launch.xml new file mode 100644 index 0000000000000000000000000000000000000000..c1193577e5627d2058b8ef5aeda8808384d3e7a0 --- /dev/null +++ b/launch/lerp_planning_pipeline.launch.xml @@ -0,0 +1,22 @@ +<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> diff --git a/launch/ompl-chomp_planning_pipeline.launch.xml b/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000000000000000000000000000000000000..178d2bef7acc8ffdaa0de8317d567c1ab9b79be3 --- /dev/null +++ b/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,19 @@ +<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> diff --git a/launch/run_benchmark_trajopt.launch b/launch/run_benchmark_trajopt.launch new file mode 100644 index 0000000000000000000000000000000000000000..762841632186b2729e2010ad1f8ed29fa9fa498e --- /dev/null +++ b/launch/run_benchmark_trajopt.launch @@ -0,0 +1,21 @@ +<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> diff --git a/launch/simple_moveit_controller_manager.launch.xml b/launch/simple_moveit_controller_manager.launch.xml index a9dff1e781a58b0fe9da09c264fd59a741846090..5b18aca3b68a510e5f46a4e0f6dd8896afae028e 100644 --- a/launch/simple_moveit_controller_manager.launch.xml +++ b/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> diff --git a/launch/stomp_planning_pipeline.launch.xml b/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000000000000000000000000000000000000..06b42dc0efeefc4dc180b99dfa825d5de0c6afac --- /dev/null +++ b/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,25 @@ +<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> diff --git a/launch/trajopt_planning_pipeline.launch.xml b/launch/trajopt_planning_pipeline.launch.xml new file mode 100644 index 0000000000000000000000000000000000000000..ec9ea9b121822124842dff8aca20522ef80a7311 --- /dev/null +++ b/launch/trajopt_planning_pipeline.launch.xml @@ -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>