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')
Beispiel #2
0
 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 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 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 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
Beispiel #10
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)        
Beispiel #12
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