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

(moveit_py) Allow editing allowed collision matrix in python + fix get_entry function #2551

Merged
merged 2 commits into from
Nov 27, 2023
Merged

(moveit_py) Allow editing allowed collision matrix in python + fix get_entry function #2551

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1810784
Allow editing allowed collision matrix in python + fix get_entry func…
JensVanhooydonck Nov 25, 2023
101f8e2
Merge branch 'main' into allow_updates_to_allowed_collision_matrix
sjahr Nov 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 4 additions & 0 deletions moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -291,9 +291,13 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
{
return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
}

/** \brief Get the allowed collision matrix */
collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst();

/** \brief Set the allowed collision matrix */
void setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm);

/**@}*/

/**
Expand Down
5 changes: 5 additions & 0 deletions moveit_core/planning_scene/src/planning_scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -558,6 +558,11 @@ collision_detection::AllowedCollisionMatrix& PlanningScene::getAllowedCollisionM
return *acm_;
}

void PlanningScene::setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm)
{
getAllowedCollisionMatrixNonConst() = acm;
}

const moveit::core::Transforms& PlanningScene::getTransforms()
{
// Trigger an update of the robot transforms
Expand Down
28 changes: 23 additions & 5 deletions moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,16 +40,34 @@ namespace moveit_py
{
namespace bind_collision_detection
{
std::pair<bool, collision_detection::AllowedCollision::Type>
getEntry(const std::shared_ptr<collision_detection::AllowedCollisionMatrix>& acm, const std::string& name1,
const std::string& name2)
// TODO: Create a custom typecaster/revise the current implementation to return std::pair<bool,
// collision_detection::AllowedCollision::Type>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I like that, can you open an issue for that it is better tracked?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

#2553

std::pair<bool, std::string> getEntry(const collision_detection::AllowedCollisionMatrix& acm, const std::string& name1,
const std::string& name2)
{
// check acm for collision
collision_detection::AllowedCollision::Type type;
bool collision_allowed = acm->getEntry(name1, name2, type);
bool collision_allowed = acm.getEntry(name1, name2, type);
std::string type_str;
if (type == collision_detection::AllowedCollision::Type::NEVER)
{
type_str = "NEVER";
}
else if (type == collision_detection::AllowedCollision::Type::ALWAYS)
{
type_str = "ALWAYS";
}
else if (type == collision_detection::AllowedCollision::Type::CONDITIONAL)
{
type_str = "CONDITIONAL";
}
else
{
type_str = "UNKNOWN";
}

// should return a tuple true/false and the allowed collision type
std::pair<bool, collision_detection::AllowedCollision::Type> result = std::make_pair(collision_allowed, type);
std::pair<bool, std::string> result = std::make_pair(collision_allowed, type_str);
return result;
}

Expand Down
5 changes: 3 additions & 2 deletions moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,8 +111,9 @@ void initPlanningScene(py::module& m)
py::return_value_policy::move)

.def_property("transforms", py::overload_cast<>(&planning_scene::PlanningScene::getTransforms), nullptr)
.def_property("allowed_collision_matrix", &planning_scene::PlanningScene::getAllowedCollisionMatrix, nullptr,
py::return_value_policy::move)
.def_property("allowed_collision_matrix", &planning_scene::PlanningScene::getAllowedCollisionMatrix,
&planning_scene::PlanningScene::setAllowedCollisionMatrix,
py::return_value_policy::reference_internal)
// methods
.def("__copy__",
[](const planning_scene::PlanningScene* self) {
Expand Down