moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = group.getPlanningFrame();
/* The id of the object is used to identify it. */
collision_object.id = "box1";
/* Define a box to add to the world. */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.005;
primitive.dimensions[1] = 0.35;
primitive.dimensions[2] = 0.05;
/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose box_pose;
box_pose.orientation.x = eps[0];
box_pose.orientation.y = eps[1];
box_pose.orientation.z = eps[2];
box_pose.orientation.w = eta;
box_pose.position.x =Pos(0);
box_pose.position.y =Pos(1);
box_pose.position.z =Pos(2)+0.4;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);
ROS_INFO("Add an object into the world");
do{
// Now, let's add the collision object into the world
planning_scene_interface.addCollisionObjects(collision_objects);
/* Sleep so we have time to see the object in RViz */
sleep(2.0);