def reset_real_robot(self, frequency, duration_sec=2.0, refs=None):
if refs is None:
refs = self._reset_real_robot_angles
time_start = time.time()
delta_t = 0
frequency_manager = real_time_tools.FrequencyManager(frequency)
while delta_t < duration_sec:
delta_t = time.time() - time_start
angles, angular_velocities, _ = self.get_real_robot()
torques = self._reset_controller.get(refs, angles,
angular_velocities)
for dof, torque in enumerate(torques):
joint = o80_roboball2d.Joint()
joint.set_torque(torque)
self._real_robot.add_command(dof, joint, o80.Mode.OVERWRITE)
self._real_robot.pulse()
frequency_manager.wait()
def run_vision(configuration,
switch,
interface_id_robot,
interface_id_vision,
render=True):
# managing running frequency
frequency_manager = real_time_tools.FrequencyManager(
configuration.Interfaces.vision_frequency)
# to simulate vision: we plug directly to the shared memory
# in which the run_reality process write world_state (originally
# for the sake of the driver)
reader = roboball2d_interface.TorquesReader(interface_id_robot)
writer = roboball2d_interface.TorquesWriter(interface_id_vision)
ball_config = BallConfig()
renderer_config = RenderingConfig(configuration.Window.visible_area_width,
configuration.Window.visual_height)
renderer_config.window.width = configuration.Window.width
renderer_config.window.height = configuration.Window.height
renderer = PygletRenderer(renderer_config,
None,
ball_config)
while run_support.should_run(switch):
sm_world_state = reader.read_world_state()
writer.write_world_state(sm_world_state)
world_state = run_support.reverse(sm_world_state,ball_config)
renderer.render(world_state,[],time_step=1.0/30.0,wait=False)
frequency_manager.wait()
def run_simulation(config,
switch,
interface_id_robot,
interface_id_ball_gun,
render=True):
nb_balls = config.Balls.nb_balls
goal = (config.Goal.x_min, config.Goal.x_max, config.Goal.color)
sim_robot = run_support.Simulation(interface_id_robot,
ball=nb_balls,
robot=True,
ball_gun=True,
render=render,
width=config.Window.width,
height=config.Window.height)
sim_ball_gun = run_support.Simulation(interface_id_ball_gun,
ball=False,
robot=False,
ball_gun=False,
render=False)
time_start = time.time()
previous_ball_gun_id = -1
previous_action_id = -1
torques = [0, 0, 0]
robot_state = None
frequency_manager = real_time_tools.FrequencyManager(
configuration.Interfaces.reality_frequency)
while run_support.should_run(switch):
# if ball gun requested to shoot, doing so.
ball_gun_action = sim_ball_gun.ball_gun_reader.read_action()
if all([
ball_gun_action.is_valid(),
ball_gun_action.should_shoot(),
ball_gun_action.id != previous_ball_gun_id
]):
previous_ball_gun_id = ball_gun_action.id
world_state = sim_robot.world.reset(None, sim_robot.ball_gun)
sim_ball_gun.ball_gun_writer.write_action(
roboball2d_interface.BallGunAction(False))
# mirroring the robot based on action provided by the driver
action = sim_robot.mirror_reader.read_action()
should_use_action = previous_action_id != action.id
should_use_action = should_use_action and action.is_valid()
if should_use_action:
# mi = [ [angle,angular_velocity], ... ]
mi = action.get_mirroring_information()
angles = [m[0] for m in mi]
angular_velocities = [m[1] for m in mi]
robot_state = roboball2d.robot.DefaultRobotState(
sim_robot.robot_config,
generalized_coordinates=angles,
generalized_velocities=angular_velocities)
previous_action_id = action.id
# running the physics (mirring robot + virtual balls dynamics)
world_state = sim_robot.world.step(
None,
mirroring_robot_states=robot_state,
current_time=time.time() - time_start)
# converting world state to something the driver will
# be able to use
sm_world_state = run_support.convert(world_state)
sim_robot.mirror_writer.write_world_state(sm_world_state)
# rendering
if sim_robot.renderer:
sim_robot.renderer.render(world_state, [goal],
time_step=1.0 / 30.0,
wait=False)
frequency_manager.wait()
def run():
frequency_manager = real_time_tools.FrequencyManager(500)
# uses o80 to send:
# torque commands to the pseudo real robot
# mirroring commands to the simulated robot
# shoot commands to the simulated ball gun
orchestrator = Orchestrator()
# dummy policy moving robot to random postures
policy = Policy()
# init
orchestrator.apply(torques=[0, 0, 0])
running = True
while running:
# resetting real robot to start position
# and sending commands to ball guns
orchestrator.apply(reset=True, shoot=True)
# resetting context manager: observation
# during real robot reset can be ignored
orchestrator.reset()
# initializing robot joints angles and
# angular velocities
angles, angular_velocities, _ = orchestrator._get_real_robot()
time_start = time.time()
torques = [0.0] * 3
while time.time() - time_start < 3:
try:
# sending torques to real robot
# and mirroring commands to simulation
orchestrator.apply(torques=torques,
angles=angles,
angular_velocities=angular_velocities)
# getting robot state from direct observation,
# and context world state
data = orchestrator.observation_manager()
(time_stamp_robot, robot_state), (time_stamp_context,
context_world_state) = data
angles = robot_state.angles
angular_velocities = robot_state.angular_velocities
# getting torques from policy
torques = policy.get_torques(angles, angular_velocities)
# printing racket contact information
_print_contacts(context_world_state)
# running at desired frequency
frequency_manager.wait()
except KeyboardInterrupt:
running = False
def run():
# encapsulates all the calls to
# o80 frontends, for clarity
# Note: hw_control also ensure clean exit
# of the robot (i.e. going to safe position)
hw_control = HardwareControl()
# initializing the simulation by having it mirroring
# the real robot
angles,angular_velocities,_ = hw_control.get_real_robot()
hw_control.set_mirroring(angles,angular_velocities)
# a dummy policy using pd control
# with random target angles
policy = Policy()
# for drawing a goal in the simulation
goal = [3,4]
# torque control, at least 500Hz
frequency_manager = real_time_tools.FrequencyManager(500)
running=True
while running:
print("\n-- starting episode --")
# starting an episode
# set back the robot to init position using
# pd controller
print("\treset robot")
hw_control.reset_real_robot(500)
# shooting a real ball
print("\tshooting real ball")
hw_control.shoot_ball()
# shooting simulated balls
print("\tshooting virtual balls")
hw_control.shoot_sim_balls()
# getting the iteration number of the simulation at the start of the
# episode. Will be used to retrieve at the end of the episode
# its full history of observations
sim_iteration = hw_control.get_sim_iteration()
time_start = time.time()
# episode ends after 3 seconds,
while time.time()-time_start < 3 :
# getting newest state of the robot
angles,angular_velocities,_ = hw_control.get_real_robot()
# getting related torques from policy
torques = policy.get_torques(angles,
angular_velocities)
# sending torques to real robot
hw_control.set_real_torques(torques)
# sending mirroring commands to simulated robot
simulated_world_state = hw_control.set_mirroring(angles,angular_velocities)
# getting position of real ball from vision system
ball_position = hw_control.get_ball_from_vision()
# imposing frequency
frequency_manager.wait()
# episode end
# getting episode's simulation history
history = hw_control.get_sim_history(sim_iteration)
# counting the number of contacts between virtual racket
# and virtual balls
nb_contacts = sum([sum(observation.get_extended_state().balls_hits_racket)
for observation in history])
print("\tvirtual contacts: "+str(nb_contacts))
def run_reality(configuration,
switch,
interface_id_robot,
interface_id_ball_gun,
render=True):
frequency_manager = real_time_tools.FrequencyManager(
configuration.Interfaces.reality_frequency)
# note: rendering real robot but omitting ball (will be displayed
# by vision system) -> render_ball=False
sim_robot = run_support.Simulation(interface_id_robot,
ball=True,
robot=True,
ball_gun=True,
render=render,
render_ball=False,
width=configuration.Window.width,
height=configuration.Window.height)
sim_ball_gun = run_support.Simulation(interface_id_ball_gun,
ball=False,
robot=False,
ball_gun=False,
render=False)
time_start = time.time()
previous_ball_gun_id = -1
previous_action_id = -1
torques = [0,0,0]
while run_support.should_run(switch):
# -- Ball Gun -- #
# ball gun requested to shoot. Doing so.
ball_gun_action = sim_ball_gun.ball_gun_reader.read_action()
if all([ball_gun_action.is_valid(),
ball_gun_action.should_shoot(),
ball_gun_action.id!=previous_ball_gun_id]):
previous_ball_gun_id=ball_gun_action.id
sim_robot.world.reset(None,sim_robot.ball_gun)
# -- Torque Control Robot -- #
# reading from shared memory the current
# active action (action : roboball2d_interface.Action).
# note: the action is written in the shared memory
# by roboball2d_interface.Driver
action = sim_robot.torques_reader.read_action()
# the action is used if it is valid (i.e. not a dummy initial
# object) and has torques defined
should_use_action = previous_action_id!=action.id
should_use_action = should_use_action and action.is_valid()
if should_use_action:
torques = action.get_torques()
# note : the above implies : for as long there a no new action provided,
# the robot keeps applying torques of the last action
# dynamics of the robot based on the
# users input torques
# (setting the current time : having the robot running at 'reality' time
world_state = sim_robot.world.step(action.get_torques(),
relative_torques=action.are_torques_relative(),
current_time=time.time()-time_start)
previous_action_id = action.id
# sim.world returns world_state as defined in the roboball2d
# python package, but roboball2d_interfaces need an instance of
# o80_roboball2d.roboball2d_interface.WorldState, which is similar but supports
# serialization, i.e. can be written in shared_memory
sm_world_state = run_support.convert(world_state)
# sharing the observed world state with rest of the world
sim_robot.torques_writer.write_world_state(sm_world_state)
# rendering robot
if sim_robot.renderer:
world_state.ball = None
sim_robot.renderer.render(world_state,[],time_step=1.0/30.0,wait=False)
frequency_manager.wait()
def run_reality(render=True):
# cleanup of previous runs
o80.clear_shared_memory(segment_id_robot)
# ensures "reality" loops at the desired frequency
frequency_manager = real_time_tools.FrequencyManager(reality_frequency)
# creating the roboball2d instances
r2d = factory.get_default(nb_balls=0,
background_color=(1, 1, 1),
ground_color=(1, 1, 1),
window_size=(400, 200))
# o80 communication
# note : robot backend will write new observations only when
# a command has been executed
write_observation_on_new_commands = True
robot_backend = o80_roboball2d.RealRobotBackEnd(
segment_id_robot, write_observation_on_new_commands)
# running the simulation
time_start = time.time()
torques = [0, 0, 0]
running = True
# initializing roboball2d world_state
world_state = r2d.world.step([0, 0, 0],
current_time=time.time() - time_start)
# function for getting robot joint states (required by o80)
# from roboball2d.WorldState instance
def _get_joint_states(world_state):
robot = world_state.robot
states = []
for dof in range(3):
joint = o80_roboball2d.Joint()
joint.set_position(robot.joints[dof].angle)
joint.set_velocity(robot.joints[dof].angular_velocity)
try:
joint.set_torque(robot.joints[dof].torque)
except TypeError: # torque was None
joint.set_torque(0)
states.append(joint)
return states
while running:
try:
# o80 pulse : will send current state to
# frontend, and return desired torques (based
# on commands sent by the frontend)
states = _get_joint_states(world_state)
all_torques = robot_backend.pulse(states)
torques = [
all_torques.get(index).get_torque() for index in range(3)
]
# running one step of roboball2d
world_state = r2d.world.step(torques,
relative_torques=True,
current_time=time.time() - time_start)
# rendering robot
if render:
world_state.ball = None # robot only, no ball
world_state.balls = []
r2d.renderer.render(world_state, [],
time_step=1.0 / 30.0,
wait=False)
# running at desired frequency
frequency_manager.wait()
except KeyboardInterrupt:
running = False