Programatically disable self-collision checking on move_group?

[Jorge Santos Simón]

On move_base interface I can order the arm to move to a named target defined as group_state on the SRDF. But to do so, MoveIt! always perform  self-collision checking, what is fine, except when I want to start the arm in the "home" position, that in my case is comfortably folded, so it keeps that pose when powered off (will post a picture later). On this folded pose, arm links are in touch with other robot parts, so sometimes MoveIt! fails to move at all due to self-collision.

I see two easy solutions:

• Programatically disable self-collision checking on move_group, but I cannot find whether and how this can be done
  
• Use low-level arm controller to reach the operational starting position "home" and then start using MoveIt!
  

For the second option, can I programatically recover joint values for a group_state defined on the SRDF file? Like that, I can easily compose a trajectory_msgs/JointTrajectory message and send to the /arm_controller/command topic.

Thanks a lot

Jorge

[v4hn]

On Mon, Aug 07, 2017 at 01:28:41AM -0700, Jorge Santos Simón wrote:
> On this folded pose, arm links are in touch with other robot parts,
> so sometimes MoveIt! fails to move at all due to self-collision.

You can disable collision checking in the AllowedCollisionMatrix of the planning scene.
But the generated trajectory will ignore *all* collisions of the allowed pairs and this could
(depending on your robot) generate infeasible trajectories where the links pass through each other.

If the only problematic point is the start state of the trajectory, you can probably use
the `fix_start_state_collision` planning request adapter and fiddle with its parameters
until it gets you out of the invalid start state reliably.

> - Programatically disable self-collision checking on move_group, but I

> cannot find whether and how this can be done

See above

> - Use low-level arm controller to reach the operational starting

> position "home" and then start using MoveIt!

If you know the "powerup" state and the "operational" state, then
it might be best to have either the driver or an external module move you
there via a known trajectory.

> For the second option, can I programatically recover joint values for a
> group_state defined on the SRDF file?

Yes. Use `MoveGroup(Interface)::getNamedTargetValues`

Best,


v4hn

PS: This would have been a wonderful answers.ros.org message :)


--
Michael Görner, M.Sc. Cognitive Science, PhD Student
Universität Hamburg
Faculty of Mathematics, Informatics and Natural Sciences
Department of Informatics
Group Technical Aspects of Multimodal Systems

Vogt-Kölln-Straße 30
D-22527 Hamburg

Room: F-315
Phone: +49 40 42883-2432
Website: https://tams.informatik.uni-hamburg.de/people/goerner/

[Jorge Santos Simón]

Thanks for the detailed answer. I tried your first advice but failed to make it work:

You can disable collision checking in the AllowedCollisionMatrix of the planning scene.
But the generated trajectory will ignore *all* collisions of the allowed pairs and this could
(depending on your robot) generate infeasible trajectories where the links pass through each other.

I tried as follows, firs allowing the offending collision:

  planning_scene_monitor::LockedPlanningSceneRW lps(planning_scene_monitor_ptr_);

  lps->getAllowedCollisionMatrixNonConst().setEntry("arm_mount_link", "arm_elbow_flex_servo_link", true);

but also enabling all collisions, as you suggest:

  lps->getAllowedCollisionMatrixNonConst().setEntry(true);

In both cases planning fails same way as before, so I'm doing something wrong. I can tell, though, that setEntry(true) does the expected thing because all 0s disappear from the collision matrix after calling it.

 

If the only problematic point is the start state of the trajectory, you can probably use
the `fix_start_state_collision` planning request adapter and fiddle with its parameters
until it gets you out of the invalid start state reliably.

First time I hear about this! is there any example out there? 

If you know the "powerup" state and the "operational" state, then
it might be best to have either the driver or an external module move you
there via a known trajectory.

This is a good idea, and in fact it's why I wanted to get the group_state defined on the SRDF; but getNamedTargetValues only provides the recorded states, not those on the SRDF file. Maybe that would be a nice addition.

Yes. Use `MoveGroup(Interface)::getNamedTargetValues`

Best,

Thanks a lot!

PS. Normally I ask here pure MoveIt! questions, and let ROS answers for more general stuff...  But zero inconvenient to port this there!

[v4hn]

On Mon, Aug 07, 2017 at 03:24:23PM -0700, Jorge Santos Simón wrote:
> I tried your first advice but failed to make it work:
>
> > You can disable collision checking in the AllowedCollisionMatrix of the
> > planning scene.
> > But the generated trajectory will ignore *all* collisions of the allowed
> > pairs and this could
> > (depending on your robot) generate infeasible trajectories where the links
> > pass through each other.
> >
> > I tried as follows, firs allowing the offending collision:
>
> planning_scene_monitor::LockedPlanningSceneRW
> lps(planning_scene_monitor_ptr_);
> lps->getAllowedCollisionMatrixNonConst().setEntry("arm_mount_link",
> "arm_elbow_flex_servo_link", true);
>
> but also enabling all collisions, as you suggest:
>
> lps->getAllowedCollisionMatrixNonConst().setEntry(true);

Unless you use a custom environment, planning is performed in the `move_group` node.
You have to set the allowed collision matrix in the planning scene of that node.
I guess you did *not* run this in that node (e.g. as a capability)?
Externally you can use the `PlanningSceneInterface::applyPlanningScene` or others
to change the collision matrix.

> > If the only problematic point is the start state of the trajectory, you
> > can probably use
> > the `fix_start_state_collision` planning request adapter and fiddle with
> > its parameters
> > until it gets you out of the invalid start state reliably.
>
> First time I hear about this! is there any example out there?

The general description of the plugin mechanism is here. I'm not aware
of a detailed description of the parameters of this particular plugin.
http://moveit.ros.org/documentation/plugins/#planningrequestadapter

> > If you know the "powerup" state and the "operational" state, then
> > it might be best to have either the driver or an external module move you
> > there via a known trajectory.
> >

> but getNamedTargetValues only provides the recorded states, not those on the SRDF file.

This is not true.

> PS. Normally I ask here pure MoveIt! questions, and let ROS answers for
> more general stuff... But zero inconvenient to port this there!

Please see http://moveit.ros.org/support/

> - Ask questions at answers.ros.org, the official question forum of ROS and MoveIt!,
> if you haven’t found similar questions.
>
> - Join the MoveIt! users mailing list (moveit-users at googlegroups dot com) to receive
> announcements from here. In the list page you can also find previous good discussions.
> Please refrain from asking basic user questions here.


v4hn

[Jorge Santos Simón]

Unless you use a custom environment, planning is performed in the `move_group` node.
You have to set the allowed collision matrix in the planning scene of that node.
I guess you did *not* run this in that node (e.g. as a capability)?
Externally you can use the `PlanningSceneInterface::applyPlanningScene` or others
to change the collision matrix.

Completely right;  was trying from another node.  For using PlanningSceneInterface::applyPlanningScene, I have a question:

Ok, I create a PlanningScene, modify the AllowedCollisionMatrix and publish it. All fine. But... how can I restore the previous collision matrix?

I tried to subscribe to the planning_scene topic, but... is not latched, so I have no way (that I know) to store a "master" planning scene configuration.

99% sure I'm missing an important point here

The general description of the plugin mechanism is here. I'm not aware
of a detailed description of the parameters of this particular plugin.
http://moveit.ros.org/documentation/plugins/#planningrequestadapter

> > If you know the "powerup" state and the "operational" state, then
> > it might be best to have either the driver or an external module move you
> > there via a known trajectory.
> >
> but getNamedTargetValues only provides the recorded states, not those on the SRDF file.

This is not true.

right again; I was calling the wrong method.

So now I can recover my named target joint values and execute a trajectory_msgs/JointTrajectory to reach it.

Is working for very small movements, though I need to to some interpolation to allow longer trajectories.

 

> PS. Normally I ask here pure MoveIt! questions, and let ROS answers for
> more general stuff...  But zero inconvenient to port this there!

Please see http://moveit.ros.org/support/

And right one more time. Next time will do.

Thanks a lot!

[v4hn]

On Fri, Aug 11, 2017 at 09:46:04AM -0700, Jorge Santos Simón wrote:
> Ok, I create a PlanningScene, modify the AllowedCollisionMatrix and publish
> it. All fine. But... how can I restore the previous collision matrix?
> I tried to subscribe to the planning_scene topic, but... is not latched, so
> I have no way (that I know) to store a "master" planning scene
> configuration.

You can use the `/get_planning_scene` service to get the current planning scene.

Enjoy your weekend,