xamla_motion_examples/example_04/example_04_collision_objects.py at master · Xamla/xamla_motion_examples · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
"""
This example generates some collision objects at poses read from world view.
Furthermore, collision free movement is show while (de)activating some of the
obstacles.
"""
import asyncio

from xamla_motion.world_view_client import WorldViewClient
from xamla_motion.motion_client import MoveGroup
from xamla_motion.data_types import JointValues, CollisionObject, CollisionPrimitive

from xamla_motion.utility import register_asyncio_shutdown_handler

from xamla_motion.xamla_motion_exceptions.exceptions import ArgumentError

import example_utils

def add_generated_folder(world_view_client: WorldViewClient, world_view_folder: str) -> None:
    """ Adds a folder to world view, deletes content if existent"""

    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)

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)

if __name__ == '__main__':
    # Called when running this script standalone
    main()