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

fix: ensure attached objects update during motion execution #3327

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
+70 −6
Open

fix: ensure attached objects update during motion execution #3327

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
b94e50b
fix: ensure attached objects update during motion execution
MarcoMagriDev Feb 6, 2025
20df675
feat: adds `getAttachedBodies` override that returns a map instead of…
MarcoMagriDev Feb 6, 2025
99bdd41
fix: removes vector allocation in getAttachedBodies map override
MarcoMagriDev Feb 7, 2025
b63718d
Merge branch 'main' into fix/collision-monitoring-attached-objects-no…
MarcoMagriDev Feb 7, 2025
8ccb626
feat: automatically determine whether attached objects need to be re-…
MarcoMagriDev Feb 8, 2025
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
3 changes: 3 additions & 0 deletions moveit_core/robot_state/include/moveit/robot_state/robot_state.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1519,6 +1519,9 @@ class RobotState
/** \brief Get all bodies attached to the model corresponding to this state */
void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;

/** \brief Get all bodies attached to the model corresponding to this state */
void getAttachedBodies(std::map<std::string, const AttachedBody*>& attached_bodies) const;

/** \brief Get all bodies attached to a particular group the model corresponding to this state */
void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const;

Expand Down
11 changes: 11 additions & 0 deletions moveit_core/robot_state/src/robot_state.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1220,6 +1220,17 @@ void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bo
attached_bodies.push_back(it.second.get());
}

void RobotState::getAttachedBodies(std::map<std::string, const AttachedBody*>& attached_bodies) const
{
attached_bodies.clear();
std::vector<const AttachedBody*> bodies;
getAttachedBodies(bodies);
for (const auto& b : bodies)
{
attached_bodies[b->getName()] = b;
}
}

void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const
{
attached_bodies.clear();
Expand Down
38 changes: 32 additions & 6 deletions moveit_ros/planning/plan_execution/src/plan_execution.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -283,34 +283,60 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
collision_detection::CollisionRequest req;
req.group_name = t.getGroupName();
req.pad_environment_collisions = false;
moveit::core::RobotState state = plan.planning_scene->getCurrentState();
std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects, sample_attached_object;
MarcoMagriDev marked this conversation as resolved.
Show resolved Hide resolved
state.getAttachedBodies(current_attached_objects);
for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
{
state = t.getWayPoint(i);
Copy link
Contributor

Choose a reason for hiding this comment

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

Thank you for this!

Copy link
Contributor Author

Choose a reason for hiding this comment

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

The only simplification I could think of is that, under the assumption that "attached objects are set to the waypoint's robot state at planning time" (which actually holds) we could query sample_attached_object only once. However, I proposed updating it for each waypoint to ensure robustness, even if this assumption doesn't hold. Do you see any potential issues or improvements with this?

Copy link
Contributor

Choose a reason for hiding this comment

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

Ah thanks for asking.

I guess I would be thinking about:

  1. Does it slow things down to be potentially attaching/detaching objects at each waypoint during this check?
  2. It's difficult for me to know what the user's intent would be on a case by case basis. Right now it seems like the current state is treated as the source of truth, but I'm unsure whether users want that in every case. I guess the previous implementation did the opposite and treated the attached objects from the pre-planned waypoint as the source of truth.

Is it worth maybe adding a flag to this function for whether one wants to prioritize current state vs. planned states? And elevate this up to the config/parameter level?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

  1. Does it slow things down to be potentially attaching/detaching objects at each waypoint during this check?

I think it depends on how frequently the planning scene changes. In a highly dynamic environment, this method would be called often, which could impact performance. Since attached objects are processed for all robot states associated with waypoints after the currently executed one, the worst-case scenario with the current implementation occurs when scene updates happen frequently at the start of the trajectory.

With the proposed simplification, this reduces to simply "high-frequency scene updates," as only one robot state from the trajectory is queried for attached objects, while the rest are assumed unchanged.

On the other hand, if future plans include attached objects that are not consistently present throughout the motion, this simplification could introduce errors.

  1. It's difficult for me to know what the user's intent would be on a case by case basis. Right now it seems like the current state is treated as the source of truth, but I'm unsure whether users want that in every case. I guess the previous implementation did the opposite and treated the attached objects from the pre-planned waypoint as the source of truth.

Is it worth maybe adding a flag to this function for whether one wants to prioritize current state vs. planned states? And elevate this up to the config/parameter level?

As for prioritizing the current state vs. planned states, since world objects are always considered in real-time rather than at planning time, I don’t see a strong reason to let the user choose the source of truth for attached collision objects. Keeping it consistent with real-time world objects could help prevent many erroneous situations.

Copy link
Contributor

Choose a reason for hiding this comment

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

Agreed. But at least in your comments on 1., maybe we do need the toggle on using the attached objects only at the start vs. updating this frequently.

Basically, asking the user "do you want this simplification or not?"

Copy link
Contributor Author

Choose a reason for hiding this comment

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

I just pushed a new commit that automates this process without requiring user input.

Before entering the monitoring phase, it verifies whether the attached objects remain consistent throughout the trajectory. If they do, they are stored and later used by the isRemainingPathValid method without needing to be queried again.

If the attached objects change during the planned trajectory, the map is left empty, signaling isRemainingPathValid to query them at each waypoint.

I believe this approach is more robust than using a parameter, as it eliminates the possibility of misconfiguration by the user. What do you think?

collision_detection::CollisionResult res;
state.getAttachedBodies(sample_attached_object);

// If sample state has attached objects that are not in the current state, remove them from the sample state
for (const auto& [name, object] : sample_attached_object)
{
if (current_attached_objects.find(name) == current_attached_objects.end())
{
RCLCPP_DEBUG(logger_, "Attached object '%s' is not in the current scene. Removing it.", name.c_str());
state.clearAttachedBody(name);
}
}

// If current state has attached objects that are not in the sample state, add them to the sample state
for (const auto& [name, object] : current_attached_objects)
{
if (sample_attached_object.find(name) == sample_attached_object.end())
{
RCLCPP_DEBUG(logger_, "Attached object '%s' is not in the robot state. Adding it.", name.c_str());
state.attachBody(std::make_unique<moveit::core::AttachedBody>(*object));
}
}

if (acm)
{
plan.planning_scene->checkCollision(req, res, t.getWayPoint(i), *acm);
plan.planning_scene->checkCollision(req, res, state, *acm);
}
else
{
plan.planning_scene->checkCollision(req, res, t.getWayPoint(i));
plan.planning_scene->checkCollision(req, res, state);
}

if (res.collision || !plan.planning_scene->isStateFeasible(t.getWayPoint(i), false))
if (res.collision || !plan.planning_scene->isStateFeasible(state, false))
{
RCLCPP_INFO(logger_, "Trajectory component '%s' is invalid for waypoint %ld out of %ld",
plan.plan_components[path_segment.first].description.c_str(), i, wpc);

// call the same functions again, in verbose mode, to show what issues have been detected
plan.planning_scene->isStateFeasible(t.getWayPoint(i), true);
plan.planning_scene->isStateFeasible(state, true);
req.verbose = true;
res.clear();
if (acm)
{
plan.planning_scene->checkCollision(req, res, t.getWayPoint(i), *acm);
plan.planning_scene->checkCollision(req, res, state, *acm);
}
else
{
plan.planning_scene->checkCollision(req, res, t.getWayPoint(i));
plan.planning_scene->checkCollision(req, res, state);
}
return false;
}
Expand Down
Loading