Exemple #1
0
def main():
    # create entities which should be added to and manipulated in world view
    joint_set1 = JointSet(['Joint1', 'Joint2', 'Joint3'])

    joint_values1 = JointValues(joint_set1, [1.0, 2.0, 3.0])

    joint_set2 = JointSet(['Joint1', 'Joint2'])
    joint_values2 = JointValues(joint_set2, [19.0, 20.0])

    joint_set3 = JointSet(['Joint1', 'Joint4'])
    joint_values3 = JointValues(joint_set3, [100.0, 30.0])

    t1 = [0.502522, 0.2580, 0.3670]
    q1 = Quaternion(w=0.304389, x=0.5272, y=0.68704, z=0.39666)

    t2 = [0.23795, 0.46845, 0.44505]
    q2 = Quaternion(w=0.212097, x=0.470916, y=0.720915, z=0.462096)

    t3 = [0.6089578, 0.3406782, 0.208865]
    q3 = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387)

    pose1 = Pose(t1, q1, 'world')
    pose2 = Pose(t2, q2, 'world')
    pose3 = Pose(t3, q3, 'world')

    cartesian_path1 = CartesianPath([pose1, pose2, pose3])

    sphere = CollisionPrimitive.create_sphere(0.1, pose2)
    cylinder = CollisionPrimitive.create_cylinder(0.4, 0.05, pose2)
    box = CollisionPrimitive.create_box(0.2, 0.2, 0.1, pose1)

    plane = CollisionPrimitive.create_plane(1.0, 1.0, 1.0, 1.0, pose3)
    cone = CollisionPrimitive.create_cone(0.2, 0.2, pose3)

    collision_object1 = CollisionObject([box])
    collision_object2 = CollisionObject([sphere, cylinder])
    collision_object3 = CollisionObject([plane, cone])

    # create a instance of WorldViewClient to get access to rosvita world view
    world_view_client = WorldViewClient()

    print('---------------- add folder --------------')
    # add folder test at WorldView root
    world_view_client.add_folder('test', '')
    # add folder test at WorldView root/test
    world_view_client.add_folder('joint_values', '/test')

    world_view_client.add_folder('poses', '/test')

    world_view_client.add_folder('cartesian_paths', '/test')

    world_view_client.add_folder('collision_objects', '/test')

    print('---------------- add joint values --------------')
    world_view_client.add_joint_values('joint_values1', 'test/joint_values',
                                       joint_values1)
    world_view_client.add_joint_values('joint_values2', 'test/joint_values',
                                       joint_values2)
    world_view_client.add_joint_values('joint_values3', 'test/joint_values',
                                       joint_values3)
    world_view_client.add_joint_values('test', 'test/joint_values',
                                       joint_values3)

    print('---------------- update joint values --------------')
    world_view_client.update_joint_values('joint_values1', 'test/joint_values',
                                          joint_values2)
    world_view_client.update_joint_values('joint_values2', 'test/joint_values',
                                          joint_values3)
    world_view_client.update_joint_values('joint_values3', 'test/joint_values',
                                          joint_values1)

    print('---------------- get joint values --------------')

    get_value = world_view_client.get_joint_values('joint_values1',
                                                   'test/joint_values')
    print('joint_values1 is: ' + str(get_value))

    print('---------------- query joint values --------------')
    # query all values which start with t under test/joint_values
    queried_values = world_view_client.query_joint_values(
        'test/joint_values', 't')

    print('---------------- add poses --------------')
    world_view_client.add_pose('pose1', 'test/poses', pose1)
    world_view_client.add_pose('pose2', 'test/poses', pose2)
    world_view_client.add_pose('pose3', 'test/poses', pose3)
    world_view_client.add_pose('test', 'test/poses', pose3)

    print('---------------- update pose --------------')
    world_view_client.update_pose('pose1', 'test/poses', pose2)
    world_view_client.update_pose('pose2', 'test/poses', pose3)
    world_view_client.update_pose('pose3', 'test/poses', pose1)

    print('---------------- get pose --------------')

    get_value = world_view_client.get_pose('pose1', 'test/poses')
    print('pose1 is: ' + str(get_value))

    print('---------------- query poses --------------')
    # query all poses which start with t under test/pose
    queried_values = world_view_client.query_poses('test/poses', 't')

    print('---------------- add cartesian path --------------')
    world_view_client.add_cartesian_path('cartesian_path1',
                                         'test/cartesian_paths',
                                         cartesian_path1)
    world_view_client.add_cartesian_path('test1', 'test/cartesian_paths',
                                         cartesian_path1)

    print('---------------- update cartesian path --------------')
    world_view_client.update_cartesian_path('cartesian_path1',
                                            'test/cartesian_paths',
                                            cartesian_path1)

    print('---------------- get cartesian path --------------')

    get_value = world_view_client.get_cartesian_path('cartesian_path1',
                                                     'test/cartesian_paths')
    print('cartesian_path1 is: ' + str(get_value))

    print('---------------- query cartesian paths --------------')
    # query all cartesian_paths which start with t under test/cartesian_path
    queried_values = world_view_client.query_cartesian_paths(
        'test/cartesian_paths', 't')

    for v in queried_values:
        print(str(v))

    print('---------------- add collision object --------------')
    world_view_client.add_collision_object('collision_object1',
                                           'test/collision_objects',
                                           collision_object1)
    world_view_client.add_collision_object('collision_object2',
                                           'test/collision_objects',
                                           collision_object2)
    world_view_client.add_collision_object('collision_object3',
                                           'test/collision_objects',
                                           collision_object3)
    world_view_client.add_collision_object('test1', 'test/collision_objects',
                                           collision_object1)

    print('---------------- update collision object --------------')
    world_view_client.update_collision_object('collision_object1',
                                              'test/collision_objects',
                                              collision_object1)

    print('---------------- get collision object --------------')

    get_value = world_view_client.get_collision_object(
        'collision_object1', 'test/collision_objects')
    print('collision_object1 is: ' + str(get_value))

    print('---------------- query collision object --------------')
    # query all collision_objects which start with t under test/collision_object
    queried_values = world_view_client.query_collision_objects(
        'test/collision_objects', 't')

    for v in queried_values:
        print(str(v))

    input('Press enter to clean up')

    print('----------------- remove folder test ---------------')
    world_view_client.remove_element('test', '')
Exemple #2
0
    CollisionObject
        The grid of boxes as a CollisionObject instance
    """

    createBoxOfPose = lambda pose: CollisionPrimitive.create_box(
        size[0], size[1], size[2], pose)
    return CollisionObject(list(map(createBoxOfPose, poses)))


if __name__ == '__main__':
    # Called when running this script standalone
    world_view_folder = "example_02_palletizing/example_create_collision_boxes"

    world_view_client = WorldViewClient()

    # Read poses from world view
    poses_dict = world_view_client.query_poses(world_view_folder)
    poses = list(poses_dict.values())
    boxes = main(poses, (0.2, 0.2, 0.2))
    # Save the generated collision boxes in world view
    try:
        # delete folder if it already exists
        world_view_client.remove_element("generated", world_view_folder)
    except Exception as e:
        None
    world_view_client.add_folder("generated", world_view_folder)
    world_view_client.add_collision_object(
        "collision_matrix", "/{}/generated".format(world_view_folder), boxes)

    input("Press enter to clean up")
    world_view_client.remove_element("generated", world_view_folder)
Exemple #3
0
def main():
    world_view_folder = "example_04_collision_objects"

    world_view_client = WorldViewClient()

    move_group = example_utils.get_move_group()

    loop = asyncio.get_event_loop()
    # register the loop handler to be shutdown appropriately
    register_asyncio_shutdown_handler(loop)

    # Read joint values for begin and end configuration
    jv_start = world_view_client.get_joint_values("Start", world_view_folder)
    jv_end = world_view_client.get_joint_values("End", world_view_folder)

    # Read poses from world view
    poses_dict = world_view_client.query_poses(
        "{}/poses".format(world_view_folder))

    # Read names too to associate the joint values to the poses
    poses_names = list(
        map(lambda posix_path: posix_path.name, list(poses_dict.keys())))
    # Create no names for the collision objects
    collision_names = list(
        map(lambda name: "collision_obect_of_{}".format(name), poses_names))

    # Get poses of dictionary
    poses = list(poses_dict.values())

    add_generated_folder(world_view_client, world_view_folder)

    # Store collision box for every positionin collision_objects
    collision_objects = {}
    for i in range(len(poses)):
        pose = poses[i]
        name = collision_names[i]
        box = CollisionPrimitive.create_box(0.15, 0.15, 0.15, pose)
        coll_obj = CollisionObject([box])
        collision_objects[name] = coll_obj

    async def move_back_and_forth(jv_start: JointValues, jv_end: JointValues):
        """ 
        Moves back and forth between jv_start and jv_end, evading collision objects

        Parameters
        ----------
        jv_start : JointValues
            Start configuration
        jv_end : JointValues
            End configuration
        """

        print("Moving to start position")
        # First, move to start joint values
        await move_group.move_joints_collision_free(jv_start,
                                                    velocity_scaling=0.5)

        # Now activate one collision object and observe how the robot finds a way
        # TODO: Update code when python api support activating/deactivating of collision objects
        for name in collision_names:
            print("Moving around obstacle  \"{}\".".format(name))
            # activate "name"
            coll_obj = collision_objects[name]
            # Add element to world view
            world_view_client.add_collision_object(
                name, "/{}/generated".format(world_view_folder), coll_obj)
            test = world_view_client.get_collision_object(
                name, "/{}/generated".format(world_view_folder))
            # move to end location
            await move_group.move_joints_collision_free(jv_end,
                                                        velocity_scaling=0.5)

            # Remove element from world view
            world_view_client.remove_element(
                name, "/{}/generated".format(world_view_folder))
            # switch start and end
            jv_temp = jv_end
            jv_end = jv_start
            jv_start = jv_temp

    try:
        loop.run_until_complete(move_back_and_forth(jv_start, jv_end))
    finally:
        loop.close()

    # clean up
    world_view_client.remove_element("generated", world_view_folder)