Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Draft: robot configuration vertex evaluator #35

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Draft: robot configuration vertex evaluator #35

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
50 changes: 47 additions & 3 deletions snp_motion_planning/src/planner_profiles.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,49 @@
#include <descartes_light/edge_evaluators/compound_edge_evaluator.h>
#include <descartes_light/edge_evaluators/euclidean_distance_edge_evaluator.h>
#include <tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h>
#include <tesseract_motion_planners/descartes/descartes_vertex_evaluator.h>
#include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
#include <tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h>

class RobotConfigurationVertexEvaluator : public tesseract_planning::DescartesVertexEvaluator
{
public:
using tesseract_planning::DescartesVertexEvaluator::DescartesVertexEvaluator;

bool operator()(const Eigen::Ref<const Eigen::VectorXd>& joint_values) const override
{
bool valid = true;
// Robot Front or Back
valid &= joint_values(0) > -M_PI / 2.0;
valid &= joint_values(0) < M_PI / 2.0;
// Elbow Up
valid &= joint_values(2) < M_PI / 2.0;
return valid;
}
};

class CompositeVertexEvaluator : public tesseract_planning::DescartesVertexEvaluator
{
public:
CompositeVertexEvaluator(std::vector<tesseract_planning::DescartesVertexEvaluator::Ptr>&& evaluators)
: evaluators_(std::move(evaluators))
{
}

bool operator()(const Eigen::Ref<const Eigen::VectorXd>& joint_values) const override
{
return std::all_of(evaluators_.begin(), evaluators_.end(),
[&joint_values](const tesseract_planning::DescartesVertexEvaluator::Ptr& ve) {
return ve->operator()(joint_values);
});
}

private:
const std::vector<tesseract_planning::DescartesVertexEvaluator::Ptr> evaluators_;
};

template <typename FloatType>
typename tesseract_planning::DescartesDefaultPlanProfile<FloatType>::Ptr createDescartesPlanProfile()
{
Expand Down Expand Up @@ -35,9 +73,15 @@ typename tesseract_planning::DescartesDefaultPlanProfile<FloatType>::Ptr createD
// std::make_shared<descartes_light::EuclideanDistanceEdgeEvaluator<FloatType>>(wrist_mask));

return eval;
};

profile->vertex_evaluator = nullptr;
};

profile->vertex_evaluator = [](const tesseract_planning::DescartesProblem<FloatType>& prob) {
std::vector<tesseract_planning::DescartesVertexEvaluator::Ptr> evaluators;
evaluators.push_back(std::make_shared<RobotConfigurationVertexEvaluator>());
evaluators.push_back(std::make_shared<tesseract_planning::DescartesJointLimitsVertexEvaluator>(
prob.manip->getLimits().joint_limits));
return std::make_shared<CompositeVertexEvaluator>(std::move(evaluators));
};

profile->target_pose_sampler =
std::bind(tesseract_planning::sampleToolZAxis, std::placeholders::_1, 10.0 * M_PI / 180.0);
Expand Down