def _parallel_world(data, lock):
# robot of this function mirrors the robot controlled
# in the function _run_world (declared below)
# data and lock : used for sharing of data between
# _parallel_world and _run_robot
# _parallel_world and _run_robot need to run in 2 different
# processes because, for some unknown reason, 2 instances of B2World
# running in the same processes crash with segfault.
# setting up the robot and the world
robot_config = DefaultRobotConfig()
ball_configs = []
visible_area_width = 6.0
visual_height = 0.05
world = B2World(robot_config, ball_configs, visible_area_width)
robot_init = DefaultRobotState(robot_config)
world.reset(robot_init)
# running until the run_robot process stops
running = True
while running:
world_state = None
with lock:
# getting the world state of the robot
# running in run robot
running = data["running"]
if data["new_data"]:
world_state_str = data["world_state"]
world_state = pickle.loads(world_state_str)
data["new_data"] = False
if world_state:
# mirroring the robot
mirroring_robot_state = world_state.robot
world_state2 = world.step(
None,
mirroring_robot_states=mirroring_robot_state,
current_time=world_state.t)
with lock:
# writting the world_state of the mirroring robot
# for display in the run_world process. Seems like 2 pyglet
# renders can not run in 2 processes. Yes. 2 world must run in 2 processes,
# but 2 renderers must not run in 2 processes. How convenient.
data["world_state2"] = pickle.dumps(world_state2)
else:
time.sleep(0.001)
def test_vertical_drops_over_ground(self):
# droping 2 balls, and checking the balls
# are reported at touching the floor at the
# expected position
pos1 = [4, 1]
pos2 = [5, 1]
positions = [pos1, pos2]
ball_guns = [DropBallGun(*pos) for pos in positions]
world = B2World(
[], # no robot
[BallConfig(), BallConfig()],
6.0)
world.reset(
None, # no robot
ball_guns)
time_start = time.time()
balls_bounce_x = [None] * 2
# running for a max time of 1 seconds
while time.time() - time_start < 1:
world_state = world.step(None) # None: no robot
bounces = world_state.balls_hits_floor
for index, bounce in enumerate(bounces):
if bounce:
balls_bounce_x[index] = bounce
if all([b is not None for b in balls_bounce_x]):
break
for index, bounce in enumerate(balls_bounce_x):
# the ball did not bounce
self.assertTrue(bounce is not None)
# the ball should have bounced after dropping vertically
self.assertEqual(bounce, positions[index][0])
def test_vertical_drops_over_robot(self):
# droping 3 balls, 2 above robot and
# 1 somewhere else. Checking the 2 first balls
# do bounce on the racket, and 3rd one does not
robot_base = 4.0
pos1 = [4.05, 1.0]
pos2 = [3.95, 1.0]
pos3 = [7.0, 1.0]
positions = [pos1, pos2, pos3]
ball_guns = [DropBallGun(*pos) for pos in positions]
robot_config = DefaultRobotConfig()
robot_config.position = robot_base
init_robot_state = DefaultRobotState(robot_config,
[0.0 for _ in positions],
[0.0 for _ in positions])
world = B2World(DefaultRobotConfig(),
[BallConfig() for _ in positions], 6.0)
world.reset(init_robot_state, ball_guns)
time_start = time.time()
touched_racket = [False] * 3
while time.time() - time_start < 0.5:
world_state = world.step(None)
bounces = world_state.balls_hits_racket
for index, bounce in enumerate(bounces):
if bounce: touched_racket[index] = True
self.assertTrue(touched_racket[0])
self.assertTrue(touched_racket[1])
self.assertFalse(touched_racket[2])
def test_vertical_drop_over_ground(self):
# droping a ball, and checking the ball
# is reported at touching the floor at the
# expected position
x = 4.0
y = 1.0
ball_gun = DropBallGun(x, y)
world = B2World(
[], # no robot
BallConfig(),
6.0)
world.reset(
None, # no robot
ball_gun)
time_start = time.time()
ball_bounce_x = None
# running for a max time of 1 seconds
while time.time() - time_start < 1:
world_state = world.step(None) # None: no robot
bounce = world_state.ball_hits_floor
if bounce:
ball_bounce_x = bounce
break
# the ball did not bounce
self.assertTrue(ball_bounce_x is not None)
# the ball should have bounced after dropping vertically
self.assertEqual(ball_bounce_x, x)
def run(rendering=True):
"""
Runs the balls demo, in which the robot moves using a PD controller
as a ball bounces around.
You may run the executable roboball2d_demo after install to
see it in action.
Parameters
----------
rendering :
renders the environment if True
"""
if rendering:
from roboball2d.rendering import PygletRenderer
from roboball2d.rendering import RenderingConfig
# configurations, using default
robot_config = DefaultRobotConfig()
ball_config = BallConfig()
visible_area_width = 6.0
visual_height = 0.05
# physics engine
world = B2World(robot_config,
ball_config,
visible_area_width)
# graphics renderer
if rendering:
renderer_config = RenderingConfig(visible_area_width,
visual_height)
renderer = PygletRenderer(renderer_config,
robot_config,
ball_config)
# ball gun : specifies the reset of
# the ball (by shooting a new one)
# Here using default ball gun
ball_gun = DefaultBallGun(ball_config)
# basic PD controller used to compute torque
controller = PDController()
references = [-math.pi/8.0,-math.pi/8.0,-math.pi/8.0]
# init robot state : specifies the reinit of the robot
# (e.g. angles of the rods and rackets, etc)
init_robot_state = DefaultRobotState(robot_config)
# tracking the number of times the ball bounced
n_bounced = 0
# we add a fixed goal
# starting at x=3 and finishing at x=6
goal = (2,4)
goal_color = (0,0.7,0)
goal_activated_color = (0,1,0)
n_episodes = 0
# running 5 episodes
for episode in range(5):
episode_end = False
# resetting the robot and shooting
# the ball gun
world_state = world.reset(init_robot_state,
ball_gun)
# keeping track of the number of times the
# ball bounced
n_bounced = 0
while not episode_end:
# running controller
angles = [joint.angle for joint
in world_state.robot.joints]
angular_velocities = [joint.angular_velocity for joint
in world_state.robot.joints]
torques = controller.get(references,
angles,
angular_velocities)
#
# One simulation step
#
# returns a snapshot of all the data computed
# and updated by the physics engine at this
# iteration (see below for all information managed)
# relative=True : torques are not given in absolute value,
# but as values in [-1,1] that will be mapped to
# [-max_torque,+max_torque]
world_state = world.step(torques,relative_torques=True)
# keeping track number of times the ball bounced
if world_state.ball_hits_floor :
n_bounced += 1
if n_bounced >= 2 :
# if bounced more than 2 : end of episode
episode_end = True
#
# Rendering
#
if rendering:
# was the goal hit ?
color = goal_color
if world_state.ball_hits_floor :
p = world_state.ball_hits_floor
if p>goal[0] and p<goal[1]:
# yes, using activated color
color = goal_activated_color
# the renderer can take in an array of goals
# to display
goals = [(goal[0],goal[1],color)]
# render based on the information provided by
# the physics engined
renderer.render(world_state,goals,time_step=1.0/60.0)
def run(rendering=True):
"""
Runs the balls demo, in which the robot moves according to random
torques as 10 balls bounces around.
You may run the executable roboball2d_balls_demo after install.
Parameters
----------
rendering :
renders the environment if True
"""
# similar to roboball2d_demo, but with several balls
n_balls = 10
# configurations :
# just create as many configs as there are balls
robot_config = DefaultRobotConfig()
ball_configs = [BallConfig() for _ in range(n_balls)]
# the first ball : in pink
ball_configs[0].color = (1, 0.08, 0.57)
visible_area_width = 6.0
visual_height = 0.05
# physics engine
# physical engine
world = B2World(robot_config, ball_configs, visible_area_width)
# graphics renderer
if rendering:
renderer_config = RenderingConfig(visible_area_width, visual_height)
renderer = PygletRenderer(renderer_config, robot_config, ball_configs)
# ball gun : specifies the reset of
# the ball (by shooting a new one)
ball_guns = [
DefaultBallGun(ball_config)
for index, ball_config in enumerate(ball_configs)
]
# robot init : specifies the reinit of the robot
# (e.g. angles of the rods and rackets, etc)
robot_init = DefaultRobotState(robot_config)
# we add a fixed goal
# starting at x=3 and finishing at x=4
goal = (2, 4)
goal_color = (0, 0.7, 0) # usual color
goal_activated_color = (0, 1, 0) # when hit by a ball
# running 10 episodes, max 3 seconds per episode
n_episodes = 0
for episode in range(10):
# tracking the number of times the ball bounced
n_bounced = 0
# reset before a new episode
world.reset(robot_init, ball_guns)
while True:
# random torques
torques = [random.uniform(-1.0, 1.0) for _ in range(3)]
# returns a snapshot of all the data computed
# and updated by the physics engine at this
# iteration
world_state = world.step(torques, relative_torques=True)
# episode ends
# if the number of bounces is 2 or above
# for the first ball ...
if world_state.ball_hits_floor:
n_bounced += 1
if n_bounced >= 2:
break
# ... or if 3 seconds passed
if world_state.t > 3:
break
# display the goal with a lighter color
# if hit by a ball at this iteration
color = goal_color
for p in world_state.balls_hits_floor:
p = world_state.ball_hits_floor
if p is not None and p > goal[0] and p < goal[1]:
# goal hit, using activated color
color = goal_activated_color
break
# the renderer can take in an array of goals
# to display. Here specifying only 1 goal
# (start_x,end_x,color)
goals = [(goal[0], goal[1], color)]
# render based on the information provided by
# the physics engine
if rendering:
renderer.render(world_state, goals, time_step=1.0 / 60.0)
def __init__(self,
interface_id,
ball=True,
robot=True,
ball_gun=False,
render=True,
render_ball=True,
width=None,
height=None,
location=[0, 0]):
for slot in self.__slots__:
setattr(self, slot, None)
if robot:
self.robot_config = DefaultRobotConfig()
nb_robots = 1
else:
nb_robots = 0
if ball:
if ball == True:
self.ball_config = BallConfig()
nb_balls = 1
else:
self.ball_config = [BallConfig() for _ in range(ball)]
nb_balls = ball
else:
nb_balls = 1
if ball_gun:
if ball == True:
self.ball_gun = DefaultBallGun(self.ball_config)
else:
self.ball_gun = [
DefaultBallGun(config) for config in self.ball_config
]
visible_area_width = 6.0
visual_height = 0.05
if robot or ball:
self.world = B2World(self.robot_config, self.ball_config,
visible_area_width)
# graphics renderer
if render:
renderer_config = RenderingConfig(visible_area_width,
visual_height)
if width is not None:
renderer_config.window.width = width
if height is not None:
renderer_config.window.height = height
if location is not None:
renderer_config.window.location = location
if render_ball:
self.renderer = PygletRenderer(renderer_config,
self.robot_config,
self.ball_config)
else:
self.renderer = PygletRenderer(renderer_config,
self.robot_config, [])
if robot:
self.robot_init = DefaultRobotState(self.robot_config)
if nb_balls == 1:
self.torques_reader = roboball2d_interface.TorquesReader(
interface_id)
self.torques_writer = roboball2d_interface.TorquesWriter(
interface_id)
self.mirror_reader = roboball2d_interface.MirrorReader(
interface_id)
self.mirror_writer = roboball2d_interface.MirrorWriter(
interface_id)
self.ball_gun_reader = roboball2d_interface.BallGunReader(
interface_id)
self.ball_gun_writer = roboball2d_interface.BallGunWriter(
interface_id)
elif nb_balls == 5:
self.torques_reader = roboball2d_interface.FiveBallsTorquesReader(
interface_id)
self.torques_writer = roboball2d_interface.FiveBallsTorquesWriter(
interface_id)
self.mirror_reader = roboball2d_interface.FiveBallsMirrorReader(
interface_id)
self.mirror_writer = roboball2d_interface.FiveBallsMirrorWriter(
interface_id)
self.ball_gun_reader = roboball2d_interface.FiveBallsBallGunReader(
interface_id)
self.ball_gun_writer = roboball2d_interface.FiveBallsBallGunWriter(
interface_id)
else:
raise Exception(
"roboball2d_interface.run_support: only 1 or 5 balls supported"
)
def run(rendering=True):
# configurations
robot_config = DefaultRobotConfig()
ball_config = BallConfig()
visible_area_width = 6.0
visual_height = 0.05
# physics engine
# physical engine
world = B2World(robot_config, ball_config, visible_area_width)
# graphics renderer
renderer_config = RenderingConfig(visible_area_width, visual_height)
# callback to draw permanent balls at bounce positions
bounce_callback = BouncePlaces(ball_config, visual_height)
renderer = PygletRenderer(renderer_config,
robot_config,
ball_config,
callbacks=[bounce_callback])
# ball gun : specifies the reset of
# the ball (by shooting a new one)
ball_gun = DefaultBallGun(ball_config)
# robot init : specifies the reinit of the robot
# (e.g. angles of the rods and rackets, etc)
robot_init = DefaultRobotState(robot_config)
# the ball and the robot will be reinit
# when this is true
reinit = True
# tracking the number of times the ball bounced
n_bounced = 0
# we add a fixed goal
# starting at x=3 and finishing at x=6
goal = (2, 4)
goal_color = (0, 0.7, 0)
goal_activated_color = (0, 1, 0)
# running 5 episodes
n_episodes = 0
for episode in range(5):
episode_end = False
# resetting the robot and shooting
# the ball gun
world.reset(robot_init, ball_gun)
# resetting the permanent drawn balls
bounce_callback.reset()
# keeping track of the number of times the
# ball bounced
n_bounced = 0
while not episode_end:
# random policy
torques = [random.randrange(-1.0, 1.0) for _ in range(3)]
#
# running the physics
#
# returns a snapshot of all the data computed
# and updated by the physics engine at this
# iteration (see below for all information managed)
# relative=True : torques are not given in absolute value,
# but as values in [-1,1] that will be mapped to
# [-max_torque,+max_torque]
world_state = world.step(torques, relative_torques=True)
# keeping track number of times the ball bounced
if world_state.ball_hits_floor:
n_bounced += 1
if n_bounced >= 2:
# if bounced more than 2 : end of episode
episode_end = True
#
# rendering
#
# was the goal hit ?
color = goal_color
if world_state.ball_hits_floor:
p = world_state.ball_hits_floor
if p > goal[0] and p < goal[1]:
# yes, using activated color
color = goal_activated_color
# the renderer can take in an array of goals
# to display
goals = [(goal[0], goal[1], color)]
# render based on the information provided by
# the physics engined
renderer.render(world_state, goals, time_step=1.0 / 60.0)
def run(rendering=True):
"""
Runs the balls demo, in which the robot moves according to random
torques as 1 ball bounce around. A rendering callback is used to
display the position the ball touched the ground.
You may run the executable roboball2d_rendering_demo after install
to see it in action.
Parameters
----------
rendering :
renders the environment if True
"""
# configurations
robot_config = DefaultRobotConfig()
ball_config = BallConfig()
visible_area_width = 6.0
visual_height = 0.05
# physics engine
# physical engine
world = B2World(robot_config,
ball_config,
visible_area_width)
# graphics renderer
# callback to draw permanent balls at bounce positions
bounce_callback = _BouncePlaces(ball_config,visual_height)
# callback for rendering of robot state with specified
# color and depth
# Note that due to the nagative z coordinate the
# additional robot is drawn behind the simulated robot.
robot_callback = _Robot(
color = (0., .5, 0.),
z_coordinate = -0.1)
if rendering:
renderer_config = RenderingConfig(visible_area_width,
visual_height)
renderer = PygletRenderer(renderer_config,
robot_config,
ball_config,
callbacks=[bounce_callback,
robot_callback])
# ball gun : specifies the reset of
# the ball (by shooting a new one)
ball_gun = DefaultBallGun(ball_config)
# robot init : specifies the reinit of the robot
# (e.g. angles of the rods and rackets, etc)
robot_init = DefaultRobotState(robot_config)
# the ball and the robot will be reinit
# when this is true
reinit = True
# tracking the number of times the ball bounced
n_bounced = 0
# we add a fixed goal
# starting at x=3 and finishing at x=6
goal = (2,4)
goal_color = (0,0.7,0)
goal_activated_color = (0,1,0)
# running 5 episodes
n_episodes = 0
for episode in range(5):
episode_end = False
# resetting the robot and shooting
# the ball gun
world.reset(robot_init,
ball_gun)
# resetting the permanent drawn balls
bounce_callback.reset()
# keeping track of the number of times the
# ball bounced
n_bounced = 0
while not episode_end:
# random policy
torques = [random.randrange(-1.0,1.0) for _ in range(3)]
#
# running the physics
#
# returns a snapshot of all the data computed
# and updated by the physics engine at this
# iteration (see below for all information managed)
# relative=True : torques are not given in absolute value,
# but as values in [-1,1] that will be mapped to
# [-max_torque,+max_torque]
world_state = world.step(torques,relative_torques=True)
# keeping track number of times the ball bounced
if world_state.ball_hits_floor :
n_bounced += 1
if n_bounced >= 2 :
# if bounced more than 2 : end of episode
episode_end = True
# update the robot state that is to be drawn behind the simulated
# robot from time to time
if random.random() <= 0.01:
robot_callback.update_state(deepcopy(world_state.robot))
#
# rendering
#
if rendering:
# was the goal hit ?
color = goal_color
if world_state.ball_hits_floor :
p = world_state.ball_hits_floor
if p>goal[0] and p<goal[1]:
# yes, using activated color
color = goal_activated_color
# the renderer can take in an array of goals
# to display
goals = [(goal[0],goal[1],color)]
# render based on the information provided by
# the physics engined
renderer.render(world_state,goals,time_step=1.0/60.0)
def get_default(nb_robots=1,
nb_balls=1,
ball_default_color=[1.0,1.0,1.0],
ball_colors={},
background_color=[0.0, 0.1, 0.4],
ground_color= [0.0, 0.3, 0.0],
visible_area_width=6.0,
visual_height=0.05,
window_size=(1600,800),
render=True):
"""
Returns an instance of :py:class:`.Roboball2d` encapsulating instances
created using default configurations (except for values passed as arguments)
Parameters
----------
nb_robots: int, number of robors (default:1)
nb_balls: int, number of balls (default:1)
ball_default_color: tuple (R,G,B) (values between 0 and 1) (default 1,1,1)
ball_colors: dict {ball index : tuple (R,G,B)} (default:{})
background_color: tuple (R,G,B) (default: 0,0.1,0.4)
ground_color: tuple (R,G,B) (default: 0,0.3,0.0)
visible_area_width: float (default: 0.6)
visual_height: float (default:0.05)
window_size: tuple (width,height), in pixels (default: 1600,800)
render: if true, the function returns a tuple of an instance of B2World and an instance
of PygletRenderer, else returns a tuple of an instance of B2World and None
(default: True)
"""
r = Roboball2d()
robot_configs = [DefaultRobotConfig()
for _ in range(nb_robots)]
r.robot_configs = robot_configs
ball_configs = [BallConfig()
for _ in range(nb_balls)]
for ball_config in ball_configs:
ball_config.color = ball_default_color
ball_config.line_color = ball_default_color
for index,color in ball_colors.items():
color = list(color)
ball_configs[index].color = color
ball_configs[index].line_color=color
r.ball_guns = [DefaultBallGun(ball_config)
for ball_config in ball_configs]
r.world = B2World(robot_configs,
ball_configs,
visible_area_width)
r.robot_reinits = [DefaultRobotState(robot_config) for
robot_config in robot_configs]
if(render):
from roboball2d.rendering import PygletRenderer
from roboball2d.rendering import RenderingConfig
renderer_config = RenderingConfig(visible_area_width,
visual_height)
background_color = list(background_color)
background_color.append(1.0)
renderer_config.background_color = background_color
renderer_config.ground_color = ground_color
class Window:
width = window_size[0]
height = window_size[1]
renderer_config.window = Window
r.renderer = PygletRenderer(renderer_config,
robot_configs,
ball_configs)
return r
def _run_world(rendering):
if rendering:
from roboball2d.rendering import PygletRenderer
from roboball2d.rendering import RenderingConfig
# preparing the robot and renderer
robot_config = DefaultRobotConfig()
ball_configs = []
visible_area_width = 6.0
visual_height = 0.05
world = B2World(robot_config, ball_configs, visible_area_width)
if rendering:
renderer_config = RenderingConfig(visible_area_width, visual_height)
# for this robot
renderer = PygletRenderer(renderer_config, robot_config, ball_configs)
# for the mirroring robot
renderer2 = PygletRenderer(renderer_config, robot_config, ball_configs)
robot_init = DefaultRobotState(robot_config)
# for sharing data with the parallel_world
manager = multiprocessing.Manager()
data = manager.dict()
data["new_data"] = False
data["running"] = True
data["world_state"] = None
data["world_state2"] = None
lock = manager.Lock()
# starting the process that will run the mirroring robot
parallel_world = multiprocessing.Process(target=_parallel_world,
args=(
data,
lock,
))
parallel_world.start()
increment = 0.01
torque_root = 0
world.reset(robot_init)
for iteration in range(400):
# applying sin and cos torques
torque_root += increment
torques = [
math.cos(torque_root),
math.sin(torque_root),
math.cos(torque_root)
]
# applying torques to the robot
world_state = world.step(torques, relative_torques=True)
# sending the robot world_state to the mirroring robot
with lock:
data["new_data"] = True
data["world_state"] = pickle.dumps(world_state)
# rendering this robot
if rendering:
renderer.render(world_state, time_step=1.0 / 60.0)
# rendering the mirroring robot
if rendering:
with lock:
ws2 = data["world_state2"]
if ws2:
world_state2 = pickle.loads(data["world_state2"])
if (world_state2 is not None):
renderer2.render(world_state2, time_step=1.0 / 60.0)
# stopping the mirroring robot
with lock:
data["new_data"] = True
data["running"] = False
parallel_world.join()