Skip to content

Commit 4e483d6

Browse files
marrtsmarrts
authored
Check trajectory return contact location (#1200)
1 parent 403fd2f commit 4e483d6

File tree

9 files changed

+2584
-482
lines changed

9 files changed

+2584
-482
lines changed

‎tesseract_collision/core/include/tesseract_collision/core/types.h‎

Lines changed: 45 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -394,6 +394,17 @@ enum class CollisionCheckProgramType : std::uint8_t
394394
INTERMEDIATE_ONLY
395395
};
396396

397+
/** @brief Controls when collision checking exits or continues collecting results */
398+
enum class CollisionCheckExitType : std::uint8_t
399+
{
400+
/** @brief Exit as soon as any collision is detected */
401+
FIRST,
402+
/** @brief For LVS, stop at first collision per step; continue to next step */
403+
ONE_PER_STEP,
404+
/** @brief Collect all collisions */
405+
ALL
406+
};
407+
397408
/** @brief Identifies how the provided AllowedCollisionMatrix should be applied relative to the isAllowedFn in the
398409
* contact manager */
399410
enum class ACMOverrideType : std::uint8_t
@@ -471,7 +482,8 @@ struct CollisionCheckConfig
471482
CollisionCheckConfig(ContactRequest request = ContactRequest(),
472483
CollisionEvaluatorType type = CollisionEvaluatorType::DISCRETE,
473484
double longest_valid_segment_length = 0.005,
474-
CollisionCheckProgramType check_program_mode = CollisionCheckProgramType::ALL);
485+
CollisionCheckProgramType check_program_mode = CollisionCheckProgramType::ALL,
486+
CollisionCheckExitType exit_condition = CollisionCheckExitType::FIRST);
475487

476488
/** @brief ContactRequest that will be used for this check. Default test type: ALL*/
477489
ContactRequest contact_request;
@@ -485,6 +497,9 @@ struct CollisionCheckConfig
485497
/** @brief Secifies the mode used when collision checking program/trajectory. Default: ALL */
486498
CollisionCheckProgramType check_program_mode{ CollisionCheckProgramType::ALL };
487499

500+
/** @brief The condition in which the trajectory collision check will exit. Default: FIRST */
501+
CollisionCheckExitType exit_condition{ CollisionCheckExitType::FIRST };
502+
488503
bool operator==(const CollisionCheckConfig& rhs) const;
489504
bool operator!=(const CollisionCheckConfig& rhs) const;
490505
};
@@ -506,6 +521,13 @@ struct ContactTrajectorySubstepResults
506521

507522
using UPtr = std::unique_ptr<ContactTrajectorySubstepResults>;
508523

524+
operator bool() const;
525+
526+
void addContact(int substep_number,
527+
const Eigen::VectorXd& start_substate,
528+
const Eigen::VectorXd& end_substate,
529+
const tesseract_collision::ContactResultMap& new_contacts);
530+
509531
int numContacts() const;
510532

511533
tesseract_collision::ContactResultVector worstCollision() const;
@@ -536,6 +558,17 @@ struct ContactTrajectoryStepResults
536558

537559
using UPtr = std::unique_ptr<ContactTrajectoryStepResults>;
538560

561+
operator bool() const;
562+
563+
void addContact(int step_number,
564+
int substep_number,
565+
int num_substeps,
566+
const Eigen::VectorXd& start_state,
567+
const Eigen::VectorXd& end_state,
568+
const Eigen::VectorXd& start_substate,
569+
const Eigen::VectorXd& end_substate,
570+
const tesseract_collision::ContactResultMap& contacts);
571+
539572
void resize(int num_substeps);
540573

541574
int numSubsteps() const;
@@ -572,6 +605,17 @@ struct ContactTrajectoryResults
572605

573606
using UPtr = std::unique_ptr<ContactTrajectoryResults>;
574607

608+
operator bool() const;
609+
610+
void addContact(int step_number,
611+
int substep_number,
612+
int num_substeps,
613+
const Eigen::VectorXd& start_state,
614+
const Eigen::VectorXd& end_state,
615+
const Eigen::VectorXd& start_substate,
616+
const Eigen::VectorXd& end_substate,
617+
const tesseract_collision::ContactResultMap& contacts);
618+
575619
void resize(int num_steps);
576620

577621
int numSteps() const;

‎tesseract_collision/core/include/tesseract_collision/core/yaml_extensions.h‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -202,6 +202,40 @@ struct convert<tesseract_collision::CollisionCheckProgramType>
202202
}
203203
};
204204

205+
//=========================== CollisionCheckExitType Enum ===========================
206+
template <>
207+
struct convert<tesseract_collision::CollisionCheckExitType>
208+
{
209+
static Node encode(const tesseract_collision::CollisionCheckExitType& rhs)
210+
{
211+
static const std::map<tesseract_collision::CollisionCheckExitType, std::string> m = {
212+
{ tesseract_collision::CollisionCheckExitType::FIRST, "FIRST" },
213+
{ tesseract_collision::CollisionCheckExitType::ONE_PER_STEP, "ONE_PER_STEP" },
214+
{ tesseract_collision::CollisionCheckExitType::ALL, "ALL" }
215+
};
216+
return Node(m.at(rhs));
217+
}
218+
219+
static bool decode(const Node& node, tesseract_collision::CollisionCheckExitType& rhs)
220+
{
221+
static const std::map<std::string, tesseract_collision::CollisionCheckExitType> inv = {
222+
{ "FIRST", tesseract_collision::CollisionCheckExitType::FIRST },
223+
{ "ONE_PER_STEP", tesseract_collision::CollisionCheckExitType::ONE_PER_STEP },
224+
{ "ALL", tesseract_collision::CollisionCheckExitType::ALL }
225+
};
226+
227+
if (!node.IsScalar())
228+
return false;
229+
230+
auto it = inv.find(node.Scalar());
231+
if (it == inv.end())
232+
return false;
233+
234+
rhs = it->second;
235+
return true;
236+
}
237+
};
238+
205239
//=========================== ContactManagerConfig ===========================
206240
template <>
207241
struct convert<tesseract_collision::ContactManagerConfig>
@@ -292,6 +326,7 @@ struct convert<tesseract_collision::CollisionCheckConfig>
292326
node["type"] = rhs.type;
293327
node["longest_valid_segment_length"] = rhs.longest_valid_segment_length;
294328
node["check_program_mode"] = rhs.check_program_mode;
329+
node["exit_condition"] = rhs.exit_condition;
295330

296331
return node;
297332
}
@@ -306,6 +341,8 @@ struct convert<tesseract_collision::CollisionCheckConfig>
306341
rhs.longest_valid_segment_length = n.as<double>();
307342
if (const YAML::Node& n = node["check_program_mode"])
308343
rhs.check_program_mode = n.as<tesseract_collision::CollisionCheckProgramType>();
344+
if (const YAML::Node& n = node["exit_condition"])
345+
rhs.exit_condition = n.as<tesseract_collision::CollisionCheckExitType>();
309346
return true;
310347
}
311348
};

‎tesseract_collision/core/src/serialization.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,7 @@ void serialize(Archive& ar, tesseract_collision::CollisionCheckConfig& g, const
160160
ar& boost::serialization::make_nvp("type", g.type);
161161
ar& boost::serialization::make_nvp("longest_valid_segment_length", g.longest_valid_segment_length);
162162
ar& boost::serialization::make_nvp("check_program_mode", g.check_program_mode);
163+
ar& boost::serialization::make_nvp("exit_condition", g.exit_condition);
163164
}
164165

165166
template <class Archive>

‎tesseract_collision/core/src/types.cpp‎

Lines changed: 82 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -419,11 +419,13 @@ bool ContactManagerConfig::operator!=(const ContactManagerConfig& rhs) const { r
419419
CollisionCheckConfig::CollisionCheckConfig(ContactRequest request,
420420
CollisionEvaluatorType type,
421421
double longest_valid_segment_length,
422-
CollisionCheckProgramType check_program_mode)
422+
CollisionCheckProgramType check_program_mode,
423+
CollisionCheckExitType exit_condition)
423424
: contact_request(std::move(request))
424425
, type(type)
425426
, longest_valid_segment_length(longest_valid_segment_length)
426427
, check_program_mode(check_program_mode)
428+
, exit_condition(exit_condition)
427429
{
428430
}
429431

@@ -435,6 +437,7 @@ bool CollisionCheckConfig::operator==(const CollisionCheckConfig& rhs) const
435437
ret_val &=
436438
tesseract_common::almostEqualRelativeAndAbs(longest_valid_segment_length, rhs.longest_valid_segment_length);
437439
ret_val &= (check_program_mode == rhs.check_program_mode);
440+
ret_val &= (exit_condition == rhs.exit_condition);
438441
return ret_val;
439442
}
440443
bool CollisionCheckConfig::operator!=(const CollisionCheckConfig& rhs) const { return !operator==(rhs); }
@@ -451,6 +454,20 @@ ContactTrajectorySubstepResults::ContactTrajectorySubstepResults(int substep_num
451454
{
452455
}
453456

457+
ContactTrajectorySubstepResults::operator bool() const { return !contacts.empty(); }
458+
459+
void ContactTrajectorySubstepResults::addContact(int substep_number,
460+
const Eigen::VectorXd& start_substate, // NOLINT
461+
const Eigen::VectorXd& end_substate, // NOLINT
462+
const tesseract_collision::ContactResultMap& new_contacts)
463+
{
464+
state0 = start_substate;
465+
state1 = end_substate;
466+
substep = substep_number;
467+
468+
contacts = new_contacts;
469+
}
470+
454471
int ContactTrajectorySubstepResults::numContacts() const { return static_cast<int>(contacts.size()); }
455472

456473
tesseract_collision::ContactResultVector ContactTrajectorySubstepResults::worstCollision() const
@@ -484,6 +501,44 @@ ContactTrajectoryStepResults::ContactTrajectoryStepResults(int step_number, cons
484501
substeps.resize(static_cast<std::size_t>(2));
485502
}
486503

504+
ContactTrajectoryStepResults::operator bool() const
505+
{
506+
for (const auto& substep : substeps)
507+
{
508+
if (static_cast<bool>(substep))
509+
return true;
510+
}
511+
return false;
512+
}
513+
514+
void ContactTrajectoryStepResults::addContact(int step_number,
515+
int substep_number,
516+
int num_substeps,
517+
const Eigen::VectorXd& start_state, // NOLINT
518+
const Eigen::VectorXd& end_state, // NOLINT
519+
const Eigen::VectorXd& start_substate, // NOLINT
520+
const Eigen::VectorXd& end_substate, // NOLINT
521+
const tesseract_collision::ContactResultMap& contacts)
522+
{
523+
if (total_substeps < num_substeps)
524+
{
525+
resize(num_substeps);
526+
state0 = start_state;
527+
state1 = end_state;
528+
step = step_number;
529+
}
530+
531+
if (substep_number < 0 || substep_number > total_substeps)
532+
{
533+
if (substep_number < 0)
534+
throw std::runtime_error("ContactTrajectoryStepResults::addContact: substep_number is negative");
535+
if (substep_number > total_substeps)
536+
throw std::out_of_range("ContactTrajectoryStepResults::addContact: substep_number out of range");
537+
}
538+
539+
substeps[static_cast<std::size_t>(substep_number)].addContact(substep_number, start_substate, end_substate, contacts);
540+
}
541+
487542
void ContactTrajectoryStepResults::resize(int num_substeps)
488543
{
489544
total_substeps = num_substeps;
@@ -552,6 +607,32 @@ ContactTrajectoryResults::ContactTrajectoryResults(std::vector<std::string> j_na
552607
steps.resize(static_cast<std::size_t>(num_steps));
553608
}
554609

610+
ContactTrajectoryResults::operator bool() const
611+
{
612+
for (const auto& step : steps)
613+
{
614+
if (static_cast<bool>(step))
615+
return true;
616+
}
617+
return false;
618+
}
619+
620+
void ContactTrajectoryResults::addContact(int step_number,
621+
int substep_number,
622+
int num_substeps,
623+
const Eigen::VectorXd& start_state, // NOLINT
624+
const Eigen::VectorXd& end_state, // NOLINT
625+
const Eigen::VectorXd& start_substate, // NOLINT
626+
const Eigen::VectorXd& end_substate, // NOLINT
627+
const tesseract_collision::ContactResultMap& contacts)
628+
{
629+
if (step_number < 0 || step_number >= total_steps)
630+
throw std::out_of_range("ContactTrajectoryResults::addContact: step_number out of range");
631+
632+
steps[static_cast<std::size_t>(step_number)].addContact(
633+
step_number, substep_number, num_substeps, start_state, end_state, start_substate, end_substate, contacts);
634+
}
635+
555636
void ContactTrajectoryResults::resize(int num_steps)
556637
{
557638
total_steps = num_steps;

0 commit comments

Comments
 (0)