From e9d5771cece88cf011476aae138a02c69a11dbc0 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 8 Jan 2024 11:51:35 +0100 Subject: [PATCH] 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 46e74b13c6b..926a93e19e7 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 @@ -298,9 +298,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()) {