rvo2/examples/blocks.py at master · jimfleming/rvo2 · 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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
"""
Example file showing a demo with 100 agents split in four groups initially positioned in four corners of the environment. Each agent attempts to move to other side of the environment through a narrow passage generated by four obstacles. There is no roadmap to guide the agents around the obstacles.
"""
import math
import random
import gym.envs.classic_control.rendering as rendering

import rvo.math as rvo_math

from rvo.vector import Vector2
from rvo.simulator import Simulator

RVO_RENDER = True


class Blocks:

    def __init__(self):
        self.goals_ = [] # Vector2
        self.obstacles_ = [] # Vector2
        self.simulator_ = Simulator()

    def setup_scenario(self):
        # Specify the global time step of the simulation.
        self.simulator_.set_time_step(0.25)

        # Specify the default parameters for agents that are subsequently added.
        self.simulator_.set_agent_defaults(15.0, 10, 5.0, 5.0, 2.0, 2.0, Vector2(0.0, 0.0))

        # Add agents, specifying their start position, and store their goals on the opposite side of the environment.
        for i in range(5):
            for j in range(5):
                self.simulator_.add_agent(Vector2(55.0 + i * 10.0, 55.0 + j * 10.0))
                self.goals_.append(Vector2(-75.0, -75.0))

                self.simulator_.add_agent(Vector2(-55.0 - i * 10.0, 55.0 + j * 10.0))
                self.goals_.append(Vector2(75.0, -75.0))

                self.simulator_.add_agent(Vector2(55.0 + i * 10.0, -55.0 - j * 10.0))
                self.goals_.append(Vector2(-75.0, 75.0))

                self.simulator_.add_agent(Vector2(-55.0 - i * 10.0, -55.0 - j * 10.0))
                self.goals_.append(Vector2(75.0, 75.0))

        # Add (polygonal) obstacles, specifying their vertices in counterclockwise order.
        obstacle1 = []
        obstacle1.append(Vector2(-10.0, 40.0))
        obstacle1.append(Vector2(-40.0, 40.0))
        obstacle1.append(Vector2(-40.0, 10.0))
        obstacle1.append(Vector2(-10.0, 10.0))
        self.simulator_.add_obstacle(obstacle1)
        self.obstacles_.append(obstacle1)

        obstacle2 = []
        obstacle2.append(Vector2(10.0, 40.0))
        obstacle2.append(Vector2(10.0, 10.0))
        obstacle2.append(Vector2(40.0, 10.0))
        obstacle2.append(Vector2(40.0, 40.0))
        self.simulator_.add_obstacle(obstacle2)
        self.obstacles_.append(obstacle2)

        obstacle3 = []
        obstacle3.append(Vector2(10.0, -40.0))
        obstacle3.append(Vector2(40.0, -40.0))
        obstacle3.append(Vector2(40.0, -10.0))
        obstacle3.append(Vector2(10.0, -10.0))
        self.simulator_.add_obstacle(obstacle3)
        self.obstacles_.append(obstacle3)

        obstacle4 = []
        obstacle4.append(Vector2(-10.0, -40.0))
        obstacle4.append(Vector2(-10.0, -10.0))
        obstacle4.append(Vector2(-40.0, -10.0))
        obstacle4.append(Vector2(-40.0, -40.0))
        self.simulator_.add_obstacle(obstacle4)
        self.obstacles_.append(obstacle4)

        # Process the obstacles so that they are accounted for in the simulation.
        self.simulator_.process_obstacles()

    def update_visualization(self, viewer):
        if not RVO_RENDER:
            return

        for i in range(self.simulator_.num_agents):
            position = self.simulator_.agents_[i].position_
            color = [0, 0, 0]
            color[i % 3] = 1
            circle = viewer.draw_circle(radius=self.simulator_.default_agent_.radius_, color=color)
            circle.add_attr(rendering.Transform(translation=(position.x, position.y)))

        for obstacle in self.obstacles_:
            v = [(vec.x, vec.y) for vec in obstacle]
            viewer.draw_polygon(v=v, color=(0, 0, 0))

        viewer.render()

    def set_preferred_velocities(self):
        # Set the preferred velocity to be a vector of unit magnitude (speed) in the direction of the goal.
        for i in range(self.simulator_.num_agents):
            goal_vector = self.goals_[i] - self.simulator_.agents_[i].position_

            if rvo_math.abs_sq(goal_vector) > 1.0:
                goal_vector = rvo_math.normalize(goal_vector)

            self.simulator_.set_agent_pref_velocity(i, goal_vector)

            # Perturb a little to avoid deadlocks due to perfect symmetry.
            angle = random.random() * 2.0 * math.pi
            dist = random.random() * 0.0001

            self.simulator_.set_agent_pref_velocity(i, self.simulator_.agents_[i].pref_velocity_ +
                dist * Vector2(math.cos(angle), math.sin(angle)))

    def reached_goal(self):
        # Check if all agents have reached their goals.
        for i in range(self.simulator_.num_agents):
            if rvo_math.abs_sq(self.simulator_.agents_[i].position_ - self.goals_[i]) > 400.0:
                return False
        return True


def main():
    viewer = None

    blocks = Blocks()

    # Set up the scenario.
    blocks.setup_scenario()

    # Perform (and manipulate) the simulation.
    while not blocks.reached_goal():
        if RVO_RENDER:
            if viewer is None:
                viewer = rendering.Viewer(750, 750)
                viewer.set_bounds(-100, 100, -100, 100)

            blocks.update_visualization(viewer)
        blocks.set_preferred_velocities()
        blocks.simulator_.step()


if __name__ == '__main__':
    main()