From 45bc2c7e1e20bba4d8a7f332f89ee12e0af09e0c Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Thu, 14 Dec 2023 14:41:30 +0100 Subject: [PATCH 1/4] Add cartesian waypoints to fixed indices --- .../trajopt/src/trajopt_motion_planner.cpp | 4 +++- .../trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index be88f59cb34..e0547e9caa8 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -288,7 +288,9 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const seed_states.push_back(request.env_state.getJointValues(joint_names)); } - /** @todo If fixed cartesian and not term_type cost add as fixed */ + // Add to fixed indices + if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ + fixed_steps.push_back(i); } 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 52dc9122803..3bfe85ab04b 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 @@ -299,7 +299,9 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // Apply profile cur_plan_profile->apply(*problem, cwp, move_instruction, composite_mi, active_links, i); - /** @todo If fixed cartesian and not term_type cost add as fixed */ + // Add to fixed indices + if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ + fixed_steps.push_back(i); } else if (move_instruction.getWaypoint().isJointWaypoint()) { From eb9487889f16baf35772dcf475237b709e7e703e Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 15 Dec 2023 16:27:29 +0100 Subject: [PATCH 2/4] Add isFixed...() methods to plan profiles --- .../trajopt/profile/trajopt_default_plan_profile.h | 3 +++ .../trajopt/profile/trajopt_profile.h | 3 +++ .../src/profile/trajopt_default_plan_profile.cpp | 10 ++++++++++ .../trajopt/src/trajopt_motion_planner.cpp | 4 ++-- .../profile/trajopt_ifopt_default_plan_profile.h | 3 +++ .../trajopt_ifopt/profile/trajopt_ifopt_profile.h | 3 +++ .../src/profile/trajopt_ifopt_default_plan_profile.cpp | 10 ++++++++++ .../trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp | 4 ++-- 8 files changed, 36 insertions(+), 4 deletions(-) diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h index 83bcdf96710..9ea5de63240 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h @@ -92,6 +92,9 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile const std::vector& active_links, int index) const override; + bool isFixedCartesian() const override; + bool isFixedJoint() const override; + tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; protected: diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h index 3bfe558b05a..2b4430365c4 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h @@ -71,6 +71,9 @@ class TrajOptPlanProfile const std::vector& active_links, int index) const = 0; + virtual bool isFixedCartesian() const = 0; + virtual bool isFixedJoint() const = 0; + virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; }; diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp index e2cc8a29a64..f0eb894bff5 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp @@ -280,6 +280,16 @@ void TrajOptDefaultPlanProfile::addConstraintErrorFunctions(trajopt::ProblemCons } } +bool TrajOptDefaultPlanProfile::isFixedCartesian() const { + // If the term type is constraint and all coefficients are non-zero + return (term_type == trajopt::TermType::TT_CNT) && (abs(cartesian_coeff.array()) >= std::numeric_limits::epsilon()).all(); +} + +bool TrajOptDefaultPlanProfile::isFixedJoint() const { + // If the term type is constraint and all coefficients are non-zero + return (term_type == trajopt::TermType::TT_CNT) && (abs(joint_coeff.array()) >= std::numeric_limits::epsilon()).all(); +} + tinyxml2::XMLElement* TrajOptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const { tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner"); diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index e0547e9caa8..6dc0f16b238 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -289,7 +289,7 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const } // Add to fixed indices - if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ + if (!cwp.isToleranced() && cur_plan_profile->isFixedCartesian()) fixed_steps.push_back(i); } else if (move_instruction.getWaypoint().isJointWaypoint()) @@ -302,7 +302,7 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const cur_plan_profile->apply(*pci, jwp, move_instruction, composite_mi, active_links, i); // Add to fixed indices - if (!jwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ + if (!jwp.isToleranced() && cur_plan_profile->isFixedJoint()) fixed_steps.push_back(i); } diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h index 9676c365dd7..6767fbcc2ed 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h @@ -64,6 +64,9 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile const std::vector& active_links, int index) const override; + bool isFixedCartesian() const override; + bool isFixedJoint() const override; + tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; }; } // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h index 35b62e19826..4706aee5d5f 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h @@ -73,6 +73,9 @@ class TrajOptIfoptPlanProfile const std::vector& active_links, int index) const = 0; + virtual bool isFixedCartesian() const = 0; + virtual bool isFixedJoint() const = 0; + virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; }; diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp index 4283be0dd5e..fed3e0811f5 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp @@ -127,6 +127,16 @@ void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem, } } +bool TrajOptIfoptDefaultPlanProfile::isFixedCartesian() const { + // If the term type is constraint and all coefficients are non-zero + return (term_type == TrajOptIfoptTermType::CONSTRAINT) && (abs(cartesian_coeff.array()) >= std::numeric_limits::epsilon()).all(); +} + +bool TrajOptIfoptDefaultPlanProfile::isFixedJoint() const { + // If the term type is constraint and all coefficients are non-zero + return (term_type == TrajOptIfoptTermType::CONSTRAINT) && (abs(joint_coeff.array()) >= std::numeric_limits::epsilon()).all(); +} + tinyxml2::XMLElement* TrajOptIfoptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& /*doc*/) const { throw std::runtime_error("TrajOptIfoptDefaultPlanProfile::toXML is not implemented!"); 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 3bfe85ab04b..021a5a0a6fa 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 @@ -300,7 +300,7 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co 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 */ + if (!cwp.isToleranced() && cur_plan_profile->isFixedCartesian()) fixed_steps.push_back(i); } else if (move_instruction.getWaypoint().isJointWaypoint()) @@ -322,7 +322,7 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co cur_plan_profile->apply(*problem, jwp, move_instruction, composite_mi, active_links, i); // Add to fixed indices - if (!jwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ + if (!jwp.isToleranced() && cur_plan_profile->isFixedJoint()) fixed_steps.push_back(i); } } From 337319871ea221230958b850428866f168602926 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 15 Dec 2023 16:35:51 +0100 Subject: [PATCH 3/4] clang-format --- .../src/profile/trajopt_default_plan_profile.cpp | 12 ++++++++---- .../profile/trajopt_ifopt_default_plan_profile.cpp | 12 ++++++++---- 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp index f0eb894bff5..826ebf76fdd 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp @@ -280,14 +280,18 @@ void TrajOptDefaultPlanProfile::addConstraintErrorFunctions(trajopt::ProblemCons } } -bool TrajOptDefaultPlanProfile::isFixedCartesian() const { +bool TrajOptDefaultPlanProfile::isFixedCartesian() const +{ // If the term type is constraint and all coefficients are non-zero - return (term_type == trajopt::TermType::TT_CNT) && (abs(cartesian_coeff.array()) >= std::numeric_limits::epsilon()).all(); + return (term_type == trajopt::TermType::TT_CNT) && + (abs(cartesian_coeff.array()) >= std::numeric_limits::epsilon()).all(); } -bool TrajOptDefaultPlanProfile::isFixedJoint() const { +bool TrajOptDefaultPlanProfile::isFixedJoint() const +{ // If the term type is constraint and all coefficients are non-zero - return (term_type == trajopt::TermType::TT_CNT) && (abs(joint_coeff.array()) >= std::numeric_limits::epsilon()).all(); + return (term_type == trajopt::TermType::TT_CNT) && + (abs(joint_coeff.array()) >= std::numeric_limits::epsilon()).all(); } tinyxml2::XMLElement* TrajOptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp index fed3e0811f5..e883ae1d2ce 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp @@ -127,14 +127,18 @@ void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem, } } -bool TrajOptIfoptDefaultPlanProfile::isFixedCartesian() const { +bool TrajOptIfoptDefaultPlanProfile::isFixedCartesian() const +{ // If the term type is constraint and all coefficients are non-zero - return (term_type == TrajOptIfoptTermType::CONSTRAINT) && (abs(cartesian_coeff.array()) >= std::numeric_limits::epsilon()).all(); + return (term_type == TrajOptIfoptTermType::CONSTRAINT) && + (abs(cartesian_coeff.array()) >= std::numeric_limits::epsilon()).all(); } -bool TrajOptIfoptDefaultPlanProfile::isFixedJoint() const { +bool TrajOptIfoptDefaultPlanProfile::isFixedJoint() const +{ // If the term type is constraint and all coefficients are non-zero - return (term_type == TrajOptIfoptTermType::CONSTRAINT) && (abs(joint_coeff.array()) >= std::numeric_limits::epsilon()).all(); + return (term_type == TrajOptIfoptTermType::CONSTRAINT) && + (abs(joint_coeff.array()) >= std::numeric_limits::epsilon()).all(); } tinyxml2::XMLElement* TrajOptIfoptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& /*doc*/) const From 5cf4ff3888fa1d495c0775ddd229eb3c6f4b40f1 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 8 Jan 2024 11:51:35 +0100 Subject: [PATCH 4/4] Removed fixed cartesian logic --- .../trajopt/profile/trajopt_default_plan_profile.h | 1 - .../trajopt/profile/trajopt_profile.h | 1 - .../trajopt/src/profile/trajopt_default_plan_profile.cpp | 7 ------- .../trajopt/src/trajopt_motion_planner.cpp | 4 +--- .../profile/trajopt_ifopt_default_plan_profile.h | 1 - .../trajopt_ifopt/profile/trajopt_ifopt_profile.h | 1 - .../src/profile/trajopt_ifopt_default_plan_profile.cpp | 7 ------- .../trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp | 4 +--- 8 files changed, 2 insertions(+), 24 deletions(-) diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h index 9ea5de63240..414c3945bbd 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h @@ -92,7 +92,6 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile const std::vector& active_links, int index) const override; - bool isFixedCartesian() const override; bool isFixedJoint() const override; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h index 2b4430365c4..7207d08f5f9 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h @@ -71,7 +71,6 @@ class TrajOptPlanProfile const std::vector& active_links, int index) const = 0; - virtual bool isFixedCartesian() const = 0; virtual bool isFixedJoint() const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp index 826ebf76fdd..9ab471a5f68 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp @@ -280,13 +280,6 @@ void TrajOptDefaultPlanProfile::addConstraintErrorFunctions(trajopt::ProblemCons } } -bool TrajOptDefaultPlanProfile::isFixedCartesian() const -{ - // If the term type is constraint and all coefficients are non-zero - return (term_type == trajopt::TermType::TT_CNT) && - (abs(cartesian_coeff.array()) >= std::numeric_limits::epsilon()).all(); -} - bool TrajOptDefaultPlanProfile::isFixedJoint() const { // If the term type is constraint and all coefficients are non-zero diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 6dc0f16b238..59564b4d543 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -288,9 +288,7 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const seed_states.push_back(request.env_state.getJointValues(joint_names)); } - // Add to fixed indices - if (!cwp.isToleranced() && cur_plan_profile->isFixedCartesian()) - 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/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h index 6767fbcc2ed..b7bf19244b9 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h @@ -64,7 +64,6 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile const std::vector& active_links, int index) const override; - bool isFixedCartesian() const override; bool isFixedJoint() const override; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h index 4706aee5d5f..42888800dce 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h @@ -73,7 +73,6 @@ class TrajOptIfoptPlanProfile const std::vector& active_links, int index) const = 0; - virtual bool isFixedCartesian() const = 0; virtual bool isFixedJoint() const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp index e883ae1d2ce..fa46c4b59ff 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp @@ -127,13 +127,6 @@ void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem, } } -bool TrajOptIfoptDefaultPlanProfile::isFixedCartesian() const -{ - // If the term type is constraint and all coefficients are non-zero - return (term_type == TrajOptIfoptTermType::CONSTRAINT) && - (abs(cartesian_coeff.array()) >= std::numeric_limits::epsilon()).all(); -} - bool TrajOptIfoptDefaultPlanProfile::isFixedJoint() const { // If the term type is constraint and all coefficients are non-zero 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 021a5a0a6fa..be9e84a58d4 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 @@ -299,9 +299,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() && cur_plan_profile->isFixedCartesian()) - fixed_steps.push_back(i); + /** @todo If fixed cartesian and not term_type cost add as fixed */ } else if (move_instruction.getWaypoint().isJointWaypoint()) {