From 0b7ac4e6ea147003fd01d0b51723452ff320a5ea Mon Sep 17 00:00:00 2001
From: Omid Heidari <omid@picknik.ai>
Date: Wed, 7 Aug 2019 14:53:18 -0600
Subject: [PATCH] Add TrajOpt launch files to panda_moveit_config (#29)
* added new files for emptyplan
* edited demo.launch to accept planner arg
* Added emptyplan_planning.yaml so it can be used for new planners. It is an empty file.
Added rosparam in emptyplan_planning_pipeline.launch.xml to load its yaml file
* from empty to trajopt
* modified some comment
* renamed and edited the context of files from empty to trajopt
* removed move_group_trajop.launch, we do not need this file
* applied Dave's comments
* restored setup_assistant
* added trajopt params in yaml file
removed extra planner arg in demo.launch
* addressed the comments
* corrected pipeline in move_group
---
config/trajopt_planning.yaml | 58 +++++++++++++++++++++
launch/demo.launch | 5 +-
launch/move_group.launch | 4 +-
launch/run_benchmark_trajopt.launch | 23 ++++++++
launch/trajopt_planning_pipeline.launch.xml | 22 ++++++++
5 files changed, 107 insertions(+), 5 deletions(-)
create mode 100644 config/trajopt_planning.yaml
create mode 100644 launch/run_benchmark_trajopt.launch
create mode 100644 launch/trajopt_planning_pipeline.launch.xml
diff --git a/config/trajopt_planning.yaml b/config/trajopt_planning.yaml
new file mode 100644
index 0000000..6c6ea43
--- /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.launch b/launch/demo.launch
index 66f1853..2ebb63b 100644
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -1,7 +1,5 @@
<launch>
- <arg name="planner" default="ompl" />
-
<!-- By default, we do not start a database (it can be large) -->
<arg name="db" default="false" />
<!-- Allow user to specify database location -->
@@ -9,6 +7,7 @@
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
+ <arg name="pipeline" default="ompl" />
<!--
By default, hide joint_state_publisher's GUI
@@ -41,11 +40,11 @@
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find panda_moveit_config)/launch/move_group.launch">
- <arg name="planner" value="$(arg planner)" />
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
+ <arg name="pipeline" value="$(arg pipeline)" />
</include>
<!-- Run Rviz -->
diff --git a/launch/move_group.launch b/launch/move_group.launch
index ccc7150..f5f9e7d 100644
--- a/launch/move_group.launch
+++ b/launch/move_group.launch
@@ -6,7 +6,7 @@
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
- <arg name="planner" default="ompl" />
+ <arg name="pipeline" default="ompl" />
<!-- GDB Debug Option -->
<arg name="debug" default="false" />
@@ -28,7 +28,7 @@
<!-- Planning Functionality -->
<include ns="move_group" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
- <arg name="pipeline" value="$(arg planner)" />
+ <arg name="pipeline" value="$(arg pipeline)" />
</include>
<!-- Trajectory Execution Functionality -->
diff --git a/launch/run_benchmark_trajopt.launch b/launch/run_benchmark_trajopt.launch
new file mode 100644
index 0000000..1103c03
--- /dev/null
+++ b/launch/run_benchmark_trajopt.launch
@@ -0,0 +1,23 @@
+<launch>
+
+ <!-- This argument must specify the list of .cfg files to process for benchmarking -->
+ <arg name="trajopt_config" />
+
+ <!-- Load URDF -->
+ <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+ <arg name="load_robot_description" value="true"/>
+ </include>
+
+ <!-- Start the database -->
+ <include file="$(find 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 trajopt_config) --benchmark-planners" respawn="false" output="screen">
+ <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+ <rosparam command="load" file="$(find panda_moveit_config)/config/trajopt_planning.yaml"/>
+ </node>
+
+</launch>
diff --git a/launch/trajopt_planning_pipeline.launch.xml b/launch/trajopt_planning_pipeline.launch.xml
new file mode 100644
index 0000000..cf57354
--- /dev/null
+++ b/launch/trajopt_planning_pipeline.launch.xml
@@ -0,0 +1,22 @@
+<launch>
+
+ <!-- TrajOpt Plugin for MoveIt! -->
+ <arg name="planning_plugin" value="trajopt_interface/TrajOptPlanner" />
+
+ <!-- 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/trajopt_planning.yaml"/>
+
+</launch>
--
GitLab