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_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