Skip to content

Commit

Permalink
Removed fixed cartesian logic
Browse files Browse the repository at this point in the history
  • Loading branch information
rjoomen committed Feb 23, 2024
1 parent 18bf735 commit aee246d
Show file tree
Hide file tree
Showing 8 changed files with 2 additions and 24 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,6 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
const std::vector<std::string>& active_links,
int index) const override;

bool isFixedCartesian() const override;
bool isFixedJoint() const override;

tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,6 @@ class TrajOptPlanProfile
const std::vector<std::string>& 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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -211,13 +211,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<double>::epsilon()).all();
}

bool TrajOptDefaultPlanProfile::isFixedJoint() const
{
// If the term type is constraint and all coefficients are non-zero
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -276,9 +276,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())
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,6 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile
const std::vector<std::string>& active_links,
int index) const override;

bool isFixedCartesian() const override;
bool isFixedJoint() const override;

tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,6 @@ class TrajOptIfoptPlanProfile
const std::vector<std::string>& 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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -138,13 +138,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<double>::epsilon()).all();
}

bool TrajOptIfoptDefaultPlanProfile::isFixedJoint() const
{
// If the term type is constraint and all coefficients are non-zero
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -290,9 +290,7 @@ std::shared_ptr<TrajOptIfoptProblem> 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())
{
Expand Down

0 comments on commit aee246d

Please sign in to comment.