Path planning without attached object

[Ane Fernandez]

Dear all,

I'm working on pick & place simulation, in Indigo. I'm able to pick the object and I attach it properly, plubishing on /attached_collision_object topic. However the plath planning that Ompl generates, doesn't take into account the attached object and collide with the environment.  It seem that the robot state is not updated once that the object has been attached. I read in some post that apart from publishing the attachment on the /attached_collision_object topic, it was also necessary to attach it on the move_group_interface with attachObject() method. I did it, but it didn't work.

Does anyone know what I'm doing wrong?

Thanks in advance,

Ane

[Simon Schmeißer]

Hi Ane,

I use PlanningSceneInterface for this
http://docs.ros.org/jade/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1PlanningSceneInterface.html

Then you need to know that this will only publish a "polite request to attach this at some time", so there is no guarantee that your PlanningScene has been updated yet and it's even less likely that your local RobotState has received these updates.

So you might want to sleep a bit and then force an update, quoting myself from another discussion

> please have a look at this post, it's an acceptable workaround for me at the moment:
> https://groups.google.com/d/msg/moveit-users/ZYsWL1jRWyY/K0ia02WC4JYJ
> make sure to scope the second part of the code with { } or put it in a function to kill the lock object before using it again.

or, as I do it, get a local detached PlanningScene, attach to it and to the real one (via planningSceneInterface) and then do the planning using the local copy. (A PlanningScene can be viewed as a static snapshot of your environment at a certain point in time, there can be many of them and you can play around with them. The "authoritative" one is the one provided and updated by PlanningSceneUpdater of move group)

You're welcome
Simon

[Ane Fernandez]

Hi Simon,

Thank you very much for your response. I have tried it but I still have the same problem. First of all I publish the attached object in attached_collision_object topic. After that I update the current planing scene and I'm able to check that the object has been attached successfully.

    planning_scene_monitor_->requestPlanningSceneState(PLANNING_SCENE_SERVICE);
    planning_scene_monitor::LockedPlanningSceneRW ps(planning_scene_monitor_);
    ps->getCurrentStateNonConst().update();
    robot_state::RobotState current_state = ps->getCurrentState();
    ROS_INFO("%d", current_state.hasAttachedBody(obj_name));

However, when I update the start state with the new robot state, I'm doing something wrong, because none attached body is detected.

        group_->setStartState(current_state);
       
       robot_state::RobotStatePtr RS = group_->getCurrentState();
        ROS_INFO("%d", RS->hasAttachedBody(obj_name));


I don't know if I missing something or I'm not using the correct method. Do you know where could be the problem?

Thanks in advance,

Ane

[Ane Fernandez]

On Wednesday, January 13, 2016 at 9:24:08 AM UTC+1, Ane Fernandez wrote:

Hi Simon,

Thank you very much for your response. I have tried it but I still have the same problem. First of all I publish the attached object in attached_collision_object topic. After that I update the current planing scene and I'm able to check that the object has been attached successfully.

    planning_scene_monitor_->requestPlanningSceneState(PLANNING_SCENE_SERVICE);
    planning_scene_monitor::LockedPlanningSceneRW ps(planning_scene_monitor_);
    ps->getCurrentStateNonConst().update();
    robot_state::RobotState current_state = ps->getCurrentState();
    ROS_INFO("%d", current_state.hasAttachedBody(obj_name));

However, when I update the start state with the new robot state, I'm doing something wrong, because none attached body is detected.

        group_->setStartState(current_state);
       

           sleep(2);
 

[Felix Burget]

Hi Ane,

i had the same problem. Getting the robot state from the PlanningScene "scene->getCurrentStateNonConst();" provided the attached collision object (see below).

//Get curent planning scene
m_planning_scene_monitor->requestPlanningSceneState(PLANNING_SCENE_SERVICE);
planning_scene_monitor::LockedPlanningSceneRW ps(m_planning_scene_monitor);
ps->getCurrentStateNonConst().update();
//if you want to modify it
planning_scene::PlanningScenePtr scene = ps->diff();
scene->decoupleParent();

//Get Current Robot State from the Planning Scene
robot_state::RobotState state = scene->getCurrentStateNonConst();
state.setToDefaultValues();

//Set Configuration of robot state
state.setVariablePositions(configuration);

//Apply robot state to planning scene
scene->setCurrentState(state);

[Simon Schmeißer]

if both your startState and goalState have the attached object, planning should happen with the attached object being considered.

[Ane Fernandez]

Hi Felix,

Thank you very much for your response. I'm not so sure if we have the same problem, because after updating the planning scene, the current state of my planning scene returns that it has attached bodies:
 
  planning_scene_monitor::LockedPlanningSceneRW ps(planning_scene_monitor_);

  ps->getCurrentStateNonConst().update();
  robot_state::RobotState current_state = ps->getCurrentState();

  ROS_INFO("%d", current_state.hasAttachedBody(obj_name));   --> The output of this ROS_INFO is one.

I thought that it was also necesary to update the robot state of the move_group instance, in my case it is called group_. That's why I have added this sentence:

        group_->setStartState(current_state);

Isn't it necesary? After doing that, if I check if there is any attached objects, the return is zero.

Why do you execute decoupleParent method? Is it mandatory?

Thansk!!!!!

Ane

[Ane Fernandez]

How can I check if my startState and goalState have the attached object?

[Simon Schmeißer]

sorry, there is no goalState, only jointTarget/poseTarget, so the attached object information is taken from startState, which you set in your code above ( group_->setStartState(current_state); )

have a look at the code here:
 http://docs.ros.org/jade/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroup_1_1MoveGroupImpl.html

[Ane Fernandez]

Sorry but I think that I misundestanding something. I looked the URL and it is the API of MoveGroupImpl, which I thought it was only for internal use of moveGroup class. I'm wrong?

Although I execute  group_->setStartState(current_state); , it seems that it doesn't update properly the start state, because no attached body is detected in the robot state of my move_group instance. Do you know why?

[Simon Schmeißer]

Well yes, the internal part is where the stuff actually happens. So if you look at the code of any function in MoveGroup like MoveGroup::setStartState(state) it will look like move_group_impl_->setStartState(state). For every function there is a line saying "Definition at 123 of ...", click this number to get to the line of code where the function is actually defined. It's quite readable in most parts.

setStartState sets an internal variable of move_group (start_state_), that's it. It doesn't update the "state of movegroup" in any way

getCurrentState returns what moveGroup thinks is the current state (but often isn't any more), it doesn't consider the startState

when you do planning, if startState is set, it will be used, otherwise "currentState" is used.

[Jorge Nicho]

Ane,

I've been able to plan with attached objects by sending motion plan request through a service call instead of through the move_group_interface.  Essentially, you store a RobotState message with an attached object in the "start_state" field of the Motion Plan Request .  

This link to the  industrial_training repo shows how to attach an object to a RobotState message and this other link shows how to create the Motion Plan Request using a start state.  Hope this helps

[Simon Schmeißer]

Well, if you look at the links I posted above, that's exactly what movegroup is doing internally. What results do you get when you try it, Ane?

[Ane Fernandez]

Yes, I have doing several test, trying to trace most important calls of moveit. During this debugging I realized that althought setStartState method was called after obejct atthaching, for path planning  current_state was being used, instead of  start_state  I review the sentences that I was executing for the path planning and the problem was there, the setStartStateToCurrentState method was called. We need to execute this function because sometimes the joint_state of the move_group wasn't updated and the path planning started from I wrong position.

Thank you very much for your attention

[Bhavya Doshi]

Hi Jorge,

The links that you have posted don't open anymore. I am trying to do the same thing that you mentioned. First I get the planning scene through the service \get_planning_scene which has the robot state with the attached object. Then I store the same robot state in the start_state field of the Motion Plan Request but still the planner doesn't show the attached object.

Any inputs as to what I can do here?

[Jorge Nicho]

Hello Bhavya

Here is a link to the ros-hydro version of that code.  I believe the transforms for the attached object shapes are relative to the robot link onto which it's being attached.  See here for a more through description of the method for attaching objects.  

[Bhavya Doshi]

Hi Jorge,

Thank you very much for the help. I found my mistake and now it's working for me. I really appreciate your quick reply.