Utilize parameter in TrajOpt planner

Author: Levi Armstrong

tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp

@@ -140,6 +140,9 @@ DefaultTrajoptProblemGenerator(const std::string& name,
   {
     const Eigen::VectorXd& position = getJointPosition(start_waypoint);
     start_plan_profile->apply(*pci, position, *start_instruction, composite_mi, active_links, index);
+
+    // Add to fixed indices
+    fixed_steps.push_back(index);
   }
   else
   {
@@ -267,6 +270,9 @@ DefaultTrajoptProblemGenerator(const std::string& name,
           // Add final point with waypoint
           cur_plan_profile->apply(*pci, cur_position, *plan_instruction, composite_mi, active_links, index);
 
+          // Add to fixed indices
+          fixed_steps.push_back(index);
+
           // Add seed state
           assert(isMoveInstruction(seed_composite->back()));
           const auto* seed_instruction = seed_composite->back().cast_const<MoveInstruction>();
@@ -327,9 +333,6 @@ DefaultTrajoptProblemGenerator(const std::string& name,
           /** @todo Should check that the joint names match the order of the manipulator */
           cur_plan_profile->apply(*pci, *cur_wp, *plan_instruction, composite_mi, active_links, index);
 
-          // Add to fixed indices
-          fixed_steps.push_back(index);
-
           // Add seed state
           assert(isMoveInstruction(seed_composite->back()));
           const auto* seed_instruction = seed_composite->back().cast_const<MoveInstruction>();
@@ -358,9 +361,12 @@ DefaultTrajoptProblemGenerator(const std::string& name,
   // Setup Basic Info
   pci->basic_info.n_steps = index;
   pci->basic_info.manip = composite_mi.manipulator;
-  pci->basic_info.start_fixed = false;
   pci->basic_info.use_time = false;
 
+  // Add the fixed timesteps. TrajOpt will constrain the optimization such that any costs applied at these timesteps
+  // will be ignored. Costs applied to variables at fixed timesteps generally causes solver failures
+  pci->basic_info.fixed_timesteps = fixed_steps;
+
   // Set trajopt seed
   assert(static_cast<long>(seed_states.size()) == pci->basic_info.n_steps);
   pci->init_info.type = trajopt::InitInfo::GIVEN_TRAJ;

tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/fix_state_collision_process_generator.cpp

@@ -125,7 +125,6 @@ bool MoveWaypointFromCollisionTrajopt(Waypoint& waypoint,
   ProblemConstructionInfo pci(input.env);
   pci.basic_info.n_steps = 1;
   pci.basic_info.manip = input.manip_info.manipulator;
-  pci.basic_info.start_fixed = false;
   pci.basic_info.use_time = false;
 
   // Create Kinematic Object

tesseract_python/tesseract_python/swig/trajopt/problem_description.i

@@ -176,11 +176,10 @@ struct TrajOptResult
 
 struct BasicInfo
 {  
-  bool start_fixed;
   int n_steps;
   std::string manip;
-  std::string robot;
-  IntVec dofs_fixed;
+  IntVec fixed_timesteps;
+  IntVec fixed_dofs;
   sco::ModelType convex_solver;
   bool use_time;
   double dt_upper_lim;