def on_received_get_robot_state(self, context: Context, data: RobotName):
env = self.env
speed = env.speed
omega = 0.0 # XXX
if data == self.robot_name:
q = env.cartesian_from_weird(env.cur_pos, env.cur_angle)
v = geometry.se2_from_linear_angular([speed, 0], omega)
state = MyRobotInfo(pose=q,
velocity=v,
last_action=env.last_action,
wheels_velocities=env.wheelVels)
rs = MyRobotState(robot_name=data,
t_effective=self.current_time,
state=state)
else:
obj: DuckiebotObj = self.npcs[data]
q = env.cartesian_from_weird(obj.pos, obj.angle)
# FIXME: how to get velocity?
v = geometry.se2_from_linear_angular([0, 0], 0)
state = MyRobotInfo(pose=q,
velocity=v,
last_action=np.array([0, 0]),
wheels_velocities=np.array([0, 0]))
rs = MyRobotState(robot_name=data,
t_effective=self.current_time,
state=state)
# timing information
t = timestamp_from_seconds(self.current_time)
ts = TimeSpec(time=t,
frame=self.episode_name,
clock=context.get_hostname())
timing = TimingInfo(acquired={'state': ts})
context.write('robot_state', rs, timing=timing) #, with_schema=True)
def _create_scenarios(self, context: Context):
available = list_maps()
for map_name in self.config.maps:
if not map_name in available:
msg = f'Cannot find map name "{map_name}, know {available}'
raise Exception(msg)
s: str = _get_map_yaml(map_name)
# yaml_str = update_map(yaml_str)
yaml_data = yaml.load(s, Loader=yaml.SafeLoader)
update_map(yaml_data)
yaml_str = yaml.dump(yaml_data)
po = dw.construct_map(yaml_data)
for imap in range(self.config.scenarios_per_map):
scenario_name = f'{map_name}-{imap}'
nrobots = self.config.robots_npcs + self.config.robots_pcs
poses = sample_many_good_starting_poses(po, nrobots,
only_straight=self.config.only_straight,
min_dist=self.config.min_dist,
delta_theta_rad=np.deg2rad(self.config.theta_tol_deg),
delta_y_m=self.config.dist_tol_m)
poses_pcs = poses[:self.config.robots_pcs]
poses_npcs = poses[self.config.robots_pcs:]
robots = {}
for i in range(self.config.robots_pcs):
pose = poses_pcs[i]
vel = g.se2_from_linear_angular([0, 0], 0)
robot_name = 'ego' if i == 0 else "player%d" % i
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(description='Playable robot',
playable=True,
configuration=configuration)
for i in range(self.config.robots_npcs):
pose = poses_npcs[i]
vel = g.se2_from_linear_angular([0, 0], 0)
robot_name = "npc%d" % i
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(description='NPC robot',
playable=False,
configuration=configuration)
ms = MyScenario(scenario_name=scenario_name, environment=yaml_str, robots=robots)
self.state.scenarios_to_go.append(ms)
def _create_scenarios(self, context: Context):
available = list_maps()
for map_name in self.config.maps:
if not map_name in available:
msg = f'Cannot find map name "{map_name}, know {available}'
raise Exception(msg)
yaml_str: str = _get_map_yaml(map_name)
po = load_map(map_name)
for i in range(self.config.scenarios_per_map):
scenario_name = f'{map_name}-{i}'
nrobots = self.config.robots_npcs + self.config.robots_pcs
poses = sample_many_good_starting_poses(
po,
nrobots,
only_straight=self.config.only_straight,
min_dist=self.config.min_dist)
poses_pcs = poses[:self.config.robots_pcs]
poses_npcs = poses[self.config.robots_pcs:]
robots = {}
for i in range(self.config.robots_pcs):
pose = poses_pcs[i]
vel = geometry.se2_from_linear_angular([0, 0], 0)
robot_name = 'ego' if i == 0 else "player%d" % i
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(
description='Playable robot',
playable=True,
configuration=configuration)
for i in range(self.config.robots_npcs):
pose = poses_npcs[i]
vel = geometry.se2_from_linear_angular([0, 0], 0)
robot_name = "npc%d" % i
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(
description='NPC robot',
playable=False,
configuration=configuration)
ms = MyScenario(scenario_name=scenario_name,
environment=yaml_str,
robots=robots)
self.state.scenarios_to_go.append(ms)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
# Your code comes here!
q,_ = self.TSE2_from_state()
_, direction = geo.translation_angle_from_SE2(q)
linear = [commands.linear_velocity, 0]
angular = (commands.linear_velocity / self.parameters.wheel_distance) * math.tan(commands.steering_angle)
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# Call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
return CarDynamics(self.parameters, c1, t1)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
# compute the linear, angular velocities for the platform
# using the simple car equations
longitudinal = commands.linear_velocity
angular = commands.linear_velocity / self.parameters.wheel_distance * math.tan(
commands.steering_angle)
lateral = 0
linear = [longitudinal, lateral]
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# Call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
return CarDynamics(self.parameters, c1, t1)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
# calculate linear velocities
x_vel = commands.linear_velocity
y_vel = 0
linear = [x_vel, y_vel]
# calculate angular velocities
angular = commands.linear_velocity * math.tan(
commands.steering_angle) / self.parameters.wheel_distance
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# Call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
return CarDynamics(self.parameters, c1, t1)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
# Your code comes here!
# linear_velocity = [x_dot, y_dot]
linear = [commands.linear_velocity, 0]
# angular = [theta_dot]
angular = commands.linear_velocity/self.parameters.wheel_distance * np.tan(commands.steering_angle)
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# Call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
return CarDynamics(self.parameters, c1, t1)
def compute_forces(self, commands):
linear = [
commands[0] * self.max_force[0], commands[1] * self.max_force[1]
]
angular = commands[2] * self.max_force[2]
vel = se2_from_linear_angular(linear, angular)
return vel
def kinematics2d_test():
q0 = geo.SE2_from_translation_angle([0, 0], 0)
v0 = geo.se2.zero()
c0 = q0, v0
s0 = GenericKinematicsSE2.initialize(c0)
k = 0.4
radius = 1.0 / k
print("radius: %s" % radius)
v = 3.1
perimeter = 2 * np.pi * radius
dt_loop = perimeter / v
w = 2 * np.pi / dt_loop
dt = dt_loop * 0.25
commands = geo.se2_from_linear_angular([v, 0], w)
s1 = s0.integrate(dt, commands)
s2 = s1.integrate(dt, commands)
s3 = s2.integrate(dt, commands)
s4 = s3.integrate(dt, commands)
seq = [s0, s1, s2, s3, s4]
for _ in seq:
q1, v1 = _.TSE2_from_state()
print("%s" % geo.SE2.friendly(q1))
assert_almost_equal(geo.translation_from_SE2(s1.TSE2_from_state()[0]),
[radius, radius])
assert_almost_equal(geo.translation_from_SE2(s2.TSE2_from_state()[0]),
[0, radius * 2])
assert_almost_equal(geo.translation_from_SE2(s3.TSE2_from_state()[0]),
[-radius, radius])
assert_almost_equal(geo.translation_from_SE2(s4.TSE2_from_state()[0]),
[0, 0])
def integrate(self, dt: float, commands: PWMCommands) -> 'DynamicModel':
# previous velocities (v0)
linear_angular_prev = geo.linear_angular_from_se2(self.v0)
linear_prev = linear_angular_prev[0]
longit_prev = linear_prev[0]
lateral_prev = linear_prev[1]
angular_prev = linear_angular_prev[1]
# predict the acceleration of the vehicle
x_dot_dot = self.model(commands,
self.parameters,
u=longit_prev,
w=angular_prev)
# convert the acceleration to velocity by forward euler
longitudinal = longit_prev + dt * x_dot_dot[0]
angular = angular_prev + dt * x_dot_dot[1]
lateral = 0.0
linear = [longitudinal, lateral]
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
return DynamicModel(self.parameters, c1, t1)
def integrate(self, dt: float, commands: WheelVelocityCommands):
"""
:param dt:
:param commands: an instance of WheelVelocityCommands
:return:
"""
# Compute the linear velocity for the wheels
# by multiplying radius times angular velocity
v_r = self.parameters.radius_right * commands.right_wheel_angular_velocity
v_l = self.parameters.radius_left * commands.left_wheel_angular_velocity
# compute the linear, angular velocities for the platform
# using the differential drive equations
longitudinal = (v_r + v_l) * 0.5
angular = (v_r - v_l) / self.parameters.wheel_distance
lateral = 0.0
linear = [longitudinal, lateral]
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# Call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
return DifferentialDriveDynamics(self.parameters, c1, t1)
def compute_velocities(self, commands):
linear = [
commands[0] * self.max_linear_velocity[0],
commands[1] * self.max_linear_velocity[1]
]
angular = commands[2] * self.max_angular_velocity
vel = se2_from_linear_angular(linear, angular)
return vel
def integrate(self, dt: float, commands: PWMCommands) -> "DynamicModel":
# previous velocities (v0)
linear_angular_prev = geo.linear_angular_from_se2(self.v0)
linear_prev = linear_angular_prev[0]
longit_prev = linear_prev[0]
lateral_prev = linear_prev[1]
angular_prev = linear_angular_prev[1]
# predict the acceleration of the vehicle
x_dot_dot = self.model(commands,
self.parameters,
u=longit_prev,
w=angular_prev)
# convert the acceleration to velocity by forward euler
longitudinal = longit_prev + dt * x_dot_dot[0]
angular = angular_prev + dt * x_dot_dot[1]
lateral = 0.0
linear = [longitudinal, lateral]
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
# now we compute the axis rotation using the inverse way...
# forward = both wheels spin positive
# angular_velocity = wR*R_r/d - Wl*R_l/d # if R rotates more, we increase theta
# linear_velocity = (wR*R_r + Wl*R_l)/2
# that is
# [ang, lin ] = [ Rr/d -Rl/d; Rr/2 Rl/2] * [wR wL]
d = self.parameters.wheel_distance
Rr = self.parameters.wheel_radius_right
Rl = self.parameters.wheel_radius_left
M = np.array([[Rr / d, -Rl / d], [Rr / 2, Rl / 2]])
anglin = np.array((angular, longitudinal))
MInv = np.linalg.inv(M)
wRL = MInv @ anglin
wR = float(wRL[0, 0])
wL = float(wRL[1, 0])
axis_left_rad = self.axis_left_rad + wL * dt
axis_right_rad = self.axis_right_rad + wR * dt
return DynamicModel(self.parameters,
c1,
t1,
axis_left_rad=axis_left_rad,
axis_right_rad=axis_right_rad)
def TSE2_from_state(self):
"""
For visualization purposes, this function gets a configuration in SE2
from the internal state.
"""
# pose
q = geo.SE2_from_R2(self.p0)
# velocity
linear = self.v0
angular = 0.0
v = geo.se2_from_linear_angular(linear, angular)
return q, v
def display_nmap(report, nmap):
with report.subsection("sensing") as sub:
display_nmap_sensing(sub, nmap)
f = report.figure()
with f.plot("map") as pylab:
for bd, pose in nmap.data:
commands = bd["commands"]
warnings.warn("redo this properly")
if len(commands) == 3:
x, y, omega = commands
else:
x, y = commands
omega = 0
vel = se2_from_linear_angular([x, y], omega)
plot_arrow_SE2(pylab, pose)
plot_arrow_se2(pylab, pose, vel, length=0.04, color="g")
pylab.axis("equal")
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
# Your code comes here!
linear = [0, 0]
angular = 0
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# Call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
return CarDynamics(self.parameters, c1, t1)
def integrate(self, dt, commands):
"""
:param dt:
:param commands: an instance of CarCommands
:return:
"""
check_isinstance(commands, CarCommands)
us = commands.linear_velocity
ut = commands.steering_angle
thetha = self.theta
L = self.parameters.wheel_distance
# Simple car equations.
linear = us * np.cos(thetha), us * np.sin(thetha)
angular = us / L * np.tan(ut)
# represent this as se(2)
commands_se2 = geo.se2_from_linear_angular(linear, angular)
# Call the "integrate" function of GenericKinematicsSE2
s1 = GenericKinematicsSE2.integrate(self, dt, commands_se2)
# new state
c1 = s1.q0, s1.v0
t1 = s1.t0
# Update state.
self.theta = s1.q0
return CarDynamics(self.parameters, c1, t1)
def main():
# Set in the docker-compose env section
config_ = env_as_yaml('middleware_parameters')
logger.info('parameters:\n\n%s' % config_)
config = cast(MyConfig, object_from_ipce(config_, MyConfig))
# first open all fifos
logger.info("Opening the sim CI")
sim_ci = ComponentInterface(config.sim_in,
config.sim_out,
expect_protocol=protocol_simulator,
nickname="simulator",
timeout=config.timeout_regular)
logger.info("Pipes connected to simulator")
logger.info("Opening the agent CI")
agent_ci = ComponentInterface(config.agent_in,
config.agent_out,
expect_protocol=protocol_agent,
nickname="agent",
timeout=config.timeout_regular)
logger.info("Pipes connected to agent")
agents = [agent_ci]
# We enable CC of the pipes for debugging
if logger.level < logging.DEBUG:
logfile = "/fifos/agentlog"
logfile2 = "/fifos/simlog"
ff = open(logfile, "wb")
ff2 = open(logfile, "wb")
agent_ci.cc(ff)
sim_ci.cc(ff2)
logger.info(f"opened {logfile} as agent cc")
logger.info(f"opened {logfile2} as sim cc")
# then check compatibility
# so that everything fails gracefully in case of error
logger.info("Checking sim protocol compatibility...")
sim_ci._get_node_protocol(timeout=config.timeout_initialization)
logger.info(
"Sim protocol compatible, checking agent protocol compatibility...")
agent_ci._get_node_protocol(timeout=config.timeout_initialization)
logger.info("Acquired nodes protocols")
check_compatibility_between_agent_and_sim(agent_ci, sim_ci)
logger.info("Compatibility verified.")
attempt_i = 0
ep = 0
try:
nfailures = 0
logger.info("Sending seed to sim")
sim_ci.write_topic_and_expect_zero('seed', config.seed)
logger.info("Sending seed to agent")
agent_ci.write_topic_and_expect_zero('seed', config.seed)
logger.info("Received feedback from peer containers")
# TODO we should have a proper handling of invalid map name
map_name = os.environ.get('MAP_NAME', 'loop_empty')
yaml_string: str = _get_map_yaml(map_name)
yaml_data = yaml.load(yaml_string, Loader=yaml.SafeLoader)
placed_obj = construct_map(yaml_data)
pose = sample_good_starting_pose(placed_obj, only_straight=True)
vel = geometry.se2_from_linear_angular([0, 0], 0)
logger.info(f"Got good starting pose at: {pose}")
robot1_config = RobotConfiguration(pose=pose, velocity=vel)
robot1 = ScenarioRobotSpec(description="Development agent",
playable=True,
configuration=robot1_config,
motion=None)
scenario1 = Scenario("scenario1",
environment=yaml_string,
robots={"agent1": robot1})
unique_episode = EpisodeSpec("episode1", scenario1)
episodes = [unique_episode]
# Since we dont have a scenario maker, we will loop the episode (for now)
while episodes:
if nfailures >= config.max_failures:
msg = 'Too many failures: %s' % nfailures
raise Exception(msg) # XXX
episode_spec = episodes[0]
episode_name = episode_spec.episode_name
ep += 1
episode_name = f'episode_{ep}'
logger.info(f'Starting {episode_name}')
# dn_final = os.path.join(log_dir, episode_name)
# if os.path.exists(dn_final):
# shutil.rmtree(dn_final)
# dn = os.path.join(attempts, episode_name + '.attempt%s' % attempt_i)
# if os.path.exists(dn):
# shutil.rmtree(dn)
# if not os.path.exists(dn):
# os.makedirs(dn)
# fn = os.path.join(dn, 'log.gs2.cbor')
# fn_tmp = fn + '.tmp'
# fw = open(fn_tmp, 'wb')
# agent_ci.cc(fw)
# sim_ci.cc(fw)
logger.info('Now running episode')
num_playable = len([
_ for _ in episode_spec.scenario.robots.values() if _.playable
])
if num_playable != len(agents):
msg = f'The scenario requires {num_playable} robots, but I only know {len(agents)} agents.'
raise Exception(msg) # XXX
try:
length_s = run_episode(
sim_ci,
agents,
episode_name=episode_name,
scenario=episode_spec.scenario,
episode_length_s=config.episode_length_s,
physics_dt=config.physics_dt)
logger.info('Finished episode %s' % episode_name)
except:
msg = 'Anomalous error from run_episode():\n%s' % traceback.format_exc(
)
logger.error(msg)
raise
# finally:
# fw.close()
# os.rename(fn_tmp, fn)
# output = os.path.join(dn, 'visualization')
# logger.info('Now creating visualization and analyzing statistics.')
# logger.warning('This might take a LONG time.')
#
# with notice_thread("Visualization", 2):
# evaluated = read_and_draw(fn, dn)
# logger.info('Finally visualization is done.')
# stats = {}
# for k, evr in evaluated.items():
# assert isinstance(evr, RuleEvaluationResult)
# for m, em in evr.metrics.items():
# assert isinstance(em, EvaluatedMetric)
# assert isinstance(m, tuple)
# if m:
# M = "/".join(m)
# else:
# M = k
# stats[M] = float(em.total)
# per_episode[episode_name] = stats
if length_s >= config.min_episode_length_s:
logger.info('%1.f s are enough' % length_s)
# episodes.pop(0) #@ We dont pop the episode, run in loop
# out_video = os.path.join(dn, 'camera.mp4')
# with notice_thread("Make video", 2):
# make_video1(fn, out_video)
# os.rename(dn, dn_final)
else:
logger.error('episode too short with %1.f s < %.1f s' %
(length_s, config.min_episode_length_s))
nfailures += 1
attempt_i += 1
except dc.InvalidSubmission:
raise
except BaseException as e:
msg = 'Anomalous error while running episodes:'
msg += '\n\n' + indent(traceback.format_exc(), ' > ')
logger.error(msg)
raise dc.InvalidEvaluator(msg) from e
finally:
agent_ci.close()
sim_ci.close()
logger.info('Simulation done.')
def compute_velocities(self, commands):
steering_angle = commands[1] * self.max_steering_angle
linear_velocity = commands[0] * self.max_linear_velocity
angular_velocity = np.tan(steering_angle) * linear_velocity / self.L
vel = se2_from_linear_angular([linear_velocity, 0], angular_velocity)
return vel
def make_scenario(
yaml_str: str,
scenario_name: str,
only_straight: bool,
min_dist: float,
delta_y_m: float,
delta_theta_rad: float,
robots_pcs: List[RobotName],
robots_npcs: List[RobotName],
robots_parked: List[RobotName],
nduckies: int,
duckie_min_dist_from_other_duckie: float,
duckie_min_dist_from_robot: float,
duckie_y_bounds: Sequence[float],
) -> Scenario:
yaml_data = yaml.load(yaml_str, Loader=yaml.SafeLoader)
po = dw.construct_map(yaml_data)
num_pcs = len(robots_pcs)
num_npcs = len(robots_npcs)
num_parked = len(robots_parked)
nrobots = num_npcs + num_pcs + num_parked
poses = sample_many_good_starting_poses(
po,
nrobots,
only_straight=only_straight,
min_dist=min_dist,
delta_theta_rad=delta_theta_rad,
delta_y_m=delta_y_m,
)
poses_pcs = poses[:num_pcs]
poses = poses[num_pcs:]
#
poses_npcs = poses[:num_npcs]
poses = poses[num_npcs:]
#
poses_parked = poses[:num_parked]
poses = poses[num_parked:]
assert len(poses) == 0
COLOR_PLAYABLE = "red"
COLOR_NPC = "blue"
COLOR_PARKED = "grey"
robots = {}
for i, robot_name in enumerate(robots_pcs):
pose = poses_pcs[i]
vel = g.se2_from_linear_angular([0, 0], 0)
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(
description=f"Playable robot {robot_name}",
controllable=True,
configuration=configuration,
# motion=None,
color=COLOR_PLAYABLE,
protocol=PROTOCOL_NORMAL,
)
for i, robot_name in enumerate(robots_npcs):
pose = poses_npcs[i]
vel = g.se2_from_linear_angular([0, 0], 0)
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(
description=f"NPC robot {robot_name}",
controllable=True,
configuration=configuration,
# motion=MOTION_MOVING,
color=COLOR_NPC,
protocol=PROTOCOL_FULL,
)
for i, robot_name in enumerate(robots_parked):
pose = poses_parked[i]
vel = g.se2_from_linear_angular([0, 0], 0)
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(
description=f"Parked robot {robot_name}",
controllable=False,
configuration=configuration,
# motion=MOTION_PARKED,
color=COLOR_PARKED,
protocol=None,
)
# logger.info(duckie_y_bounds=duckie_y_bounds)
names = [f"duckie{i:02d}" for i in range(nduckies)]
poses = sample_duckies_poses(
po,
nduckies,
robot_positions=poses,
min_dist_from_other_duckie=duckie_min_dist_from_other_duckie,
min_dist_from_robot=duckie_min_dist_from_robot,
from_side_bounds=(duckie_y_bounds[0], duckie_y_bounds[1]),
delta_theta_rad=np.pi,
)
d = [ScenarioDuckieSpec("yellow", _) for _ in poses]
duckies = dict(zip(names, d))
ms = Scenario(
scenario_name=scenario_name,
environment=yaml_str,
robots=robots,
duckies=duckies,
player_robots=list(robots_pcs),
)
return ms
def test_pwm1():
parameters = get_DB18_nominal(delay=0)
# initial configuration
init_pose = np.array([0, 0.8])
init_vel = np.array([0, 0])
q0 = geo.SE2_from_R2(init_pose)
v0 = geo.se2_from_linear_angular(init_vel, 0)
tries = {
"straight_50": (PWMCommands(+0.5, 0.5)),
"straight_max": (PWMCommands(+1.0, +1.0)),
"straight_over_max": (PWMCommands(+1.5, +1.5)),
"pure_left": (PWMCommands(motor_left=-0.5, motor_right=+0.5)),
"pure_right": (PWMCommands(motor_left=+0.5, motor_right=-0.5)),
"slight-forward-left": (PWMCommands(motor_left=0, motor_right=0.25)),
"faster-forward-right": (PWMCommands(motor_left=0.5, motor_right=0)),
# 'slight-right': (PWMCommands(-0.1, 0.1)),
}
dt = 0.03
t_max = 10
map_data_yaml = """
tiles:
- [floor/W,floor/W, floor/W, floor/W, floor/W]
- [straight/W , straight/W , straight/W, straight/W, straight/W]
- [floor/W,floor/W, floor/W, floor/W, floor/W]
tile_size: 0.61
"""
map_data = yaml.load(map_data_yaml)
root = construct_map(map_data)
timeseries = {}
for id_try, commands in tries.items():
seq = integrate_dynamics(parameters, q0, v0, dt, t_max, commands)
ground_truth = seq.transform_values(
lambda t: SE2Transform.from_SE2(t[0]))
poses = seq.transform_values(lambda t: t[0])
velocities = get_velocities_from_sequence(poses)
linear = velocities.transform_values(linear_from_se2)
angular = velocities.transform_values(angular_from_se2)
# print(linear.values)
# print(angular.values)
root.set_object(id_try, DB18(), ground_truth=ground_truth)
sequences = {}
sequences["motor_left"] = seq.transform_values(
lambda _: commands.motor_left)
sequences["motor_right"] = seq.transform_values(
lambda _: commands.motor_right)
plots = TimeseriesPlot(f"{id_try} - PWM commands", "pwm_commands",
sequences)
timeseries[f"{id_try} - commands"] = plots
sequences = {}
sequences["linear_velocity"] = linear
sequences["angular_velocity"] = angular
plots = TimeseriesPlot(f"{id_try} - Velocities", "velocities",
sequences)
timeseries[f"{id_try} - velocities"] = plots
outdir = os.path.join(get_comptests_output_dir(), "together")
draw_static(root, outdir, timeseries=timeseries)
def compute_velocities(self, commands):
linear = [self.linear_velocity, 0]
angular = float(commands[0]) * self.max_angular_velocity
vel = se2_from_linear_angular(linear, float(angular))
return vel
def vel_from_friendly(p: FriendlyVelocity) -> se2value:
return se2_from_linear_angular([p.x, p.y], np.deg2rad(p.theta_deg))
def compute_velocities(self, commands):
linear = [commands[0] * self.max_linear_velocity[0],
commands[1] * self.max_linear_velocity[1]]
angular = commands[2] * self.max_angular_velocity
vel = se2_from_linear_angular(linear, angular)
return vel
def compute_velocities(self, commands):
linear = [self.linear_velocity, 0]
angular = float(commands[0]) * self.max_angular_velocity
vel = se2_from_linear_angular(linear, float(angular))
return vel
def compute_forces(self, commands):
linear = [commands[0] * self.max_force[0],
commands[1] * self.max_force[1]]
angular = commands[2] * self.max_force[2]
vel = se2_from_linear_angular(linear, angular)
return vel
def debug_get_vel_from_commands(self, commands):
vx = commands[0] * self.max_lin_vel
vy = commands[1] * self.max_lin_vel
w = 0
se2 = se2_from_linear_angular([vx, vy], w)
return se3_from_se2(se2)
def compute_velocities(self, commands):
linear = np.array([commands[1] * self.max_linear_velocity, 0])
angular = commands[0] * self.max_angular_velocity
vel = se2_from_linear_angular(linear, angular)
return vel
def make_scenario(
yaml_str: str,
scenario_name: str,
only_straight: bool,
min_dist: float,
delta_y_m: float,
delta_theta_rad: float,
robots_pcs: List[RobotName],
robots_npcs: List[RobotName],
robots_parked: List[RobotName],
nduckies: int,
duckie_min_dist_from_other_duckie: float,
duckie_min_dist_from_robot: float,
tree_density: float,
tree_min_dist: float,
duckie_y_bounds: Sequence[float],
pc_robot_protocol: ProtocolDesc = PROTOCOL_NORMAL,
npc_robot_protocol: ProtocolDesc = PROTOCOL_FULL,
) -> Scenario:
yaml_data = yaml.load(yaml_str, Loader=yaml.SafeLoader)
if "objects" not in yaml_data:
yaml_data["objects"] = {}
objects = yaml_data["objects"]
if isinstance(objects, list):
raise ZValueError(yaml_data=yaml_data)
po = dw.construct_map(yaml_data)
num_pcs = len(robots_pcs)
num_npcs = len(robots_npcs)
num_parked = len(robots_parked)
nrobots = num_npcs + num_pcs + num_parked
all_robot_poses = sample_many_good_starting_poses(
po,
nrobots,
only_straight=only_straight,
min_dist=min_dist,
delta_theta_rad=delta_theta_rad,
delta_y_m=delta_y_m,
)
remaining_robot_poses = list(all_robot_poses)
poses_pcs = remaining_robot_poses[:num_pcs]
remaining_robot_poses = remaining_robot_poses[num_pcs:]
#
poses_npcs = remaining_robot_poses[:num_npcs]
remaining_robot_poses = remaining_robot_poses[num_npcs:]
#
poses_parked = remaining_robot_poses[:num_parked]
remaining_robot_poses = remaining_robot_poses[num_parked:]
assert len(remaining_robot_poses) == 0
COLORS_PLAYABLE = ["red", "green", "blue"]
COLOR_NPC = "blue"
COLOR_PARKED = "grey"
robots = {}
for i, robot_name in enumerate(robots_pcs):
pose = poses_pcs[i]
vel = g.se2_from_linear_angular([0, 0], 0)
configuration = RobotConfiguration(pose=pose, velocity=vel)
color = COLORS_PLAYABLE[i % len(COLORS_PLAYABLE)]
robots[robot_name] = ScenarioRobotSpec(
description=f"Playable robot {robot_name}",
controllable=True,
configuration=configuration,
# motion=None,
color=color,
protocol=pc_robot_protocol,
)
for i, robot_name in enumerate(robots_npcs):
pose = poses_npcs[i]
vel = g.se2_from_linear_angular([0, 0], 0)
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(
description=f"NPC robot {robot_name}",
controllable=True,
configuration=configuration,
color=COLOR_NPC,
protocol=npc_robot_protocol,
)
for i, robot_name in enumerate(robots_parked):
pose = poses_parked[i]
vel = g.se2_from_linear_angular([0, 0], 0)
configuration = RobotConfiguration(pose=pose, velocity=vel)
robots[robot_name] = ScenarioRobotSpec(
description=f"Parked robot {robot_name}",
controllable=False,
configuration=configuration,
# motion=MOTION_PARKED,
color=COLOR_PARKED,
protocol=None,
)
# logger.info(duckie_y_bounds=duckie_y_bounds)
names = [f"duckie{i:02d}" for i in range(nduckies)]
poses = sample_duckies_poses(
po,
nduckies,
robot_positions=all_robot_poses,
min_dist_from_other_duckie=duckie_min_dist_from_other_duckie,
min_dist_from_robot=duckie_min_dist_from_robot,
from_side_bounds=(duckie_y_bounds[0], duckie_y_bounds[1]),
delta_theta_rad=np.pi,
)
d = [ScenarioDuckieSpec("yellow", _) for _ in poses]
duckies = dict(zip(names, d))
trees = sample_trees(po, tree_density, tree_min_dist)
for i, pose_tree in enumerate(trees):
obname = f"tree_random_{i}"
st = dw.SE2Transform.from_SE2(pose_tree)
objects[obname] = {"kind": "tree", "pose": st.as_json_dict(), "height": random.uniform(0.15, 0.4)}
add_signs(po, objects)
yaml_str = yaml.dump(yaml_data)
# logger.info(trees=trees)
ms = Scenario(
scenario_name=scenario_name,
environment=yaml_str,
robots=robots,
duckies=duckies,
player_robots=list(robots_pcs),
)
return ms
def compute_velocities(self, commands):
linear = np.array([commands[1] * self.max_linear_velocity, 0])
angular = commands[0] * self.max_angular_velocity
vel = se2_from_linear_angular(linear, angular)
return vel
def compute_velocities(self, commands):
steering_angle = commands[1] * self.max_steering_angle
linear_velocity = commands[0] * self.max_linear_velocity
angular_velocity = np.tan(steering_angle) * linear_velocity / self.L
vel = se2_from_linear_angular([linear_velocity, 0], angular_velocity)
return vel