Unable to recognize collision objects on UR5 robotic arm
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
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?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?
What prevents you from upgrading your Polyscope version?
3.4
is really old.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.
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.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 ) ?
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 byur_robot_driver
.