Ask Your Question
2

MoveIt visualization of planned trajectories with attached objects

asked 2016-11-14 17:38:33 -0500

BrettHemes gravatar image

I have an application where being able to visualize the trajectory before executing is beneficial for a number of reasons. I recently added attached collision objects to the robot but they don't seem to show up in the trajectories visualized using move_group's /move_group/display_planned_path topic. Is this the intended behavior? If so, does anyone have suggestions on how to elegantly achieve what I am trying to get?

Somewhat related to http://answers.ros.org/question/248071/rviz-ignoring-collisions-between-attached-object-and-environment/

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2016-11-15 09:50:03 -0500

v4hn gravatar image

There exists an issue for this in the moveit repository: https://github.com/ros-planning/movei... Although this is talking about the visualized trail, the underlying issue is the same.

So, currently the attached objects are not visualized there, but a patch to change that is welcome upstream!

edit flag offensive delete link more

Comments

Thanks for the pointer v4hn. I'll see if I can't find some time to implement this...

BrettHemes gravatar imageBrettHemes ( 2016-11-15 10:52:16 -0500 )edit
0

answered 2016-11-15 14:33:49 -0500

BrettHemes gravatar image

Just to clarify on my particular issue. I was sending a trajectory with an empty start state (which defaults to use the current state) BUT it seems that the move_group doesn't keep track of objects attached through the planning_scene (am I right here?). I was able to fix my issue by pulling the attached objects from a planning_scene_interface and adding them to the appropriate field of my DisplayTrajectory message. The attached objects now show up.

edit flag offensive delete link more

Comments

That explanation does not make sense. The PlanningScene you are talking about is part of move_group, so I am confident it keeps track of the attached objects. The bug you hit seems to be that attached objects are not part of the displayed traj if they result from a request without a start state

v4hn gravatar imagev4hn ( 2016-12-08 03:16:01 -0500 )edit

I understand that the planning scene is part of move_group and tried really hard to get the behavior you allude to but states returned through the movegroup API have no attached bodies...

BrettHemes gravatar imageBrettHemes ( 2016-12-08 08:57:55 -0500 )edit

... this doesn't work

moveit::core::robotStateToRobotStateMsg(*movegroup_->getCurrentState(), display_trajectory.trajectory_start)
BrettHemes gravatar imageBrettHemes ( 2016-12-08 08:59:02 -0500 )edit

this does...

attached_objects = planning_scene_interface_.getAttachedObjects();
for (auto it = attached_objects.begin(); it != attached_objects.end(); ++it)
  display_trajectory.trajectory_start.attached_collision_objects.push_back(it->second);
BrettHemes gravatar imageBrettHemes ( 2016-12-08 09:00:35 -0500 )edit

I also found a thread in which someone has the same issue here. In particular see the 7th email and Ioan's respective response saying that "the current state as monitored by the MoveGroupInterface does not monitor attached objects"

BrettHemes gravatar imageBrettHemes ( 2016-12-08 09:34:49 -0500 )edit

I see. It makes some sense that MoveGroupInterface::getCurrentState does not know about attached objects. It only knows about joint states. Could you please create an issue for this in the project so we can document or change this behavior in the future?

v4hn gravatar imagev4hn ( 2016-12-08 11:29:57 -0500 )edit

Your original question did not concern getCurrentState though. You wrote you submitted a planning request with an empty (i.e. no) start state and MoveIt failed to visualize the attached object in the trajectory. This is a bug in the move_group node and not a shortcoming of the interface.

v4hn gravatar imagev4hn ( 2016-12-08 11:33:44 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2016-11-14 17:38:33 -0500

Seen: 583 times

Last updated: Nov 15 '16