RL-RVOp/pyrvo2_pure.py at master · XinyuWuu/RL-RVOp · 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
# from pyrvo2 import rvo2
import pyrvo2.rvo2 as rvo2
from base_config import PARAMs
sim = rvo2.PyRVOSimulator(
    1 / PARAMs['framerate'], PARAMs['dmax'], PARAMs['max_obs'],
    2 / PARAMs['framerate'], 1 / PARAMs['framerate'], 0.2, PARAMs['vmax'])
# sim = rvo2.PyRVOSimulator(1 / 60., 1.5, 5, 1.5, 2, 0.4, 2)
# Pass either just the position (the other parameters then use
# the default values passed to the PyRVOSimulator constructor),
# or pass all available parameters.
a0 = sim.addAgent((10, 10))
a1 = sim.addAgent((10, 0))
a2 = sim.addAgent((10, -10))
a3 = sim.addAgent((0, 10))


# Obstacles are also supported.
o1 = sim.addObstacle([(0.1, 0.1), (-0.1, 0.1), (-0.1, -0.1)])
sim.processObstacles()

sim.setAgentPrefVelocity(a0, (1, 1))
sim.setAgentPrefVelocity(a1, (-1, 1))
sim.setAgentPrefVelocity(a2, (-1, -1))
sim.setAgentPrefVelocity(a3, (1, -1))

print('Simulation has %i agents and %i obstacle vertices in it.' %
      (sim.getNumAgents(), sim.getNumObstacleVertices()))

print('Running simulation')

for step in range(20):
    sim.doStep()

    positions = ['(%5.3f, %5.3f)' % sim.getAgentPosition(agent_no)
                 for agent_no in (a0, a1, a2, a3)]
    print('step=%2i  t=%.3f  %s' %
          (step, sim.getGlobalTime(), '  '.join(positions)))
    PrefVelocity = ['(%5.3f, %5.3f)' % sim.getAgentPrefVelocity(agent_no)
                 for agent_no in (a0, a1, a2, a3)]
    Velocity = ['(%5.3f, %5.3f)' % sim.getAgentVelocity(agent_no)
                 for agent_no in (a0, a1, a2, a3)]
    print('step=%2i  t=%.3f  %s' %
          (step, sim.getGlobalTime(), '  '.join(PrefVelocity)))
    print('step=%2i  t=%.3f  %s' %
          (step, sim.getGlobalTime(), '  '.join(Velocity)))
    print()