diff --git a/config/ompl_planning.yaml b/config/ompl_planning.yaml
index 6f8d2f7..be4fc45 100644
--- a/config/ompl_planning.yaml
+++ b/config/ompl_planning.yaml
@@ -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
@@ -153,6 +155,7 @@ panda_arm:
- LazyPRMstar
- SPARS
- SPARStwo
+ - TrajOpt
hand:
planner_configs:
- AnytimePathShortening
@@ -179,3 +182,4 @@ hand:
- LazyPRMstar
- SPARS
- SPARStwo
+ - TrajOpt
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/run_benchmark_trajopt.launch b/launch/run_benchmark_trajopt.launch
new file mode 100644
index 0000000..5063186
--- /dev/null
+++ b/launch/run_benchmark_trajopt.launch
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/trajopt_planning_pipeline.launch.xml b/launch/trajopt_planning_pipeline.launch.xml
new file mode 100644
index 0000000..ec9ea9b
--- /dev/null
+++ b/launch/trajopt_planning_pipeline.launch.xml
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+