def reset_attached_objects(self): ''' Removes all collision objects that are currently attached to the robot. ''' #and all attached objects reset_attached_objects = AttachedCollisionObject() reset_attached_objects.link_name = 'all' reset_attached_objects.object.header.frame_id = 'base_link' reset_attached_objects.object.header.stamp = rospy.Time.now() reset_attached_objects.object = self._get_reset_object() self._publish(reset_attached_objects, self._attached_object_pub) rospy.logdebug('Attached object reset')
def detach_object(self, arm_name, object_id): ''' Detaches a single object from the arm and removes it from the collision space entirely. The object must have been attached in the world or planning scene diff previously. This removes the object from the planning scene even if it currently exists in the world. **Args:** **arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object **object_id (string):** The ID of the object to detach **Returns:** False if the object was not previously attached in the world or the diff and therefore cannot be detached; True otherwise. ''' obj = AttachedCollisionObject() obj.link_name = self.hands[arm_name].attach_link #did the object ever exist in the world? #if so, we need to continually remove it after detaching #but it will always be in #corresponding collision object if it exists for co in self.current_diff.planning_scene_diff.collision_objects: if co.id == object_id: #if the planning scene originally added this object, we could #just remove it from the list of collision objects but we have #no way of knowing that at this point co.operation.operation = co.operation.REMOVE for p in range(len(co.poses)): co.poses[p] = self.transform_pose(self.world_frame, co.header.frame_id, co.poses[p]) co.header.frame_id = self.world_frame break for ao in self.current_diff.planning_scene_diff.attached_collision_objects: if ao.object.id == object_id: rospy.loginfo('Object was attached by the planning scene interface') self.current_diff.planning_scene_diff.attached_collision_objects.remove(ao) self.send_diff() return True rospy.loginfo('Object was not attached by the planning scene interface') aos = self.attached_collision_objects() for ao in aos: if ao.object.id == object_id: obj.object = ao.object obj.object.operation.operation = obj.object.operation.REMOVE self.current_diff.planning_scene_diff.attached_collision_objects.append(obj) self.send_diff() return True rospy.logwarn('Object '+object_id+' not attached to gripper') return False
def reset_collision_map(self): #remove all objects reset_object = CollisionObject() reset_object.operation.operation = CollisionObjectOperation.REMOVE reset_object.header.frame_id = "base_link" reset_object.header.stamp = rospy.Time.now() reset_object.id = "all" self.object_in_map_pub.publish(reset_object) #and all attached objects reset_attached_objects = AttachedCollisionObject() reset_attached_objects.link_name = "all" reset_attached_objects.object.header.frame_id = "base_link" reset_attached_objects.object.header.stamp = rospy.Time.now() reset_attached_objects.object = reset_object self.attached_object_pub.publish(reset_attached_objects) #and clear the octomap self.clear_octomap() rospy.loginfo("collision objects reset") self.object_in_map_current_id = 0. return 1
def attach_object_to_gripper(self, arm_name, object_id): ''' Attaches an object to the robot's end effector. The object must already exist in the planning scene (although it does not need to exist in the current state of the world). This does NOT "snap" the object to the end effector. Rather, now when the robot moves, the object is assumed to remain stationary with respect to the robot's end effector instead of the world. Collisions will be checked between the object and the world as the object moves, but collisions between the object and the end effector will be ignored. The link the object is attached to and the links with which collisions are ignored are defined by the hand description (see hand_description.py). The object will be attached to the robot according to the current state of the robot and object in the planning scene as maintained by this class (i.e. with any prior changes you made to the diff). To undo this function, you can use detach_object and add_object but you must remember the position of the object where it was originally attached! If you call detach_and_add_back_attached_object, it will add the object back at its current location in the planning scene, NOT at the location at which it was originally attached. The planning scene doesn't support ATTACH_AND_REMOVE so we do those two operations simultaneously, passing an attached object with operation ADD and a collision object with operation REMOVE. **Args:** **arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object to **object_id (string):** The ID of the object to attach **Returns:** False if the object did not previously exist in the world or planning scene diff and therefore cannot be attached; True otherwise. ''' #Note: It is important to send the full collision object (not just the id) in the attached collision object. #This was quite complicated to figure out the first time. obj = AttachedCollisionObject() obj.link_name = self.hands[arm_name].attach_link obj.touch_links = self.hands[arm_name].touch_links diff_obj = None for co in self.current_diff.planning_scene_diff.collision_objects: if co.id == object_id: rospy.loginfo('Object was added to or modified in planning scene.') diff_obj = co break if not diff_obj: rospy.loginfo('Object was not previously added to or modified in planning scene.') diff_obj = self.collision_object(object_id) if not diff_obj: rospy.logerr('Cannot attach object '+object_id+'. This object does not exist') return False self.current_diff.planning_scene_diff.collision_objects.append(diff_obj) self.current_diff.planning_scene_diff.attached_collision_objects.append(obj) #convert it into the frame of the hand - it remains stationary with respect to this for p in range(len(diff_obj.poses)): diff_obj.poses[p] = self.transform_pose(obj.link_name, diff_obj.header.frame_id, diff_obj.poses[p]) diff_obj.header.frame_id = obj.link_name diff_obj.operation.operation = diff_obj.operation.REMOVE obj.object = copy.deepcopy(diff_obj) obj.object.operation.operation = obj.object.operation.ADD self.send_diff() return True
def setUp(self): self.tf = TransformListener() self.move_arm_action_client = actionlib.SimpleActionClient( "move_right_arm", MoveArmAction) att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject) obj_pub = rospy.Publisher('collision_object', CollisionObject) rospy.init_node('test_motion_execution_buffer') #let everything settle down rospy.sleep(5.) obj1 = CollisionObject() obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1) obj1.header.frame_id = "base_footprint" obj1.id = "obj2" obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD obj1.shapes = [Shape() for _ in range(3)] obj1.poses = [Pose() for _ in range(3)] obj1.shapes[0].type = Shape.BOX obj1.shapes[0].dimensions = [float() for _ in range(3)] obj1.shapes[0].dimensions[0] = .5 obj1.shapes[0].dimensions[1] = 1.0 obj1.shapes[0].dimensions[2] = .2 obj1.poses[0].position.x = .95 obj1.poses[0].position.y = -.25 obj1.poses[0].position.z = .62 obj1.poses[0].orientation.x = 0 obj1.poses[0].orientation.y = 0 obj1.poses[0].orientation.z = 0 obj1.poses[0].orientation.w = 1 obj1.shapes[2].type = Shape.BOX obj1.shapes[2].dimensions = [float() for _ in range(3)] obj1.shapes[2].dimensions[0] = .5 obj1.shapes[2].dimensions[1] = .1 obj1.shapes[2].dimensions[2] = 1.0 obj1.poses[2].position.x = .95 obj1.poses[2].position.y = -.14 obj1.poses[2].position.z = 1.2 obj1.poses[2].orientation.x = 0 obj1.poses[2].orientation.y = 0 obj1.poses[2].orientation.z = 0 obj1.poses[2].orientation.w = 1 obj1.shapes[1].type = Shape.BOX obj1.shapes[1].dimensions = [float() for _ in range(3)] obj1.shapes[1].dimensions[0] = .5 obj1.shapes[1].dimensions[1] = .1 obj1.shapes[1].dimensions[2] = 1.0 obj1.poses[1].position.x = .95 obj1.poses[1].position.y = .12 obj1.poses[1].position.z = 1.2 obj1.poses[1].orientation.x = 0 obj1.poses[1].orientation.y = 0 obj1.poses[1].orientation.z = 0 obj1.poses[1].orientation.w = 1 obj_pub.publish(obj1) att_object = AttachedCollisionObject() att_object.object.header.stamp = rospy.Time.now() att_object.object.header.frame_id = "r_gripper_r_finger_tip_link" att_object.link_name = "r_gripper_r_finger_tip_link" att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD att_object.object = CollisionObject() att_object.object.header.stamp = rospy.Time.now() att_object.object.header.frame_id = "r_gripper_r_finger_tip_link" att_object.object.id = "pole" att_object.object.shapes = [Shape() for _ in range(1)] att_object.object.shapes[0].type = Shape.CYLINDER att_object.object.shapes[0].dimensions = [float() for _ in range(2)] att_object.object.shapes[0].dimensions[0] = .02 att_object.object.shapes[0].dimensions[1] = .1 att_object.object.poses = [Pose() for _ in range(1)] att_object.object.poses[0].position.x = -.02 att_object.object.poses[0].position.y = .04 att_object.object.poses[0].position.z = 0 att_object.object.poses[0].orientation.x = 0 att_object.object.poses[0].orientation.y = 0 att_object.object.poses[0].orientation.z = 0 att_object.object.poses[0].orientation.w = 1 att_pub.publish(att_object) rospy.sleep(5.0)
def test_add_convert_objects(): obj_pub = rospy.Publisher('collision_object',CollisionObject) att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject) rospy.init_node('test_collision_objects') #let everything settle down rospy.sleep(5.) obj1 = CollisionObject() obj1.header.stamp = rospy.Time.now() obj1.header.frame_id = "base_link" obj1.id = "obj1"; obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD obj1.shapes = [Shape() for _ in range(1)] obj1.shapes[0].type = Shape.BOX obj1.shapes[0].dimensions = [float() for _ in range(3)] obj1.shapes[0].dimensions[0] = .1 obj1.shapes[0].dimensions[1] = .1 obj1.shapes[0].dimensions[2] = .75 obj1.poses = [Pose() for _ in range(1)] obj1.poses[0].position.x = .6 obj1.poses[0].position.y = -.6 obj1.poses[0].position.z = .375 obj1.poses[0].orientation.x = 0 obj1.poses[0].orientation.y = 0 obj1.poses[0].orientation.z = 0 obj1.poses[0].orientation.w = 1 att_obj = AttachedCollisionObject() att_obj.link_name = "r_gripper_palm_link" att_obj.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link', 'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link', 'r_wrist_roll_link', 'r_wrist_flex_link', 'r_forearm_link', 'r_gripper_motor_accelerometer_link'] obj2 = CollisionObject() obj2.header.stamp = rospy.Time.now() obj2.header.frame_id = "r_gripper_palm_link" obj2.id = "obj2"; obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD obj2.shapes = [Shape() for _ in range(1)] obj2.shapes[0].type = Shape.CYLINDER obj2.shapes[0].dimensions = [float() for _ in range(2)] obj2.shapes[0].dimensions[0] = .025 obj2.shapes[0].dimensions[1] = .5 obj2.poses = [Pose() for _ in range(1)] obj2.poses[0].position.x = .12 obj2.poses[0].position.y = 0 obj2.poses[0].position.z = 0 obj2.poses[0].orientation.x = 0 obj2.poses[0].orientation.y = 0 obj2.poses[0].orientation.z = 0 obj2.poses[0].orientation.w = 1 att_obj.object = obj2 r = rospy.Rate(.1) while(True): obj1.header.stamp = rospy.Time.now() obj_pub.publish(obj1) att_obj.object.header.stamp = rospy.Time.now() att_pub.publish(att_obj) r.sleep()
def setUp(self): self.tf = TransformListener() self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction) att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject) obj_pub = rospy.Publisher('collision_object',CollisionObject) rospy.init_node('test_motion_execution_buffer') #let everything settle down rospy.sleep(5.) obj1 = CollisionObject() obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1) obj1.header.frame_id = "base_footprint" obj1.id = "obj2"; obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD obj1.shapes = [Shape() for _ in range(3)] obj1.poses = [Pose() for _ in range(3)] obj1.shapes[0].type = Shape.BOX obj1.shapes[0].dimensions = [float() for _ in range(3)] obj1.shapes[0].dimensions[0] = .5 obj1.shapes[0].dimensions[1] = 1.0 obj1.shapes[0].dimensions[2] = .2 obj1.poses[0].position.x = .95 obj1.poses[0].position.y = -.25 obj1.poses[0].position.z = .62 obj1.poses[0].orientation.x = 0 obj1.poses[0].orientation.y = 0 obj1.poses[0].orientation.z = 0 obj1.poses[0].orientation.w = 1 obj1.shapes[2].type = Shape.BOX obj1.shapes[2].dimensions = [float() for _ in range(3)] obj1.shapes[2].dimensions[0] = .5 obj1.shapes[2].dimensions[1] = .1 obj1.shapes[2].dimensions[2] = 1.0 obj1.poses[2].position.x = .95 obj1.poses[2].position.y = -.14 obj1.poses[2].position.z = 1.2 obj1.poses[2].orientation.x = 0 obj1.poses[2].orientation.y = 0 obj1.poses[2].orientation.z = 0 obj1.poses[2].orientation.w = 1 obj1.shapes[1].type = Shape.BOX obj1.shapes[1].dimensions = [float() for _ in range(3)] obj1.shapes[1].dimensions[0] = .5 obj1.shapes[1].dimensions[1] = .1 obj1.shapes[1].dimensions[2] = 1.0 obj1.poses[1].position.x = .95 obj1.poses[1].position.y = .12 obj1.poses[1].position.z = 1.2 obj1.poses[1].orientation.x = 0 obj1.poses[1].orientation.y = 0 obj1.poses[1].orientation.z = 0 obj1.poses[1].orientation.w = 1 obj_pub.publish(obj1) att_object = AttachedCollisionObject() att_object.object.header.stamp = rospy.Time.now() att_object.object.header.frame_id = "r_gripper_r_finger_tip_link" att_object.link_name = "r_gripper_r_finger_tip_link" att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD att_object.object = CollisionObject(); att_object.object.header.stamp = rospy.Time.now() att_object.object.header.frame_id = "r_gripper_r_finger_tip_link" att_object.object.id = "pole" att_object.object.shapes = [Shape() for _ in range(1)] att_object.object.shapes[0].type = Shape.CYLINDER att_object.object.shapes[0].dimensions = [float() for _ in range(2)] att_object.object.shapes[0].dimensions[0] = .02 att_object.object.shapes[0].dimensions[1] = .1 att_object.object.poses = [Pose() for _ in range(1)] att_object.object.poses[0].position.x = -.02 att_object.object.poses[0].position.y = .04 att_object.object.poses[0].position.z = 0 att_object.object.poses[0].orientation.x = 0 att_object.object.poses[0].orientation.y = 0 att_object.object.poses[0].orientation.z = 0 att_object.object.poses[0].orientation.w = 1 att_pub.publish(att_object) rospy.sleep(5.0)
def get_obj(): attached_obj = AttachedCollisionObject() obj = CollisionObject() shape = Shape() pose = Pose() vert = Point() verts = [] in_verts = True in_position = True f = open('test.txt', 'r') for line in f: fields = line.split(':') fields = [fields[i].strip() for i in range(len(fields))] # print fields if fields[0] == "frame_id": obj.header.frame_id = fields[1] elif fields[0] == "id": obj.id = fields[1] elif fields[0] == "operation" and fields[1] != "": obj.operation.operation = int(fields[1]) elif fields[0] == "type": shape.type = int(fields[1]) elif fields[0] == "triangles": array = fields[1][1:-2] ind = array.split(',') inds = [int(i) for i in ind] shape.triangles = inds elif fields[0] == "x" and in_verts: vert = Point() vert.x = float(fields[1]) elif fields[0] == "y" and in_verts: vert.y = float(fields[1]) elif fields[0] == "z" and in_verts: vert.z = float(fields[1]) verts.append(vert) elif fields[0] == "poses": in_verts = False elif fields[0] == "x" and in_position: pose.position.x = float(fields[1]) elif fields[0] == "y" and in_position: pose.position.y = float(fields[1]) elif fields[0] == "z" and in_position: pose.position.z = float(fields[1]) in_position = False elif fields[0] == "x" and not in_position: pose.orientation.x = float(fields[1]) elif fields[0] == "y" and not in_position: pose.orientation.y = float(fields[1]) elif fields[0] == "z" and not in_position: pose.orientation.z = float(fields[1]) elif fields[0] == "w": pose.orientation.w = float(fields[1]) obj.id = "graspable_object_1001" shape.vertices = verts obj.shapes = [shape] obj.poses = [pose] attached_obj.object = obj return attached_obj