From ee0e83ae551d4c13942a97a140c98ff665df4dd0 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Thu, 21 Dec 2023 08:01:53 +0100 Subject: [PATCH] Revert "Add cartesian waypoints to fixed indices" This reverts commit e55888e7cb45b403ffc9d1d7d673c9a606f6573e. --- .../trajopt/src/trajopt_motion_planner.cpp | 4 +--- .../trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp | 4 +--- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index ec15dc41bc2..13905c9d6d0 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -273,9 +273,7 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const seed_states.push_back(request.env_state.getJointValues(joint_names)); } - // Add to fixed indices - if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ - fixed_steps.push_back(i); + /** @todo If fixed cartesian and not term_type cost add as fixed */ } else if (move_instruction.getWaypoint().isJointWaypoint()) { diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index d4b1dd578e3..3e533872dfa 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp @@ -314,9 +314,7 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // Apply profile cur_plan_profile->apply(*problem, cwp, move_instruction, composite_mi, active_links, i); - // Add to fixed indices - if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ - fixed_steps.push_back(i); + /** @todo If fixed cartesian and not term_type cost add as fixed */ } else if (move_instruction.getWaypoint().isJointWaypoint()) {