def on_received_set_params(self, context: Context, data: PlanningSetup): context.info("initialized") self.params = data # This is the interval of allowed linear velocity # Note that min_velocity_x_m_s and max_velocity_x_m_s might be different. # Note that min_velocity_x_m_s may be 0 in advanced exercises (cannot go backward) max_velocity_x_m_s: float = self.params.max_linear_velocity_m_s min_velocity_x_m_s: float = self.params.min_linear_velocity_m_s # This is the max curvature. In earlier exercises, this is +inf: you can turn in place. # In advanced exercises, this is less than infinity: you cannot turn in place. max_curvature: float = self.params.max_curvature # these have the same meaning as the collision exercises body: List[PlacedPrimitive] = self.params.body environment: List[PlacedPrimitive] = self.params.environment # these are the final tolerances - the precision at which you need to arrive at the goal tolerance_theta_deg: float = self.params.tolerance_theta_deg tolerance_xy_m: float = self.params.tolerance_xy_m # For convenience, this is the rectangle that contains all the available environment, # so you don't need to compute it bounds: Rectangle = self.params.bounds
def on_received_get_commands(self, context: Context, data: GetCommands): self.n += 1 # behavior = 0 # random trajectory behavior = 1 # primary motions if behavior == 0: pwm_left = np.random.uniform(0.5, 1.0) pwm_right = np.random.uniform(0.5, 1.0) col = RGB(0.0, 1.0, 1.0) elif behavior == 1: t = data.at_time d = 1.0 phases = [ (+1, -1, RGB(1.0, 0.0, 0.0)), (-1, +1, RGB(0.0, 1.0, 0.0)), (+1, +1, RGB(0.0, 0.0, 1.0)), (-1, -1, RGB(1.0, 1.0, 0.0)), ] phase = int(t / d) % len(phases) pwm_right, pwm_left, col = phases[phase] else: raise ValueError(behavior) led_commands = LEDSCommands(col, col, col, col, col) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = DB20Commands(pwm_commands, led_commands) context.write("commands", commands)
def on_received_get_commands(self, context: Context, data: GetCommands): if self.n == 0: pwm_left = 0.0 pwm_right = 0.0 else: pwm_left = 0.1 pwm_right = 0.1 self.n += 1 t = data.at_time d = 1.0 phase = int(t / d) % 4 # if phase == 0: # pwm_right = +1 # pwm_left = -1 # elif phase == 1: # pwm_right = +1 # pwm_left = +1 # elif phase == 2: # pwm_right = -1 # pwm_left = +1 # elif phase == 3: # pwm_right = -1 # pwm_left = -1 # pwm_left = 1.0 # pwm_right = 1.0 col = RGB(0.0, 0.0, 1.0) led_commands = LEDSCommands(col, col, col, col, col) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = DB20Commands(pwm_commands, led_commands) context.write("commands", commands)
def init(self, context: Context): context.info("Check GPU...") self.check_gpu_available(context) # Model predicts linear and angular velocity but we need to issue # wheel velocity commands to robot, this wrapper converts to the latter. self.convertion_wrapper = SteeringToWheelVelWrapper() context.info('init()') model = Modelv1() model.load_state_dict( torch.load( "modelv1_maserati_bill_simulated_amadobot_base_epoch_200.pth", # "modelv1_maserati_bill_simulated_amadobot_base.pth", # 'modelv1_maserati_plus_simulated.pth', map_location=self.device, )) model.eval() self.model = model.to(self.device) self.current_image = np.zeros(self.expect_shape) self.input_image = np.zeros((150, 200, 3)) self.to_predictor = np.expand_dims(self.input_image, axis=0)
def update_observations(self, blur_time: float, context: Context): context.info(f'update_observations() at {self.current_time}') assert self.render_observations to_average = [] n = len(self.render_observations) # context.info(str(self.render_timestamps)) # context.info(f'now {self.current_time}') for i in range(n): ti = self.render_timestamps[i] if math.fabs(self.current_time - ti) < blur_time: to_average.append(self.render_observations[i]) obs0 = to_average[0].astype('float32') context.info(str(obs0.shape)) for obs in to_average[1:]: obs0 += obs obs = obs0 / len(to_average) obs = obs.astype('uint8') # context.info(f'update {obs.shape} {obs.dtype}') jpg_data = rgb2jpg(obs) camera = JPGImage(jpg_data) obs = Duckiebot1Observations(camera) self.ro = DB18RobotObservations(self.robot_name, self.current_time, obs) self.last_observations_time = self.current_time
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 on_received_get_sim_state(self, context: Context): d = self.env._compute_done_reward() done = d.done done_why = d.done_why done_code = d.done_code sim_state = SimulationState(done, done_why, done_code) context.write('sim_state', sim_state)
def on_received_get_commands(self, context: Context): linear, angular = self.compute_action( self.to_predictor) # * Changed to custom size #0.6 1.5 linear = linear angular = angular #! Inverse Kinematics pwm_left, pwm_right = convertion_wrapper.convert(linear, angular) pwm_left = float(np.clip(pwm_left, -1, +1)) pwm_right = float(np.clip(pwm_right, -1, +1)) #! LED Commands Sherrif Duck grey = RGB(0.0, 0.0, 0.0) red = RGB(255.0, 0.0, 0.0) blue = RGB(0.0, 0.0, 255.0) led_commands = LEDSCommands(red, grey, blue, red, blue) # if (self.led_counter < 30): # led_commands = LEDSCommands(grey, red, blue, red, blue) # self.led_counter += 1 # elif (self.led_counter >= 60): # self.led_counter = 0 # led_commands = LEDSCommands(grey, red, blue, red, blue) # elif(self.led_counter > 30): # led_commands = LEDSCommands(blue, red, grey, blue, red) # self.led_counter += 1 #! Do not modify here! pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = Duckiebot1Commands(pwm_commands, led_commands) context.write('commands', commands)
def init(self, context: Context): context.info("Check GPU...") self.check_gpu_available(context) # Model predicts linear and angular velocity but we need to issue # wheel velocity commands to robot, this wrapper converts to the latter. self.convertion_wrapper = SteeringToWheelVelWrapper() context.info('init()') model_lin = NvidiaModel() model_lin.load_state_dict( torch.load( 'saved_nvidia_model_lin.pth', map_location=self.device, )) model_lin.eval() self.model_lin = model_lin.to(self.device) model_ang = NvidiaModel() model_ang.load_state_dict( torch.load( 'saved_nvidia_model_ang.pth', map_location=self.device, )) model_ang.eval() self.model_ang = model_ang.to(self.device) self.current_image = np.zeros(self.expect_shape) self.input_image = np.zeros((150, 200, 3)) self.to_predictor = np.expand_dims(self.input_image, axis=0)
def check_tensorflow_gpu(self, context: Context): import tensorflow as tf name = tf.test.gpu_device_name() context.info(f"gpu_device_name: {name!r} ") if not name: # None or '' no_hardware_GPU_available(context)
def on_received_get_commands(self, context: Context): pwm_left, pwm_right = self.compute_action(self.current_image) grey = RGB(0.0, 0.0, 0.0) led_commands = LEDSCommands(grey, grey, grey, grey, grey) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = Duckiebot1Commands(pwm_commands, led_commands) context.write('commands', commands)
def init(self, context: Context): print(time.time(), 'init') context.info('init()') frozen_model_filename = "frozen_graph.pb" # We use our "load_graph" function to load the graph self.graph = load_graph(frozen_model_filename) self.session = tf.Session(graph=self.graph)
def init(self, context: Context): context.info("init()") self.rgb = None self.l_max = -math.inf self.r_max = -math.inf self.l_min = math.inf self.r_min = math.inf self.left = None self.right = None
def on_received_get_commands(self, context: Context): pwm_left, pwm_right = self.compute_action(self.current_image) pwm_left = float(np.clip(pwm_left, -1, +1)) pwm_right = float(np.clip(pwm_right, -1, +1)) grey = RGB(0.0, 0.0, 0.0) led_commands = LEDSCommands(grey, grey, grey, grey, grey) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = DB20Commands(pwm_commands, led_commands) context.write('commands', commands)
def on_received_observations(self, context: Context, data: DB20ObservationsWithTimestamp): profiler = context.get_profiler() camera: JPGImageWithTimestamp = data.camera odometry: DB20OdometryWithTimestamp = data.odometry context.info(f"camera timestamp: {camera.timestamp}") context.info(f"odometry timestamp: {odometry.timestamp}") with profiler.prof("jpg2rgb"): _rgb = jpg2rgb(camera.jpg_data)
def _start_episode(self, context: Context): if self.state.episode >= self.config.num_episodes: context.write("no_more_episodes", None) return self.state.episode += 1 self.state.nimages = 0 self.state.episode_name = f"episode{self.state.episode}" context.write("episode_start", EpisodeStart(self.state.episode_name))
def on_received_set_robot_commands(self, data: DB18SetRobotCommands, context: Context): l, r = data.commands.wheels.motor_left, data.commands.wheels.motor_right if max(math.fabs(l), math.fabs(r)) > 1: msg = f'Received invalid PWM commands. They should be between -1 and +1.' \ f' Received left = {l!r}, right = {r!r}.' context.error(msg) raise Exception(msg) self.last_commands = data.commands
def on_received_get_commands(self, context: Context): velocity, omega = self.compute_action(self.current_image) # multiplying steering angle by a gain omega *= 7 pwm_left, pwm_right = self.wrapper.convert(velocity, omega) grey = RGB(0.0, 0.0, 0.0) led_commands = LEDSCommands(grey, grey, grey, grey, grey) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = DB20Commands(pwm_commands, led_commands) context.write('commands', commands)
def on_received_step(self, context: Context, data: Step): delta_time = data.until - self.current_time if delta_time > 0: self.update_physics_and_observations(until=data.until, context=context) else: context.warning(f'Already at time {data.until}') d = self.env._compute_done_reward() self.reward_cumulative += d.reward * delta_time self.current_time = data.until
def on_received_get_robot_observations(self, context: Context): # timing information t = timestamp_from_seconds(self.last_observations_time) ts = TimeSpec(time=t, frame=self.episode_name, clock=context.get_hostname()) timing = TimingInfo(acquired={'image': ts}) context.write('robot_observations', self.ro, with_schema=True, timing=timing)
def on_received_get_commands(self, context: Context): l, u = self.config.pwm_left_interval pwm_left = np.random.uniform(l, u) l, u = self.config.pwm_right_interval pwm_right = np.random.uniform(l, u) grey = RGB(0.0, 0.0, 0.0) led_commands = LEDSCommands(grey, grey, grey, grey, grey) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = Duckiebot1Commands(pwm_commands, led_commands) context.write('commands', commands)
def on_received_get_commands(self, context: Context, data: GetCommands): pwm_left, pwm_right = self.compute_commands() col = RGB(0.0, 0.0, 1.0) col_left = RGB(pwm_left, pwm_left, 0.0) col_right = RGB(pwm_right, pwm_right, 0.0) led_commands = LEDSCommands(col, col_left, col_right, col_left, col_right) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = DB20Commands(pwm_commands, led_commands) context.write("commands", commands)
def init(self, context: Context): context.info('init()') self.image_processor = DTPytorchWrapper() self.action_processor = ActionWrapper(FakeWrap()) from model import DDPG self.check_gpu_available(context) self.model = DDPG(state_dim=self.image_processor.shape, action_dim=2, max_action=1, net_type="cnn") self.current_image = np.zeros((640, 480, 3)) self.model.load("model", directory="./models")
def on_received_get_commands(self, context: Context): l, u = self.config.pwm_left_interval # pwm_left = np.random.uniform(l, u) l, u = self.config.pwm_right_interval # pwm_right = np.random.uniform(l, u) pwm_left, pwm_right = self.compute_action(self.config.current_image) grey = RGB(0.0, 0.0, 0.0) led_commands = LEDSCommands(grey, grey, grey, grey, grey) pwm_commands = PWMCommands(motor_left=float(pwm_left), motor_right=float(pwm_right)) commands = Duckiebot1Commands(pwm_commands, led_commands) context.write('commands', commands)
def on_received_get_commands(self, context: Context, data: GetCommands): # context.info(f'on_received_get_commands') if not self.agent.initialized: pwm_left, pwm_right = [0, 0] else: # TODO: let's use a queue here. Performance suffers otherwise. # What you should do is: *get the last command*, if available # otherwise, wait for one command. t0 = time.time() while not self.agent.updated: dt = time.time() - t0 if dt > 2.0: context.info(f"agent not ready since {dt:.1f} s") time.sleep(0.5) if dt > 60: msg = "I have been waiting for commands from the ROS part" f" since {int(dt)} s" context.error(msg) raise Exception(msg) time.sleep(0.02) dt = time.time() - t0 if dt > 2.0: context.info(f"obtained agent commands after {dt:.1f} s") time.sleep(0.2) pwm_left, pwm_right = self.agent.action self.agent.updated = False grey = RGB(0.5, 0.5, 0.5) led_commands = LEDSCommands(grey, grey, grey, grey, grey) pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = DB20Commands(pwm_commands, led_commands) context.write("commands", commands)
def on_received_get_robot_performance(self, context: Context, data: RobotName): context.info(f"get_robot_interface_description()") metrics = {} metrics["reward"] = Metric( higher_is_better=True, cumulative_value=self.current_time, description="Dummy reward equal to survival time.", ) pm = PerformanceMetrics(metrics) rid = RobotPerformance(robot_name=data, t_effective=self.current_time, performance=pm) context.write("robot_performance", rid)
def on_received_get_robot_performance(self, context: Context, data: RobotName): metrics = {} metrics['survival_time'] = Metric(higher_is_better=True, cumulative_value=self.current_time, description="Survival time.") metrics['reward'] = Metric(higher_is_better=True, cumulative_value=self.reward_cumulative, description="Cumulative reward.") pm = PerformanceMetrics(metrics) rid = RobotPerformance(robot_name=data, t_effective=self.current_time, performance=pm) context.write('robot_performance', rid)
def on_received_query(self, context: Context, data: PlanningQuery): # A planning query is a pair of initial and goal poses start: FriendlyPose = data.start goal: FriendlyPose = data.target # You start at the start pose. You must reach the goal with a tolerance given by # tolerance_xy_m and tolerance_theta_deg. # You need to declare if it is feasible or not feasible = True if not feasible: # If it's not feasible, just return this. result: PlanningResult = PlanningResult(False, None) context.write("response", result) return # If it is feasible you need to provide a plan. # A plan is a list of PlanStep plan: List[PlanStep] = [] # A plan step consists in a duration, a linear and angular velocity. # Let's trace a square of side L at maximum velocity. L = 1.0 duration_straight_m_s = L / self.params.max_linear_velocity_m_s duration_turn_deg_s = 90.0 / self.params.max_angular_velocity_deg_s # The plan will be: straight, turn, straight, turn, straight, turn, straight, turn straight = PlanStep(duration=duration_straight_m_s, angular_velocity_deg_s=0.0, velocity_x_m_s=self.params.max_linear_velocity_m_s) turn = PlanStep( duration=duration_turn_deg_s, angular_velocity_deg_s=self.params.max_angular_velocity_deg_s, velocity_x_m_s=0.0) plan.append(straight) plan.append(turn) plan.append(straight) plan.append(turn) plan.append(straight) plan.append(turn) plan.append(straight) plan.append(turn) result: PlanningResult = PlanningResult(feasible, plan) context.write("response", result)
def init(self, context: Context): context.info("Check GPU...") limit_gpu_memory() test_payload() self.check_tensorflow_gpu() from cbcNet import cbcNet from helperFncs import SteeringToWheelVelWrapper self.convertion_wrapper = SteeringToWheelVelWrapper() context.info('init()') self.model = cbcNet.get_inference("cbcNet.h5") self.current_image = np.zeros(self.expect_shape) self.input_image = np.zeros((150, 200, 3)) self.to_predictor = np.expand_dims(self.input_image, axis=0)
def init(self, context: Context): context.info("Check GPU...") limit_gpu_memory() self.check_tensorflow_gpu() from frankModel import FrankNet from helperFncs import SteeringToWheelVelWrapper self.convertion_wrapper = SteeringToWheelVelWrapper() context.info('init()') self.model = FrankNet.build(200, 150) self.model.load_weights("FrankNet.h5") self.current_image = np.zeros(self.expect_shape) self.input_image = np.zeros((150, 200, 3)) self.to_predictor = np.expand_dims(self.input_image, axis=0)