added trajopt params in yaml file

removed extra planner arg in demo.launch

config/panda_arm.xacro

@@ -21,7 +21,7 @@
  </group_state>
  <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
  <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
- <virtual_joint name="virtual_joint" type="floating" parent_frame="world" child_link="panda_link0" />
+ <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="panda_link0" />
  <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
  <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent" />
  <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never" />

config/trajopt_planning.yaml

@@ -0,0 +1,50 @@
+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: flase # 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

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 -->
@@ -42,7 +40,6 @@
 
  <!-- 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"/>