def _log(segment_id, file_path, frequency, logger_id):
# creating an o80 frontend
try:
frontend = o80_pam.FrontEnd(segment_id)
except:
print(("\nfailed to start an o80 frontend on segment_id: {}. "
"Is a corresponding robot running ?\n").format(segment_id))
return
# serializer will compress observation instances into string
serializer = o80_pam.Serializer()
# setting the collecting loop frequency
frequency_manager = o80.FrequencyManager(frequency)
latest = None
# running the loop
with open(file_path, "wb+") as f:
# any other process may stop this loop by calling
# _set_stop
while not _should_stop(segment_id, logger_id):
if latest is None:
# first call
observations = [frontend.latest()]
else:
# reading all new observations written by the backend
# since the last pass of this loop
observations = frontend.get_observations_since(latest + 1)
for observation in observations:
# serializing and writting all observations
f.write(serializer.serialize(observation))
f.flush()
if observations:
# keeping track of the latest observation written
latest = observations[-1].get_iteration()
# running at desired frequency
frequency_manager.wait()
def _set_min_pressures(config, pam_config):
frontend = o80_pam.FrontEnd(config.segment_id)
for dof in range(4):
min_ago = pam_config.min_pressure(dof, pam_interface.sign.agonist)
min_antago = pam_config.min_pressure(dof, pam_interface.sign.antagonist)
frontend.add_command(dof, min_ago, min_antago, o80.Duration_us.seconds(3), o80.Mode.OVERWRITE)
frontend.pulse()
del frontend
def __init__(self, segment_id, frontend=None, burster=None):
if frontend is None:
self._frontend = o80_pam.FrontEnd(segment_id)
else:
self._frontend = frontend
if burster is None:
self._burster = self._frontend
else:
self._burster = burster
def __init__(self, segment_id, config, frequency, window=WINDOW):
# o80 frontend
self._frontend = o80_pam.FrontEnd(segment_id)
# data holder. "False" means the value has not been plotted
# yet
self._desired = [[ReadOnce(), ReadOnce()] for dof in range(4)]
self._observed = [[ReadOnce(), ReadOnce()] for dof in range(4)]
self._positions = [ReadOnce() for dof in range(4)]
self._frequency = ReadOnce()
# only data corresponding to a new iteration will be plotted
self._iteration = None
# avoiding concurrent read and write of data holder
self._lock = threading.Lock()
# a thread will get the data using the frontend
# (and the fyplot instance will read the data)
self._running = False
self._thread = None
# dynamic plot
self._plot = fyplot.Plot(segment_id, 50, window)
# min and max pressure according to the configuration
self._min_pressure = min(config.min_pressures_ago +
config.min_pressures_antago)
self._max_pressure = max(config.max_pressures_ago +
config.max_pressures_antago)
# 1 plot per dof (desired and observed pressure, position/angle)
for dof in range(4):
dof_plot = (
(partial(self.get_desired, dof, 0), (150, 0, 0)),
(partial(self.get_desired, dof, 1), (0, 150, 0)),
(partial(self.get_observed, dof, 0), (0, 255, 0)),
(partial(self.get_observed, dof, 1), (255, 0, 0)),
)
self._plot.add_subplot((self._min_pressure, self._max_pressure),
500, dof_plot)
# 1 plot for the frequency
freq_plot = ((self.get_frequency, (255, 255, 0)), )
max_frequency = frequency + frequency / 10.0
self._plot.add_subplot((0, max_frequency), 500, freq_plot)
def start(self):
"""
starts the observations collecting process
"""
try:
frontend = o80_pam.FrontEnd(self._segment_id)
del frontend
except Exception as e:
raise Exception("Failed to create an o80 frontend" +
"on segment_id {}: {}".format(self._segment_id, e))
self._process = Process(
target=_log,
args=(self._segment_id, self._file_path, self._frequency,
self._id),
)
_set_start(self._segment_id, self._id)
self._process.start()
def init(cls, segment_id, width=100):
cls._frontend = o80_pam.FrontEnd(segment_id)
# display config
cls._width = width
# init curses
cls._screen = curses.initscr()
curses.noecho()
curses.curs_set(0)
curses.start_color()
curses.use_default_colors()
curses.init_pair(1, curses.COLOR_GREEN, -1)
curses.init_pair(2, curses.COLOR_BLUE, -1)
cls._screen.keypad(1)
cls._monitor_exit_thread = threading.Thread(target=cls._monitor_exit)
cls._monitor_exit_thread.setDaemon(True)
cls._monitor_exit_thread.start()
def _run(
tennicam_segment_id: str,
o80_pam_segment_id: str,
frequency: float,
filepath: pathlib.Path,
):
"""
Creates an tennicam_client frontend and a o80_pam frontend and
uses them to get all ball observations and pam robot observations;
and dumping them in a corresponding string in filepath.
"""
ball_frontend = tennicam_client.FrontEnd(tennicam_segment_id)
robot_frontend = o80_pam.FrontEnd(o80_pam_segment_id)
iteration = ball_frontend.latest().get_iteration()
ball_getters = ("get_ball_id", "get_time_stamp", "get_position",
"get_velocity")
robot_getters = ("get_time_stamp", "get_positions", "get_velocities")
frequency_manager = o80.FrequencyManager(frequency)
with filepath.open(mode="w") as f:
try:
while not signal_handler.has_received_sigint():
iteration += 1
obs_ball = ball_frontend.latest()
obs_robot = robot_frontend.latest()
ball_values = tuple(
(getattr(obs_ball, getter)() for getter in ball_getters))
robot_values = tuple(
(getattr(obs_robot, getter)() for getter in robot_getters))
str_ = repr((ball_values, robot_values))
f.write(str_)
f.write("\n")
frequency_manager.wait()
except (KeyboardInterrupt, SystemExit):
pass
except Exception as e:
print("Error:", e)
def __init__(self, hysr_config, reward_function):
self._hysr_config = hysr_config
# we will track the episode number
self._episode_number = -1
# we will track the step number (reset at the start
# of each episode)
self._step_number = -1
# we end an episode after a fixed number of steps
self._nb_steps_per_episode = hysr_config.nb_steps_per_episode
# note: if self._nb_steps_per_episode is 0 or less,
# an episode will end based on a threshold
# in the z component of the ball position
# (see method _episode_over)
# this instance of HysrOneBall interacts with several
# instances of mujoco (pseudo real robot, simulated robot,
# possibly instances of mujoco for extra balls).
# Listing all the corresponding mujoco_ids
self._mujoco_ids = []
# pam muscles configuration
self._pam_config = pam_interface.JsonConfiguration(
hysr_config.pam_config_file)
# to control pseudo-real robot (pressure control)
if not hysr_config.real_robot:
self._real_robot_handle, self._real_robot_frontend = configure_mujoco.configure_pseudo_real(
hysr_config.pam_config_file,
graphics=hysr_config.graphics_pseudo_real,
accelerated_time=hysr_config.accelerated_time,
)
self._mujoco_ids.append(self._real_robot_handle.get_mujoco_id())
else:
# real robot: making some sanity check that the
# rest of the configuration is ok
if hysr_config.instant_reset:
raise ValueError(
str("HysrOneBall configured for "
"real robot and instant reset."
"Real robot does not support "
"instant reset."))
if hysr_config.accelerated_time:
raise ValueError(
str("HysrOneBall configured for "
"real robot and accelerated time."
"Real robot does not support "
"accelerated time."))
# to control the simulated robot (joint control)
self._simulated_robot_handle = configure_mujoco.configure_simulation(
hysr_config)
self._mujoco_ids.append(self._simulated_robot_handle.get_mujoco_id())
# where we want to shoot the ball
self._target_position = hysr_config.target_position
self._goal = self._simulated_robot_handle.interfaces[SEGMENT_ID_GOAL]
# to read all recorded trajectory files
self._trajectory_reader = context.BallTrajectories()
# if requested, logging info about the frequencies of the steps and/or the
# episodes
if hysr_config.frequency_monitoring_step:
size = 1000
self._frequency_monitoring_step = frequency_monitoring.FrequencyMonitoring(
SEGMENT_ID_STEP_FREQUENCY, size)
else:
self._frequency_monitoring_step = None
if hysr_config.frequency_monitoring_episode:
size = 1000
self._frequency_monitoring_episode = (
frequency_monitoring.FrequencyMonitoring(
SEGMENT_ID_EPISODE_FREQUENCY, size))
else:
self._frequency_monitoring_episode = None
# if o80_pam (i.e. the pseudo real robot)
# has been started in accelerated time,
# the corresponding o80 backend will burst through
# an algorithm time step
self._accelerated_time = hysr_config.accelerated_time
if self._accelerated_time:
self._o80_time_step = hysr_config.o80_pam_time_step
self._nb_robot_bursts = int(hysr_config.algo_time_step /
hysr_config.o80_pam_time_step)
# pam_mujoco (i.e. simulated ball and robot) should have been
# started in accelerated time. It burst through algorithm
# time steps
self._mujoco_time_step = hysr_config.mujoco_time_step
self._nb_sim_bursts = int(hysr_config.algo_time_step /
hysr_config.mujoco_time_step)
# the config sets either a zero or positive int (playing the
# corresponding indexed pre-recorded trajectory) or a negative int
# (playing randomly selected indexed trajectories)
if hysr_config.trajectory >= 0:
self._ball_behavior = _BallBehavior(index=hysr_config.trajectory)
else:
self._ball_behavior = _BallBehavior(random=True)
# the robot will interpolate between current and
# target posture over this duration
self._period_ms = hysr_config.algo_time_step
# reward configuration
self._reward_function = reward_function
# to get information regarding the ball
# (instance of o80_pam.o80_ball.o80Ball)
self._ball_communication = self._simulated_robot_handle.interfaces[
SEGMENT_ID_BALL]
# to send pressure commands to the real or pseudo-real robot
# (instance of o80_pam.o80_pressures.o80Pressures)
# hysr_config.real robot is either false (i.e. pseudo real
# mujoco robot) or the segment_id of the real robot backend
if not hysr_config.real_robot:
self._pressure_commands = self._real_robot_handle.interfaces[
SEGMENT_ID_PSEUDO_REAL_ROBOT]
else:
self._real_robot_frontend = o80_pam.FrontEnd(
hysr_config.real_robot)
self._pressure_commands = o80_pam.o80Pressures(
hysr_config.real_robot, frontend=self._real_robot_frontend)
# will encapsulate all information
# about the ball (e.g. min distance with racket, etc)
self._ball_status = context.BallStatus(hysr_config.target_position)
# to send mirroring commands to simulated robots
self._mirrorings = [
self._simulated_robot_handle.interfaces[SEGMENT_ID_ROBOT_MIRROR]
]
# to move the hit point marker
# (instance of o80_pam.o80_hit_point.o80HitPoint)
self._hit_point = self._simulated_robot_handle.interfaces[
SEGMENT_ID_HIT_POINT]
# tracking if this is the first step of the episode
# (a call to the step function sets it to false, call to reset function sets it
# back to true)
self._first_episode_step = True
# normally an episode ends when the ball z position goes
# below a certain threshold (see method _episode_over)
# this is to allow user to force ending an episode
# (see force_episode_over method)
self._force_episode_over = False
# if false, the system will reset via execution of commands
# if true, the system will reset by resetting the simulations
# Only "false" is supported by the real robot
self._instant_reset = hysr_config.instant_reset
# adding extra balls (if any)
if (hysr_config.extra_balls_sets is not None
and hysr_config.extra_balls_sets > 0):
self._extra_balls = []
for setid in range(hysr_config.extra_balls_sets):
# balls: list of instances of _ExtraBalls (defined in this file)
# mirroring : for sending mirroring command to the robot
# of the set (joint controlled)
# (instance of
# o80_pam.o80_robot_mirroring.o80RobotMirroring)
balls, mirroring, mujoco_id, frontend = _get_extra_balls(
setid, hysr_config)
self._extra_balls.extend(balls)
self._mirrorings.append(mirroring)
self._mujoco_ids.append(mujoco_id)
self._extra_balls_frontend = frontend
else:
self._extra_balls = []
self._extra_balls_frontend = None
# for running all simulations (main + for extra balls)
# in parallel (i.e. when bursting is called, all mujoco
# instance perform step(s) in parallel)
self._parallel_burst = pam_mujoco.mirroring.ParallelBurst(
self._mirrorings)
# if set, logging the position of the robot at the end of reset, and possibly
# get a warning when this position drifts as the number of episodes increase
if hysr_config.robot_integrity_check is not None:
self._robot_integrity = robot_integrity.RobotIntegrity(
hysr_config.robot_integrity_threshold,
file_path=hysr_config.robot_integrity_check,
)
else:
self._robot_integrity = None
# when starting, the real robot and the virtual robot(s)
# may not be aligned, which may result in graphical issues,
# so aligning them
# (get values of real robot via self._pressure_commands,
# and set values to all simulated robot via self._mirrorings)
# source of mirroring in pam_mujoco.mirroring.py
pam_mujoco.mirroring.align_robots(self._pressure_commands,
self._mirrorings)
import time
import o80_pam
segment_id = "o80_pam_robot"
frontend = o80_pam.FrontEnd(segment_id)
def _print_title(title):
print("\n")
print("\n".join(["-" * len(title), title, "-" * len(title)]), "\n")
_print_title("latest observation")
# newest observation generated by the backend
observation = frontend.latest()
# here all the attributes encapsulated by an observation
# at each backend iteration, an observation is generated.
print("iteration", observation.get_iteration())
# the time stamp at which the observation was generated.
# it is an integer representing time in nano seconds
print("timestamp", observation.get_time_stamp())
# the pressure of the muscles as measured by sensors
print("observed pressures", observation.get_observed_pressures())
# the pressure of the muscles as the commands requested them to be.
def _get_frontend(config):
try :
frontend = o80_pam.FrontEnd(config.segment_id)
return frontend
except Exception as e :
return None
def __init__(
self,
mujoco_id: str,
burst_mode: bool = False,
accelerated_time: bool = False,
graphics: bool = True,
time_step: float = 0.002,
table: MujocoTable = None,
robot1: MujocoRobot = None,
robot2: MujocoRobot = None,
balls: list = [], # list of mujoco_item.MujocoItem
goals: list = [], # list of mujoco_item.MujocoItem
hit_points: list = [],
# list of mujoco_item.MujocoItems (with 's' at the end)
combined: MujocoItems = None,
read_only: bool = False,
):
self._mujoco_id = mujoco_id
if not read_only:
# combined (instance of mujoco_item.MujocoItems)
# supports only a limited set of size (see source of MujocoItems)
self.combined = combined
if combined and (combined.size not in combined.accepted_sizes):
raise ValueError(
"pam_mujoco.mujoco_item.MujocoItems supports "
"a limited set of size ({}). "
"{} provided".format(
", ".join([str(a) for a in combined.accepted_sizes]),
combined.size,
))
# creating the mujoco xml model file
logging.info(
"creating the xml model file for {}".format(mujoco_id))
if combined:
all_balls = list(balls) + combined.items["balls"]
all_goals = list(goals) + combined.items["goals"]
all_hit_points = list(
hit_points) + combined.items["hit_points"]
else:
all_balls = balls
all_goals = goals
all_hit_points = hit_points
items = models.model_factory(
mujoco_id,
time_step=time_step,
table=table,
balls=all_balls,
goals=all_goals,
hit_points=all_hit_points,
robot1=robot1,
robot2=robot2,
)
# creating the mujoco config
logging.info(
"creating mujoco configuration for {}".format(mujoco_id))
config = pam_mujoco_wrp.MujocoConfig(mujoco_id)
config.set_burst_mode(burst_mode)
config.set_accelerated_time(accelerated_time)
config.set_model_path(items["path"])
config.set_graphics(graphics)
if items["robot1"]:
config.set_racket_robot1(items["robot1"].geom_racket)
if items["robot2"]:
config.set_racket_robot2(items["robot2"].geom_racket)
if items["table"]:
config.set_table(items["table"].geom_plate)
_get_ball = partial(_get_mujoco_item_control,
pam_mujoco_wrp.MujocoItemTypes.ball)
_get_hit_point = partial(_get_mujoco_item_control,
pam_mujoco_wrp.MujocoItemTypes.hit_point)
_get_goal = partial(_get_mujoco_item_control,
pam_mujoco_wrp.MujocoItemTypes.goal)
mujoco_item_controls = []
if balls:
logging.info("creating item controls for {} balls".format(
len(balls)))
mujoco_item_controls.extend([
_get_ball(mujoco_item, model_item)
for mujoco_item, model_item in zip(
balls, items["balls"][:len(balls)])
])
if hit_points:
logging.info("creating item controls for {} hit points".format(
len(hit_points)))
mujoco_item_controls.extend([
_get_hit_point(mujoco_item, model_item)
for mujoco_item, model_item in zip(
hit_points, items["hit_points"][:len(hit_points)])
])
if goals:
logging.info("creating item controls for {} goals".format(
len(goals)))
mujoco_item_controls.extend([
_get_goal(mujoco_item, model_item)
for mujoco_item, model_item in zip(
goals, items["goals"][:len(goals)])
])
for mujoco_item_control in mujoco_item_controls:
config.add_control(mujoco_item_control)
if combined:
logging.info("creating item controls for combined items")
mujoco_combined_items_control = _get_mujoco_items_control(
combined,
items["balls"][len(balls):],
items["goals"][len(goals):],
items["hit_points"][len(hit_points):],
items["robot1"].geom_racket,
)
# function name, depending on the number of combined mujoco items.
# e.g. add_3_control or add_10_control, see
# include/mujoco_config.hpp and/or srcpy/wrappers.cpp
add_function_name = "_".join(
["add", str(combined.size), "control"])
# pointer to the function
add_function = getattr(config, add_function_name)
# calling the function
add_function(mujoco_combined_items_control)
for key, robot in zip(("robot1", "robot2"), (robot1, robot2)):
if robot:
logging.info("creating item controls for {}".format(key))
r = _get_mujoco_robot_control(robot, items[key])
if r:
config.add_control(r)
# waiting for pam_mujoco to have created the shared memory segment_id
logging.info("waiting for pam_mujoco {}".format(mujoco_id))
created = False
while not created:
created = shared_memory.wait_for_segment(mujoco_id, 100)
# writing the mujoco config in the shared memory.
# the mujoco executable is expected to read it and start
logging.info(
"sharing mujoco configuration for {}".format(mujoco_id))
pam_mujoco_wrp.set_mujoco_config(config)
# waiting for pam_mujoco to have created the shared memory segment_id
logging.info("waiting for pam_mujoco {}".format(mujoco_id))
created = False
while not created:
created = shared_memory.wait_for_segment(mujoco_id, 100)
# waiting for mujoco to report it is ready
logging.info("waiting for mujoco executable {}".format(mujoco_id))
pam_mujoco_wrp.wait_for_mujoco(mujoco_id)
# if read only, we did not create the mujoco configuration,
# (which has been written by another process)
# so we read it from the shared memory
if read_only:
config = pam_mujoco_wrp.get_mujoco_config(mujoco_id)
balls, goals, hit_points = [], [], []
for item in config.item_controls:
if item.active_only:
control = MujocoItem.COMMAND_ACTIVE_CONTROL
else:
control = MujocoItem.CONSTANT_CONTROL
instance = MujocoItem(item.segment_id, control=control)
if item.type == pam_mujoco_wrp.MujocoItemTypes.ball:
balls.append(instance)
elif item.type == pam_mujoco_wrp.MujocoItemTypes.goal:
goals.append(instance)
else:
hit_points.append(instance)
robots = []
for joint_robot in config.joint_controls:
r = MujocoRobot(
True, # has no effect, because no xml config file is written
joint_robot.segment_id,
control=MujocoRobot.JOINT_CONTROL,
)
robots.append(r)
for pressure_robot in config.pressure_controls:
r = MujocoRobot(
True, # has no effect, because no xml config file is written
joint_robot.segment_id,
control=MujocoRobot.PRESSURE_CONTROL,
)
robots.append(r)
try:
robot1 = robots[0]
except Exception:
robot1 = None
try:
robot2 = robots[1]
except Exception:
robot2 = None
class _Combined:
size = 0
segment_id = None
combined = None
item_controls_attrs = [(nb_balls,
"item_{}_controls".format(nb_balls))
for nb_balls in (3, 10, 20, 50, 100)]
for (nb_balls, attr) in item_controls_attrs:
mujoco_items_control_instance = getattr(config, attr)
if mujoco_items_control_instance:
combined = _Combined
combined.size = nb_balls
combined.segment_id = mujoco_items_control_instance[
0].segment_id
break
# if bursting mode, creating a burster client
if burst_mode:
self._burster_client = o80.BursterClient(mujoco_id)
else:
self._burster_client = None
# creating o80 frontends
self.frontends = {}
self.interfaces = {}
for item in balls:
if item.control != MujocoItem.NO_CONTROL:
logging.info("creating o80 frontend for ball {} / {}".format(
mujoco_id, item.segment_id))
frontend = o80_pam.BallFrontEnd(item.segment_id)
interface = o80_pam.o80Ball(item.segment_id, frontend)
self.frontends[item.segment_id] = frontend
self.interfaces[item.segment_id] = interface
for item in goals:
if item.control != MujocoItem.NO_CONTROL:
logging.info("creating o80 frontend for goal {} / {}".format(
mujoco_id, item.segment_id))
frontend = o80_pam.BallFrontEnd(item.segment_id)
interface = o80_pam.o80Goal(item.segment_id, frontend)
self.frontends[item.segment_id] = frontend
self.interfaces[item.segment_id] = interface
for item in hit_points:
if item.control != MujocoItem.NO_CONTROL:
logging.info(
"creating o80 frontend for hit point {} / {}".format(
mujoco_id, item.segment_id))
frontend = o80_pam.BallFrontEnd(item.segment_id)
interface = o80_pam.o80HitPoint(item.segment_id, frontend)
self.frontends[item.segment_id] = frontend
self.interfaces[item.segment_id] = interface
for robot in robot1, robot2:
if robot:
if robot.control == MujocoRobot.JOINT_CONTROL:
logging.info(
"creating o80 frontend for joint control of {} / {}".
format(mujoco_id, robot.segment_id))
frontend = o80_pam.JointFrontEnd(robot.segment_id)
interface = o80_pam.o80RobotMirroring(
robot.segment_id,
frontend=frontend,
burster=self._burster_client,
)
self.frontends[robot.segment_id] = frontend
self.interfaces[robot.segment_id] = interface
if robot.control == MujocoRobot.PRESSURE_CONTROL:
logging.info(
"creating o80 frontend for pressure control of {} / {}"
.format(mujoco_id, robot.segment_id))
frontend = o80_pam.FrontEnd(robot.segment_id)
interface = o80_pam.o80Pressures(
robot.segment_id,
frontend=frontend,
burster=self._burster_client,
)
self.frontends[robot.segment_id] = frontend
self.interfaces[robot.segment_id] = interface
if combined:
self.frontends[
combined.segment_id] = self.get_extra_balls_frontend(
combined.segment_id, combined.size)
# for tracking contact
self.contacts = {}
for item in list(balls) + list(goals) + list(hit_points):
if item.contact_type == pam_mujoco_wrp.ContactTypes.table:
self.contacts[item.segment_id] = item.segment_id + "_table"
if item.contact_type == pam_mujoco_wrp.ContactTypes.racket1:
self.contacts[item.segment_id] = item.segment_id + "_racket1"
if item.contact_type == pam_mujoco_wrp.ContactTypes.racket2:
self.contacts[item.segment_id] = item.segment_id + "_racket2"
if combined and not read_only:
# see src/add_controllers.cpp, function add_items_control
for index, item in enumerate(list(combined.iterate())):
if item.contact_type == pam_mujoco_wrp.ContactTypes.table:
self.contacts[item.segment_id] = (combined.segment_id +
"_table_" + str(index))
if item.contact_type == pam_mujoco_wrp.ContactTypes.racket1:
self.contacts[item.segment_id] = (combined.segment_id +
"_racket1_" + str(index))
if item.contact_type == pam_mujoco_wrp.ContactTypes.racket2:
self.contacts[item.segment_id] = (combined.segment_id +
"_racket2_" + str(index))
for sid in self.contacts.keys():
self.reset_contact(sid)
logging.info("handle for mujoco {} created".format(mujoco_id))
