Unable to recognize collision objects on UR5 robotic arm

asked 2021-11-09 09:23:53 -0600

zahid990170 gravatar image

updated 2021-11-09 11:23:45 -0600

Hi,

I need some help adding collision objects to the environment.

I am creating an obstacle primitive and adding it to the planning scene in the following manner:

void addCollisionObject()
{
      moveit_msgs::CollisionObject m_CollisionObject;
      std::vector<moveit_msgs::CollisionObject> collision_objects; 
      shape_msgs::SolidPrimitive primitive;
      geometry_msgs::Pose a_pose;

      primitive.type = primitive.BOX;
      primitive.dimensions.resize(3);
      primitive.dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.065; //0.265;
      primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.24;
      primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.20;

      a_pose.orientation.w = 1.0;
      a_pose.position.x = 0.280578; //0.180578; // + x_pos;       // 0.30;
      a_pose.position.y = -0.405245; //-0.305245 + y_pos;  // -0.093
      a_pose.position.z = 0.378328;//0.40;       //  0.50;

      m_CollisionObject.primitives.push_back(primitive);
      m_CollisionObject.primitive_poses.push_back(a_pose);
      m_CollisionObject.operation = m_CollisionObject.ADD;

     collision_objects.push_back(m_CollisionObject);
     m_PlanningSceneInterface.addCollisionObjects(collision_objects);     
}

I am testing motion plans on UR5 robotic arm. On RViz I can see, the colllision object being added to the scene and also the robot motion takes into account, avoiding the obstacle when possible.

However, I tried to run the same motion on actual robotic arm. My sequence of commands is the following.

roslaunch ur_modern_driver ur5_bringup_compatible.launch robot_ip:="192.168.0.103"
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=false
roslaunch p4_moveit_pkg motion_node.launch option:="4"

It appears as if the actual robotic arm does not recognize there is an obstacle added. It follows the same motion to the goal, with or without the obstacle.

Also, earlier, I tried publishing to the /collision_object topic, but using this approach, no obstacle was appearing even within RViz. Do I need a particular listener on that topic for the obstacle to appear.

Edit

I have a base class CMotionPlanner that has the following protected members

boost::scoped_ptr<moveit::planning_interface::MoveGroupInterface> m_MoveGroup;
moveit::planning_interface::PlanningSceneInterface          m_PlanningSceneInterface;

In particular, I am using the methods such as m_MoveGroup->move() or m_MoveGroup->plan() for respective functionalities. Also, I have another class that is responsible for adding obstacles to the scene. This class inherits from CMotionPlanner, i.e., class CCollisionObject : public CMotionPlanner. The function addCollisionObject is a member of CCollisionObject.

When testing on RViz, I use the following sequence of commands

roslaunch p4_moveit_pkg demo.launch
roslaunch p4_moveit_pkg motion_node.launch option:="4"

I do receive the following output on the terminal where I launch demo.launch.

[ INFO] [1636475429.915064241]: Starting planning scene monitor
[ INFO] [1636475429.917320608]: Listening to '/move_group/monitored_planning_scene'

However, I do not see, any such output when working with robotic arm. Do, I have to manually start the planning scene monitor somewhere.

Thanks,

Zahid

edit retag flag offensive close merge delete

Comments

I don't know what's causing this, but it's very likely the "robot arm" is not involved here. From what you describe this is all on the MoveIt side.

The driver does not change anything on the MoveIt side.

Also (but off-topic): you are aware ur_modern_driver is deprecated?

gvdhoorn gravatar image gvdhoorn  ( 2021-11-09 09:37:12 -0600 )edit

thanks @gvdhoorn, don't know if it can add anything to my question, please refer to the edit to this question. We have PolyScope version 3.4 on UR5. Could you please recommend another driver for this version. I remember, perviously that new driver was supporting versions 3.7 and above?

zahid990170 gravatar image zahid990170  ( 2021-11-09 10:06:28 -0600 )edit

What prevents you from upgrading your Polyscope version? 3.4 is really old.

gvdhoorn gravatar image gvdhoorn  ( 2021-11-09 12:15:23 -0600 )edit

Probably the robotic arm came with version 3.4. I think, Its an apprehension that something might go wrong setting up the Polyscope, the URCAp and the new driver :( leaving the robotic arm inconsistent state with not enough expertise available to fix.

zahid990170 gravatar image zahid990170  ( 2021-11-10 05:54:20 -0600 )edit

Whether it's worth it is something only you (or your colleagues / teammates) can decide, but the new driver is so much better than the previous one, you're denying yourself quite some improved performance, robustness and level of integration.

Besides that you're currently using deprecated software (ie: ur_modern_driver) which is unmaintained and has known issues.

gvdhoorn gravatar image gvdhoorn  ( 2021-11-10 06:01:20 -0600 )edit

ok, thanks, I will give it some thought and try. But, does upgrading the PolyScope and using the new driver need any changes on the code side ( i..e., can I keep the currently used programs ) ?

zahid990170 gravatar image zahid990170  ( 2021-11-10 07:09:59 -0600 )edit

Without knowing your "currently used programs" I cannot say anything with certainty. And even if I knew them, I still couldn't, as there are too many factors which could influence this.

If you are asking about the ROS side: the driver interfaces (ie: topics, services and actions) are largely the same, with the new driver offering a much better ROS API and integration. So ROS nodes which make use of the ROS API of ur_modern_driver most likely work without change.

The only major difference would be the "low bandwidth mode" of ur_modern_driver. That's not supported by ur_robot_driver.

gvdhoorn gravatar image gvdhoorn  ( 2021-11-10 10:02:28 -0600 )edit