Ejemplo n.º 1
0
    def to_collision_object(self):
        """
        :return: the map as a collision object
        :type: CollisionObject
        """
        co = CollisionObject()
        co.header.frame_id = "/odom_combined"
        co.id = "map"
        primitive = SolidPrimitive()
        primitive.type = SolidPrimitive.BOX
        primitive.dimensions.append(self.cell_size)
        primitive.dimensions.append(self.cell_size)
        primitive.dimensions.append(2)

        for x in range(0, len(self.field)):
            for y in range(0, len(self.field[x])):
                if self.field[x][y].is_free() or self.field[x][y].is_object():
                    continue
                if self.field[x][y].is_obstacle():
                    primitive.dimensions[primitive.BOX_Z] = self.get_cell_by_index(x, y).highest_z * 2
                else:
                    primitive.dimensions[primitive.BOX_Z] = self.get_average_z_of_surrounded_obstacles(x, y) * 2
                primitive.dimensions[primitive.BOX_Z] += 0.02
                co.primitives.append(deepcopy(primitive))
                pose = Pose()
                (pose.position.x, pose.position.y) = self.index_to_coordinates(x, y)
                pose.orientation.w = 1
                co.primitive_poses.append(pose)

        return co
Ejemplo n.º 2
0
def calculate_grasp_position_list(collision_object, transform_func):
    """
    Calculates grasp positions for a composition of collision objects.
    :param collision_object: object composition to be grapsed
    :type: CollisionObject
    :param transform_func(object, frame_id): transform function
    :return: list of graps positions
    :type: [PoseStamped]
    """
    grasp_positions = []
    for i in range(0, len(collision_object.primitives)):
        co = CollisionObject()
        co.id = collision_object.id
        co.primitives.append(collision_object.primitives[i])
        co.header = collision_object.header
        co.primitive_poses.append(collision_object.primitive_poses[i])
        temp = calculate_grasp_position(co, transform_func, False, 4)

        for j in range(0, len(temp)):
            tmp_pose = transform_func(co, "/" + collision_object.id).primitive_poses[0].position
            temp[j].pose.position = add_point(temp[j].pose.position, tmp_pose)

        grasp_positions.extend(temp)

    return grasp_positions
Ejemplo n.º 3
0
    def test2_1(self):

        co = CollisionObject()
        co.operation = CollisionObject.ADD
        co.id = "muh"
        co.header.frame_id = "/odom_combined"
        cylinder = SolidPrimitive()
        cylinder.type = SolidPrimitive.CYLINDER
        cylinder.dimensions.append(0.3)
        cylinder.dimensions.append(0.03)
        co.primitives = [cylinder]
        co.primitive_poses = [Pose()]
        co.primitive_poses[0].position = Point(1.2185, 0, 0)
        co.primitive_poses[0].orientation = Quaternion(0, 0, 0, 1)

        box = SolidPrimitive()
        box.type = SolidPrimitive.BOX
        box.dimensions.append(0.1)
        box.dimensions.append(0.1)
        box.dimensions.append(0.1)
        co.primitives.append(box)
        co.primitive_poses.append(Pose())
        co.primitive_poses[1].position = Point(1.1185, 0, 0)
        co.primitive_poses[1].orientation = Quaternion(0, 0, 0, 1)

        co.primitives.append(box)
        co.primitive_poses.append(Pose())
        co.primitive_poses[2].position = Point(0, 0, 0)
        co.primitive_poses[2].orientation = Quaternion(0, 0, 0, 1)

        p = PoseStamped()
        p.header.frame_id = "/odom_combined"
        p.pose.position = Point(1, 0, 0)
        p.pose.orientation = euler_to_quaternion(0, 0, 0)
        self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
Ejemplo n.º 4
0
    def to_collision_object(self):
        """
        :return: the map as a collision object
        :type: CollisionObject
        """
        co = CollisionObject()
        co.header.frame_id = "/odom_combined"
        co.id = "map"
        primitive = SolidPrimitive()
        primitive.type = SolidPrimitive.BOX
        primitive.dimensions.append(self.cell_size)
        primitive.dimensions.append(self.cell_size)
        primitive.dimensions.append(2)

        for x in range(0, len(self.field)):
            for y in range(0, len(self.field[x])):
                if self.field[x][y].is_free() or self.field[x][y].is_object():
                    continue
                if self.field[x][y].is_obstacle():
                    primitive.dimensions[
                        primitive.BOX_Z] = self.get_cell_by_index(
                            x, y).highest_z * 2
                else:
                    primitive.dimensions[
                        primitive.
                        BOX_Z] = self.get_average_z_of_surrounded_obstacles(
                            x, y) * 2
                primitive.dimensions[primitive.BOX_Z] += 0.02
                co.primitives.append(deepcopy(primitive))
                pose = Pose()
                (pose.position.x,
                 pose.position.y) = self.index_to_coordinates(x, y)
                pose.orientation.w = 1
                co.primitive_poses.append(pose)

        return co
Ejemplo n.º 5
0
 def make_cylinder(name, pose, size):
     """
     Creates a cylinder collision object.
     :param name: name of the cylinder
     :type: str
     :param pose: position of the cylinder
     :type: PoseStamped
     :param size: cylinder size
     :type: [float(height), float(radius)]
     :return: cylinder collisionobject
     :type: CollisionObject
     """
     co = CollisionObject()
     co.operation = CollisionObject.ADD
     co.id = name
     co.header = pose.header
     cylinder = SolidPrimitive()
     cylinder.type = SolidPrimitive.CYLINDER
     cylinder.dimensions = list(size)
     co.primitives = [cylinder]
     co.primitive_poses = [pose.pose]
     return co
Ejemplo n.º 6
0
 def make_box(name, pose, size):
     """
     Creates a box collision object.
     :param name: name of the box
     :type: str
     :param pose: position of the box
     :type: PoseStamped
     :param size: box size
     :type: [float(x), float(y), float(z)]
     :return: box collision object
     :type: CollisionObject
     """
     co = CollisionObject()
     co.operation = CollisionObject.ADD
     co.id = name
     co.header = pose.header
     box = SolidPrimitive()
     box.type = SolidPrimitive.BOX
     box.dimensions = list(size)
     co.primitives = [box]
     co.primitive_poses = [pose.pose]
     return co
 def make_box(name, pose, size):
     """
     Creates a box collision object.
     :param name: name of the box
     :type: str
     :param pose: position of the box
     :type: PoseStamped
     :param size: box size
     :type: [float(x), float(y), float(z)]
     :return: box collision object
     :type: CollisionObject
     """
     co = CollisionObject()
     co.operation = CollisionObject.ADD
     co.id = name
     co.header = pose.header
     box = SolidPrimitive()
     box.type = SolidPrimitive.BOX
     box.dimensions = list(size)
     co.primitives = [box]
     co.primitive_poses = [pose.pose]
     return co
 def make_cylinder(name, pose, size):
     """
     Creates a cylinder collision object.
     :param name: name of the cylinder
     :type: str
     :param pose: position of the cylinder
     :type: PoseStamped
     :param size: cylinder size
     :type: [float(height), float(radius)]
     :return: cylinder collisionobject
     :type: CollisionObject
     """
     co = CollisionObject()
     co.operation = CollisionObject.ADD
     co.id = name
     co.header = pose.header
     cylinder = SolidPrimitive()
     cylinder.type = SolidPrimitive.CYLINDER
     cylinder.dimensions = list(size)
     co.primitives = [cylinder]
     co.primitive_poses = [pose.pose]
     return co
Ejemplo n.º 9
0
    def test2_1(self):

        co = CollisionObject()
        co.operation = CollisionObject.ADD
        co.id = "muh"
        co.header.frame_id = "/odom_combined"
        cylinder = SolidPrimitive()
        cylinder.type = SolidPrimitive.CYLINDER
        cylinder.dimensions.append(0.3)
        cylinder.dimensions.append(0.03)
        co.primitives = [cylinder]
        co.primitive_poses = [Pose()]
        co.primitive_poses[0].position = Point(1.2185, 0,0)
        co.primitive_poses[0].orientation = Quaternion(0,0,0,1)

        box = SolidPrimitive()
        box.type = SolidPrimitive.BOX
        box.dimensions.append(0.1)
        box.dimensions.append(0.1)
        box.dimensions.append(0.1)
        co.primitives.append(box)
        co.primitive_poses.append(Pose())
        co.primitive_poses[1].position = Point(1.1185, 0,0)
        co.primitive_poses[1].orientation = Quaternion(0,0,0,1)

        co.primitives.append(box)
        co.primitive_poses.append(Pose())
        co.primitive_poses[2].position = Point(0, 0,0)
        co.primitive_poses[2].orientation = Quaternion(0,0,0,1)


        p = PoseStamped()
        p.header.frame_id = "/odom_combined"
        p.pose.position = Point(1,0,0)
        p.pose.orientation = euler_to_quaternion(0,0,0)
        self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
Ejemplo n.º 10
0
    def execute(self, taskdata):
        rospy.loginfo('Executing state PlaceObject')

        destination = taskdata.place_position

        if self._retry == 2:
            rospy.logwarn('Failed to place two times. Dropping object.')
            self._retry = 0
            if not utils.manipulation.open_gripper():
                #cant happen
                taskdata.failed_object = EurocObject()
                return 'fail'

        if taskdata.enable_movement:
            move_to_func = utils.manipulation.move_arm_and_base_to
        else:
            move_to_func = utils.manipulation.move_to

        co = utils.manipulation.get_planning_scene().get_attached_object()
        if co is None:
            self._retry = 0
            taskdata.failed_object = EurocObject()
            return 'noObjectAttached'
        co = co.object
        rospy.logdebug("Placing: " + str(co))

        destination = utils.manipulation.transform_to(destination)
        rospy.logdebug("at::: " + str(destination))
        if taskdata.yaml.task_type == Task.TASK_5:
            destination.point.z = 0.4
            dest_orientation = deepcopy(taskdata.yaml.puzzle_fixture.orientation)
            place_poses = get_place_position_for_puzzle(destination, dest_orientation)
        else:
            place_poses = get_place_position(co, destination, utils.manipulation.transform_to, taskdata.dist_to_obj,
                                         taskdata.grasp)
        print place_poses
        # place_poses = utils.map.filter_invalid_poses3(destination.point.x, destination.point.y, place_poses)
        if not taskdata.enable_movement:
            place_poses = utils.manipulation.filter_close_poses(place_poses)

        for place_pose in place_poses:
            rospy.logdebug("Try to place at: " + str(place_pose))

            if taskdata.yaml.task_type != Task.TASK_5:
                if not move_to_func(get_pre_place_position(place_pose), do_not_blow_up_list=(co.id)):
                    rospy.logwarn("Can't reach preplaceposition.")
                    continue
                else:
                    rospy.logdebug("preplaceposition taken")

                time.sleep(0.5)
                rospy.sleep(1)

            if not move_to_func(place_pose, do_not_blow_up_list=(co.id, "map")):
                rospy.logwarn("Can't reach placeposition.")
                continue
            else:
                rospy.logdebug("placeposition taken")

            time.sleep(0.5)
            rospy.sleep(1)

            self._retry = 0
            gripper_target = min(utils.manipulation.get_current_gripper_state()[1] + 0.002, gripper_max_pose)
            if not utils.manipulation.open_gripper(gripper_target):
                #cant happen
                taskdata.failed_object = EurocObject()
                return 'fail'

            rospy.sleep(3)
            if not utils.manipulation.open_gripper():
                #cant happen
                taskdata.failed_object = EurocObject()
                return 'fail'

            if taskdata.yaml.task_type == Task.TASK_5:
                utils.manipulation.get_planning_scene().remove_object(co.id)

            time.sleep(0.5)
            rospy.sleep(1)

            # post_place_pose = utils.manipulation.transform_to(place_pose, co.id)
            if not move_to_func(get_pre_grasp(place_pose), do_not_blow_up_list=(co.id, "map")) and \
                    not move_to_func(get_pre_place_position(place_pose), do_not_blow_up_list=(co.id, "map")):
                rospy.logwarn("Can't reach postplaceposition. Continue anyway")
                taskdata.failed_object = EurocObject()
                self._retry = 0
                return 'success'
            else:
                rospy.logdebug("postplaceposition taken")
            rospy.sleep(0.25)
            rospy.loginfo("placed " + co.id)
            taskdata.failed_object = EurocObject()
            self._retry = 0
            return 'success'

        #try to place the object where it currently is
        rospy.logdebug("Placement failed, to to place where we are.")
        taskdata.place_position = self.new_place_position()
        o = EurocObject
        o.mpe_object = CollisionObject()
        o.object = CollisionObject()
        o.object.id = co.id
        o.mpe_object.id = co.id
        o.c_centroid = Point(0,0,0)
        taskdata.failed_object = o

        self._retry += 1
        return 'noPlacePosition'