From 3c0255ebfc0b24d9a4bcd1f180e2b260ccb0d633 Mon Sep 17 00:00:00 2001
From: Michael Ripperger <michael.ripperger@swri.org>
Date: Thu, 31 Dec 2020 16:11:47 -0600
Subject: [PATCH] Added fixed timesteps to TrajOpt config

---
 .../config/trajopt_planner_default_config.cpp      | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/config/trajopt_planner_default_config.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/config/trajopt_planner_default_config.cpp
index b8664d59e97..a11e444991c 100644
--- a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/config/trajopt_planner_default_config.cpp
+++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/config/trajopt_planner_default_config.cpp
@@ -64,6 +64,20 @@ std::shared_ptr<trajopt::ProblemConstructionInfo> TrajOptPlannerDefaultConfig::g
   std::vector<int> fixed_steps;
   addWaypoints(pci, fixed_steps);
 
+  // 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
+  for (auto idx : fixed_steps)
+  {
+    if (idx == 0)
+    {
+      pci.basic_info.start_fixed = true;
+    }
+    else
+    {
+      pci.basic_info.fixed_timesteps.push_back(idx);
+    }
+  }
+
   if (collision_constraint_config.enabled)
     addCollisionConstraint(pci, fixed_steps);