Skip to content

Commit 9f85b1d

Browse files
mpowelsonLevi-Armstrong
mpowelson
authored and
Levi-Armstrong
committed
Update planners to use CollisionCheckConfig
1 parent 45579b4 commit 9f85b1d

File tree

11 files changed

+70
-61
lines changed

11 files changed

+70
-61
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/descartes_collision.h

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,15 +46,16 @@ class DescartesCollision : public descartes_light::CollisionInterface<FloatType>
4646
* @param collision_env The collision Environment
4747
* @param active_links The list of active links
4848
* @param joint_names The list of joint names in the order that the data will be provided to the validate function.
49-
* @param collision_safety_margin The minimum distance allowed from a collision object
49+
* @param edge_collision_check_config Config used to set up collision checking
5050
* @param longest_valid_segment_length Used to check collisions between two state if norm(state0-state1) >
5151
* longest_valid_segment_length.
5252
* @param debug If true, this print debug information to the terminal
5353
*/
5454
DescartesCollision(const tesseract_environment::Environment::ConstPtr& collision_env,
5555
std::vector<std::string> active_links,
5656
std::vector<std::string> joint_names,
57-
double collision_safety_margin = 0.025,
57+
tesseract_collision::CollisionCheckConfig collision_check_config =
58+
tesseract_collision::CollisionCheckConfig{ 0.025 },
5859
bool debug = false);
5960
~DescartesCollision() override = default;
6061

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_collision.hpp

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -41,16 +41,16 @@ template <typename FloatType>
4141
DescartesCollision<FloatType>::DescartesCollision(const tesseract_environment::Environment::ConstPtr& collision_env,
4242
std::vector<std::string> active_links,
4343
std::vector<std::string> joint_names,
44-
double collision_safety_margin,
44+
tesseract_collision::CollisionCheckConfig collision_check_config,
4545
bool debug)
4646
: state_solver_(collision_env->getStateSolver())
4747
, acm_(*(collision_env->getAllowedCollisionMatrix()))
4848
, active_link_names_(std::move(active_links))
4949
, joint_names_(std::move(joint_names))
5050
, contact_manager_(collision_env->getDiscreteContactManager())
51+
, collision_check_config_(std::move(collision_check_config))
5152
, debug_(debug)
5253
{
53-
collision_check_config_.collision_margin_data = tesseract_collision::CollisionMarginData(collision_safety_margin);
5454
contact_manager_->setActiveCollisionObjects(active_link_names_);
5555
contact_manager_->setCollisionMarginData(collision_check_config_.collision_margin_data);
5656
contact_manager_->setIsContactAllowedFn(
@@ -85,8 +85,9 @@ bool DescartesCollision<FloatType>::validate(const FloatType* pos, std::size_t s
8585
tesseract_environment::EnvState::Ptr env_state = state_solver_->getState(joint_names_, joint_angles);
8686

8787
std::vector<tesseract_collision::ContactResultMap> results;
88-
tesseract_collision::ContactRequest request(tesseract_collision::ContactTestType::FIRST);
89-
bool in_contact = checkTrajectoryState(results, *contact_manager_, env_state, collision_check_config_);
88+
tesseract_collision::CollisionCheckConfig config(collision_check_config_);
89+
config.contact_request.type = tesseract_collision::ContactTestType::FIRST;
90+
bool in_contact = checkTrajectoryState(results, *contact_manager_, env_state, config);
9091
return (!in_contact);
9192
}
9293

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp

Lines changed: 23 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -57,8 +57,9 @@ DescartesDefaultPlanProfile<FloatType>::DescartesDefaultPlanProfile(const tinyxm
5757
if (vertex_collisions_element)
5858
{
5959
const tinyxml2::XMLElement* enabled_element = vertex_collisions_element->FirstChildElement("Enabled");
60-
const tinyxml2::XMLElement* coll_safety_margin_element = vertex_collisions_element->FirstChildElement("CollisionSaf"
61-
"etyMargin");
60+
// const tinyxml2::XMLElement* coll_safety_margin_element =
61+
// vertex_collisions_element->FirstChildElement("CollisionSaf"
62+
// "etyMargin");
6263

6364
if (enabled_element)
6465
{
@@ -67,19 +68,22 @@ DescartesDefaultPlanProfile<FloatType>::DescartesDefaultPlanProfile(const tinyxm
6768
throw std::runtime_error("DescartesPlanProfile: VertexCollisions: Error parsing Enabled string");
6869
}
6970

70-
if (coll_safety_margin_element)
71-
{
72-
std::string coll_safety_margin_string;
73-
status = tesseract_common::QueryStringText(coll_safety_margin_element, coll_safety_margin_string);
74-
if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
75-
throw std::runtime_error("DescartesPlanProfile: VertexCollisions:: Error parsing CollisionSafetyMargin string");
76-
77-
if (!tesseract_common::isNumeric(coll_safety_margin_string))
78-
throw std::runtime_error("DescartesPlanProfile: VertexCollisions:: CollisionSafetyMargin is not a numeric "
79-
"values.");
80-
81-
tesseract_common::toNumeric<double>(coll_safety_margin_string, collision_safety_margin);
82-
}
71+
/** @todo Update XML */
72+
// if (coll_safety_margin_element)
73+
// {
74+
// std::string coll_safety_margin_string;
75+
// status = tesseract_common::QueryStringText(coll_safety_margin_element, coll_safety_margin_string);
76+
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
77+
// throw std::runtime_error("DescartesPlanProfile: VertexCollisions:: Error parsing CollisionSafetyMargin
78+
// string");
79+
80+
// if (!tesseract_common::isNumeric(coll_safety_margin_string))
81+
// throw std::runtime_error("DescartesPlanProfile: VertexCollisions:: CollisionSafetyMargin is not a numeric
82+
// "
83+
// "values.");
84+
85+
// tesseract_common::toNumeric<double>(coll_safety_margin_string, collision_safety_margin);
86+
// }
8387
}
8488

8589
if (edge_collisions_element)
@@ -181,7 +185,7 @@ void DescartesDefaultPlanProfile<FloatType>::apply(DescartesProblem<FloatType>&
181185
typename descartes_light::CollisionInterface<FloatType>::Ptr ci = nullptr;
182186
if (enable_collision)
183187
ci = std::make_shared<DescartesCollision<FloatType>>(
184-
prob.env, active_links, prob.manip_inv_kin->getJointNames(), collision_safety_margin, debug);
188+
prob.env, active_links, prob.manip_inv_kin->getJointNames(), vertex_collision_check_config, debug);
185189

186190
Eigen::Isometry3d manip_baselink_to_waypoint = Eigen::Isometry3d::Identity();
187191
if (it == active_links.end())
@@ -314,9 +318,9 @@ tinyxml2::XMLElement* DescartesDefaultPlanProfile<FloatType>::toXML(tinyxml2::XM
314318
vertex_collisions_enabled->SetText(enable_collision);
315319
vertex_collisions->InsertEndChild(vertex_collisions_enabled);
316320

317-
tinyxml2::XMLElement* vertex_collisions_safety_margin = doc.NewElement("CollisionSafetyMargin");
318-
vertex_collisions_safety_margin->SetText(collision_safety_margin);
319-
vertex_collisions->InsertEndChild(vertex_collisions_safety_margin);
321+
// tinyxml2::XMLElement* vertex_collisions_safety_margin = doc.NewElement("CollisionSafetyMargin");
322+
// vertex_collisions_safety_margin->SetText(collision_safety_margin);
323+
// vertex_collisions->InsertEndChild(vertex_collisions_safety_margin);
320324

321325
xml_descartes->InsertEndChild(vertex_collisions);
322326

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -72,11 +72,11 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile<FloatType>
7272

7373
// Applied to sampled states
7474
bool enable_collision{ true };
75-
double collision_safety_margin{ 0 };
75+
tesseract_collision::CollisionCheckConfig vertex_collision_check_config{ 0 };
7676

7777
// Applied during edge evaluation
7878
bool enable_edge_collision{ false };
79-
tesseract_collision::CollisionCheckConfig edge_collision_check_config;
79+
tesseract_collision::CollisionCheckConfig edge_collision_check_config{ 0 };
8080
int num_threads{ 1 };
8181
bool allow_collision{ false };
8282
bool debug{ false };

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/ompl/continuous_motion_validator.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
4747
ompl::base::StateValidityCheckerPtr state_validator,
4848
const tesseract_environment::Environment::ConstPtr& env,
4949
tesseract_kinematics::ForwardKinematics::ConstPtr kin,
50-
double collision_safety_margin,
50+
const tesseract_collision::CollisionMarginData& collision_margin_data,
5151
OMPLStateExtractor extractor);
5252

5353
bool checkMotion(const ompl::base::State* s1, const ompl::base::State* s2) const override;

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -105,12 +105,11 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile
105105
/** @brief If true, collision checking will be enabled. Default: true*/
106106
bool collision_check = true;
107107

108+
tesseract_collision::CollisionMarginData collision_margin_data;
109+
108110
/** @brief If true, use continuous collision checking */
109111
bool collision_continuous = false;
110112

111-
/** @brief Max distance over which collisions are checked */
112-
double collision_safety_margin = 0.00;
113-
114113
/** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
115114
* to be considered valid in post checking of trajectory returned by trajopt.
116115
*

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/ompl/state_collision_validator.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker
4545
StateCollisionValidator(const ompl::base::SpaceInformationPtr& space_info,
4646
tesseract_environment::Environment::ConstPtr env,
4747
tesseract_kinematics::ForwardKinematics::ConstPtr kin,
48-
double collision_safety_margin,
48+
const tesseract_collision::CollisionMarginData& collision_margin_data,
4949
OMPLStateExtractor extractor);
5050

5151
bool isValid(const ompl::base::State* state) const override;

‎tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/continuous_motion_validator.cpp

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -33,12 +33,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3333

3434
namespace tesseract_planning
3535
{
36-
ContinuousMotionValidator::ContinuousMotionValidator(const ompl::base::SpaceInformationPtr& space_info,
37-
ompl::base::StateValidityCheckerPtr state_validator,
38-
const tesseract_environment::Environment::ConstPtr& env,
39-
tesseract_kinematics::ForwardKinematics::ConstPtr kin,
40-
double collision_safety_margin,
41-
OMPLStateExtractor extractor)
36+
ContinuousMotionValidator::ContinuousMotionValidator(
37+
const ompl::base::SpaceInformationPtr& space_info,
38+
ompl::base::StateValidityCheckerPtr state_validator,
39+
const tesseract_environment::Environment::ConstPtr& env,
40+
tesseract_kinematics::ForwardKinematics::ConstPtr kin,
41+
const tesseract_collision::CollisionMarginData& collision_margin_data,
42+
OMPLStateExtractor extractor)
4243
: MotionValidator(space_info)
4344
, state_validator_(std::move(state_validator))
4445
, state_solver_(env->getStateSolver())
@@ -55,7 +56,7 @@ ContinuousMotionValidator::ContinuousMotionValidator(const ompl::base::SpaceInfo
5556
links_ = adj_map.getActiveLinkNames();
5657

5758
continuous_contact_manager_->setActiveCollisionObjects(links_);
58-
continuous_contact_manager_->setDefaultCollisionMarginData(collision_safety_margin);
59+
continuous_contact_manager_->setCollisionMarginData(collision_margin_data);
5960
}
6061

6162
bool ContinuousMotionValidator::checkMotion(const ompl::base::State* s1, const ompl::base::State* s2) const

‎tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/profile/ompl_default_plan_profile.cpp

Lines changed: 20 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,8 @@ OMPLDefaultPlanProfile::OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_e
5757
const tinyxml2::XMLElement* planners_element = xml_element.FirstChildElement("Planners");
5858
const tinyxml2::XMLElement* collision_check_element = xml_element.FirstChildElement("CollisionCheck");
5959
const tinyxml2::XMLElement* collision_continuous_element = xml_element.FirstChildElement("CollisionContinuous");
60-
const tinyxml2::XMLElement* collision_safety_margin_element = xml_element.FirstChildElement("CollisionSafetyMargin");
60+
// const tinyxml2::XMLElement* collision_safety_margin_element =
61+
// xml_element.FirstChildElement("CollisionSafetyMargin");
6162
const tinyxml2::XMLElement* longest_valid_segment_fraction_element = xml_element.FirstChildElement("LongestValidSegme"
6263
"ntFraction");
6364
const tinyxml2::XMLElement* longest_valid_segment_length_element = xml_element.FirstChildElement("LongestValidSegment"
@@ -234,18 +235,19 @@ OMPLDefaultPlanProfile::OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_e
234235
throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionContinuous string");
235236
}
236237

237-
if (collision_safety_margin_element)
238-
{
239-
std::string collision_safety_margin_string;
240-
status = tesseract_common::QueryStringText(collision_safety_margin_element, collision_safety_margin_string);
241-
if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
242-
throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionSafetyMargin string");
238+
/// @todo Update XML
239+
// if (collision_safety_margin_element)
240+
// {
241+
// std::string collision_safety_margin_string;
242+
// status = tesseract_common::QueryStringText(collision_safety_margin_element, collision_safety_margin_string);
243+
// if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
244+
// throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionSafetyMargin string");
243245

244-
if (!tesseract_common::isNumeric(collision_safety_margin_string))
245-
throw std::runtime_error("OMPLPlanProfile: CollisionSafetyMargin is not a numeric values.");
246+
// if (!tesseract_common::isNumeric(collision_safety_margin_string))
247+
// throw std::runtime_error("OMPLPlanProfile: CollisionSafetyMargin is not a numeric values.");
246248

247-
tesseract_common::toNumeric<double>(collision_safety_margin_string, collision_safety_margin);
248-
}
249+
// tesseract_common::toNumeric<double>(collision_safety_margin_string, collision_safety_margin);
250+
// }
249251

250252
if (longest_valid_segment_fraction_element)
251253
{
@@ -283,7 +285,7 @@ void OMPLDefaultPlanProfile::setup(OMPLProblem& prob) const
283285
prob.max_solutions = max_solutions;
284286
prob.simplify = simplify;
285287
prob.optimize = optimize;
286-
prob.contact_checker->setDefaultCollisionMarginData(collision_safety_margin);
288+
prob.contact_checker->setCollisionMarginData(collision_margin_data);
287289

288290
const std::vector<std::string>& joint_names = prob.manip_fwd_kin->getJointNames();
289291
const auto dof = prob.manip_fwd_kin->numJoints();
@@ -580,9 +582,10 @@ tinyxml2::XMLElement* OMPLDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc)
580582
xml_collision_continuous->SetText(collision_continuous);
581583
xml_ompl->InsertEndChild(xml_collision_continuous);
582584

583-
tinyxml2::XMLElement* xml_collision_safety_margin = doc.NewElement("CollisionSafetyMargin");
584-
xml_collision_safety_margin->SetText(collision_safety_margin);
585-
xml_ompl->InsertEndChild(xml_collision_safety_margin);
585+
/// @todo Update XML
586+
// tinyxml2::XMLElement* xml_collision_safety_margin = doc.NewElement("CollisionSafetyMargin");
587+
// xml_collision_safety_margin->SetText(collision_safety_margin);
588+
// xml_ompl->InsertEndChild(xml_collision_safety_margin);
586589

587590
tinyxml2::XMLElement* xml_long_valid_seg_frac = doc.NewElement("LongestValidSegmentFraction");
588591
xml_long_valid_seg_frac->SetText(longest_valid_segment_fraction);
@@ -616,7 +619,7 @@ OMPLDefaultPlanProfile::processStateValidator(OMPLProblem& prob,
616619
if (collision_check && !collision_continuous)
617620
{
618621
auto svc = std::make_shared<StateCollisionValidator>(
619-
prob.simple_setup->getSpaceInformation(), env, kin, collision_safety_margin, prob.extractor);
622+
prob.simple_setup->getSpaceInformation(), env, kin, collision_margin_data, prob.extractor);
620623
csvc->addStateValidator(svc);
621624
}
622625
prob.simple_setup->setStateValidityChecker(csvc);
@@ -645,7 +648,7 @@ void OMPLDefaultPlanProfile::processMotionValidator(ompl::base::StateValidityChe
645648
svc_without_collision,
646649
env,
647650
kin,
648-
collision_safety_margin,
651+
collision_margin_data,
649652
prob.extractor);
650653
}
651654
else

‎tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/state_collision_validator.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,7 @@ namespace tesseract_planning
3737
StateCollisionValidator::StateCollisionValidator(const ompl::base::SpaceInformationPtr& space_info,
3838
tesseract_environment::Environment::ConstPtr env,
3939
tesseract_kinematics::ForwardKinematics::ConstPtr kin,
40-
double collision_safety_margin,
40+
const tesseract_collision::CollisionMarginData& collision_margin_data,
4141
OMPLStateExtractor extractor)
4242
: StateValidityChecker(space_info)
4343
, env_(std::move(env))
@@ -55,7 +55,7 @@ StateCollisionValidator::StateCollisionValidator(const ompl::base::SpaceInformat
5555
links_ = adj_map.getActiveLinkNames();
5656

5757
contact_manager_->setActiveCollisionObjects(links_);
58-
contact_manager_->setDefaultCollisionMarginData(collision_safety_margin);
58+
contact_manager_->setCollisionMarginData(collision_margin_data);
5959
}
6060

6161
bool StateCollisionValidator::isValid(const ompl::base::State* state) const

0 commit comments

Comments
 (0)