Problem with dynamic collision detection and octomap

[Teresa Seco]

Hello,

We are working with an UR10 and a kinect sensor in the enviroment that generates an octomap. We can see through RVIZ how any object tha appears in the scene is detected.

We have problems with the robot when a object appears in the scene during the displacement of the robot. The operation sequence is:

- First, there in no object in scene and we send a target pose to the robot
- The UR starts its movement
- We add an object in the trajectory of the robot and the octomap update the scene. It is possible to see the object included in the octomap with RVIZ
- In theory, the robot should stop its movement but it does not stop

If the object is in the scene from the begining, the planner provides a trajectory avoiding the object, but if the object appears once the robot has started its movement, the robot does not stop.

Any suggestions?

Best regards,

Teresa

[Qiang Qiu]

Hi Teresa,

the planning_scene::PlanningScene::isPathValid() can be called to check the path validity. you can use it, and call /follow_joint_trajectory/cancel when the path is in-collision.

[Teresa Seco]

Hi,

I have tried with your suggestion but it does not work. Perhaps I am doing something wrong...

I get the planning scene with:

planning_scene::PlanningScene *planning_scene_;

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();

planning_scene_ = new planning_scene::PlanningScene(kinematic_model);

When the  planner provides the plan  for a particular target pose, the start_state and the trajectory are stored:

   

    moveit::planning_interface::MoveGroup::Plan plan;
    bool success = group_->plan(plan);
    ROS_INFO("Visualizing plan  (pose goal) %s",success?"":"FAILED");

    group_->asyncExecute(plan);
    start_state_ = plan.start_state_;
    trajectory_ = plan.trajectory_;

Then, in the spin function, the isvalidPath is checked during the robot movement.

void UR10_collision_control::spin(const ros::TimerEvent& e)
{
   
        bool ispathvalid = planning_scene_->isPathValid(start_state_,trajectory_,"manipulator");

        if (!ispathvalid)
        {
            ROS_INFO("Invalid path");
            group_->stop();
        }
}

The operation is the described one in my first post. The new object appears in the octomap but the isValidPath function does not returns false never.

Any suggestions?

[Simon Schmeißer]

A PlanningScene is a description of an environment at a fixed time. Your code creates a new environment containing nothing but the robot ("robot_description"). If you want to have information (collision or not?) about the current environment, somebody needs to keep track of all collision_object_msgs or octomap_msgs that were published since you started your system. This is what a PlanningSceneMonitor does. You can either have your own instance (there can be as many as you want/have memory) or you can use the one from MoveGroup: https://github.com/ros-planning/moveit_ros/blob/jade-devel/move_group/src/default_capabilities/get_planning_scene_service_capability.h

If you use your own PlanningSceneMonitor, use
planning_scene_monitor::LockedPlanningSceneRO ps(m_planningSceneMonitor);
ps->isPathValid(....
(remember to let that object go out of scope)

[Qiang Qiu]

As Simon said, you should use PlanningSceneMonitor, and update the planning scene before calling isPathValid()

[Dina Youakim]

Hi all,

I am interested in this thread, as I am facing exactly same problem as Teresa. Octomap updates or even adding boxes manually to the planning_scene are not taken into account during path execution.

The only difference I have is that I tried to used group.move() directly (which goes to different execution path in MoveIt! as it passes by plan_execution.cpp), the good thing about it is that it checks for new scene updates by itself, but the only thing that does not work is that the call "isRemainingPathValid" returns true even though I see it is invalid. I debugged more and I saw the first if condition in this function [if (path_segment.first >= 0 && path_segment.second >= 0 && plan.plan_components_[path_segment.first].trajectory_monitoring_)] is failing because of the second condition (it returns -1). I tried to understand more why this condition and how these segments are computed in execution manager but I am lost.

Also once removing this if condition, it shows there is collision and stops the execution.

I would appreciate any help as I am not sure I went in the right direction.

Cheers,

[v4hn]

Heya,

On Tue, Jun 14, 2016 at 03:50:24AM -0700, Dina Youakim wrote:
> I am interested in this thread, as I am facing exactly same problem as
> Teresa. Octomap updates or even adding boxes manually to the planning_scene
> are not taken into account during path execution.
> The only difference I have is that I tried to used group.move() directly
> (which goes to different execution path in MoveIt! as it passes by
> plan_execution.cpp), the good thing about it is that it checks for new
> scene updates by itself, but the only thing that does not work is that the
> call "isRemainingPathValid" returns true even though I see it is invalid. I
> debugged more and I saw the first if condition in this function [if
> (path_segment.first >= 0 && path_segment.second >= 0 && plan.
> plan_components_[path_segment.first].trajectory_monitoring_)] is failing
> because of the second condition (it returns -1). I tried to understand more
> why this condition and how these segments are computed in execution manager
> but I am lost.

Nice analysis.

The path_segment.second corresponds to the index to the current waypoint.
According to [0] this is set to -1 when the time index map is empty.
To me, this sounds like your generated trajectories are missing time stamps.

Nevertheless, technically, there is no reason to abort collision checking
in this case and the rest of the code works fine. So I agree, this check could
be removed. One thing to keep in mind though, is that in this case,
the *whole* trajectory has to be valid at each point during the execution,
not just the remaining part. Thus, the execution will probably abort if
you place an obstacle in an already executed part of the trajectory.
Could you create a pull request for this on the github project?

> I would appreciate any help as I am not sure I went in the right direction.

You did :-)


v4hn

---
[0] - https://github.com/ros-planning/moveit_ros/blob/indigo-devel/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp#L1284

[Dina Youakim]

Hey,

Thank you so much for your reply. It makes perfect sense this *whole* trajectory validity.

I have been analyzing a bit more the execution manager First I verified that my trajectory points have time stamp.
I think the  "trajectory" "isRemainingPathValid" fails because it passes this as an argument [trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex()]
And the indices checked by the later function are filled in trajectory_execution_manager_->ExcutePart at the end after sending the whole path to the controller.
So what happens if the get function is called before that it goes to second condition  if (time_index_.empty()) and it returns -1 for the second segment.
I came out with a change for this line of code to replace it by:

if (time_index_.empty())
    return std::make_pair((int)current_context_, trajectories_[current_context_]->trajectory_parts_.size());

and in this case it recognizes that then environment has changed.

I am still not sure this is the best solution, and I have some doubts:

1- At any point point of time MoveIt! re-planning was working with dynamic environment ?

2- The fact that the time_index is filled at the end of ExecutePart so that getCurrentExpectedTrajectoryIndex fails at the time it is called for me is not normal behavior. Not sure is it me doing something wrong or it was like this from the beginning.

3- I am confused about the trajectory notion. Cause one trajectory has several points while there is a possibility to have more than one trajectory sent to the execution manager. Does it handle this ? as I see there is ExecutePart (per my understanding a part is a single trajectory not a point in the trajectory) and there are trajectories_ vector versus trajectory_parts. Am I understanding correctly ?

Cheers,
Dina.