Using Descartes with attached collision objects

[julien-audet]

Hey all,

I've been using Descartes with collision objects without problem, but I didn't manage to use it with attached collision objects (collision is ignored between the attached collision object and the robot + environment). In my case, I want to use an attached collision object as a tool to facilitate tool changing, but the tool currently ignores collision with the robot/environment.

It is possible to use Descartes with attached collision objects?

I'm using the kinetic version of Descartes and I modified Descartes according to this tutorial for enabling collision since moveit planning scene was not being updated correctly: http://wiki.ros.org/descartes/Tutorials/Robot%20Welding%20With%20Descartes

If this is not an issue since Descartes is not supposed to work with attached collision objects, I'll close this issue right away.

[julien-audet]

I think I managed to use an attached collision object (as a tool) by further modifying Descartes. The planning now fails if the tool touches the environment and doesn't if the path is valid. Although, it is at least 10x slower than before to plan a path. I haven't tested it that much though. Here's what I did :

bool MoveitStateAdapter::isInCollision(const std::vector<double>& joint_pose) const
{
  bool in_collision = false;
  if (check_collisions_)
  {
    robot_state::RobotState state(planning_scene_.getCurrentState()); // ADDED
//    moveit::core::RobotState state (robot_model_ptr_); // REMOVED
    state.setToDefaultValues();
    state.setJointGroupPositions(joint_group_, joint_pose);
    in_collision = planning_scene_->isStateColliding(state, group_name_);

  }
  return in_collision;
}

Where planningscene is updated at the end of MoveitStateAdapter::initialize() :

  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_ptr =
  std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
  planning_scene_monitor_ptr->requestPlanningSceneState();
  planning_scene_monitor::LockedPlanningSceneRO ps(planning_scene_monitor_ptr);
  planning_scene_ = ps->diff();
  planning_scene_->decoupleParent();

Do you guys have any idea how to speed it up?

[mrunaljsarvaiya]

I used a similar approach to collision check with attached collision objects. I modified the code further and used checkCollision function instead. I don't know if it's faster or slower than using isStateColliding but you can specify some parameters in the collision_detection::CollisionRequest that might speed things up.

[dbdxnuliba]

I used a similar approach to collision check with attached collision objects. I modified the code further and used checkCollision function instead. I don't know if it's faster or slower than using isStateColliding but you can specify some parameters in the collision_detection::CollisionRequest that might speed things up.

hello , can you show me the function you modified about bool MoveitStateAdapter::isInCollision(const std::vector& joint_pose) const because I has encountered robot-collison with environment
show as the following picture:

[mrunaljsarvaiya]

I used a similar approach to collision check with attached collision objects. I modified the code further and used checkCollision function instead. I don't know if it's faster or slower than using isStateColliding but you can specify some parameters in the collision_detection::CollisionRequest that might speed things up.

hello , can you show me the function you modified about bool MoveitStateAdapter::isInCollision(const std::vector& joint_pose) const because I has encountered robot-collison with environment show as the following picture:

Here's how I modified the function

bool descartes_moveit::MoveitStateAdapter::isInCollision(const std::vector<double>& joint_pose) const
{
  bool in_collision = false;

  if (check_collisions_)
  {
    collision_detection::CollisionResult collision_result;

    moveit::core::RobotState state = planning_scene_->getCurrentStateNonConst();
    state.setJointGroupPositions(joint_group_, joint_pose);

    planning_scene_->checkCollision(collision_request_, collision_result, state, acm_);
    in_collision = collision_result.collision;

    if (in_collision){
      collision_detection::CollisionResult::ContactMap::const_iterator it;
      for ( it = collision_result.contacts.begin(); it != collision_result.contacts.end(); it++ )
      {
        ROS_WARN_STREAM_THROTTLE(0.5, "Contact between: "<<it->first.first.c_str()<<" and "<<it->first.second.c_str());
      }
    }

  }

  return in_collision;
}