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 May 30, 2024
1 parent 674fb1b commit e9d5771
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 @@ -92,7 +92,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 @@ -71,7 +71,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 @@ -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<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 @@ -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())
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,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 @@ -73,7 +73,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 @@ -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<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 @@ -298,9 +298,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 e9d5771

Please sign in to comment.