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_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 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 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_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 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_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 _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_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 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) 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_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_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_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 on_received_get_commands(self, context: Context): if self.n == 0: pwm_left = 0.0 pwm_right = 0.0 else: pwm_left = np.random.uniform(0.5, 1.0) pwm_right = np.random.uniform(0.5, 1.0) self.n += 1 # pwm_left = 1.0 # pwm_right = 1.0 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): linear, angular = self.compute_action(self.to_predictor) # ! Inverse Kinematics pwm_left, pwm_right = self.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(1.0, 0.0, 0.0) blue = RGB(0.0, 0.0, 1.0) led_commands = LEDSCommands(red, grey, blue, red, blue) # ! Send PWM Command pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = DB20Commands(pwm_commands, led_commands) context.write('commands', commands)
def on_received_next_image(self, context: Context): if self.state.nimages >= self.config.images_per_episode: context.write("no_more_images", None) return H, W = self.config.shape values = (128 + np.random.randn(H, W, 3) * 60).astype("uint8") jpg_data = bgr2jpg(values) image = JPGImage(jpg_data) delta = 0.15 t = self.state.nimages * delta time = timestamp_from_seconds(t) ts = TimeSpec(time=time, frame=self.state.episode_name, clock=context.get_hostname()) acquired = {"image": ts} timing = TimingInfo(acquired=acquired) context.write(topic="image", data=image, timing=timing)
def on_received_get_commands(self, context: Context, data: GetCommands): 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. while not self.agent.updated: time.sleep(0.01) 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 = Duckiebot1Commands(pwm_commands, led_commands) context.write("commands", commands)
def on_received_get_commands(self, context: Context): """ Converts requested linear/angular velocities to control commands and issues them to the robot. Don't change this, standard stuff from the submission template. """ linear, angular = self.compute_action(self.to_predictor) # Inverse kinematics pwm_left, pwm_right = self.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 grey = RGB(0.0, 0.0, 0.0) red = RGB(1.0, 0.0, 0.0) blue = RGB(0.0, 0.0, 1.0) led_commands = LEDSCommands(red, grey, blue, red, blue) # Send PWM command pwm_commands = PWMCommands(motor_left=pwm_left, motor_right=pwm_right) commands = DB20Commands(pwm_commands, led_commands) context.write('commands', commands)
def on_received_next_scenario(self, context: Context): if self.state.scenarios_to_go: scenario = self.state.scenarios_to_go.pop(0) context.write('scenario', scenario) else: context.write('finished', None)
def on_received_get_robot_state(self, context: Context, data: RobotName): context.info(f"get_robot_state({data!r})") rs = RobotState(robot_name=data, t_effective=self.current_time, state=None) context.write("robot_state", rs)
def on_received_dump_state(self, context: Context): context.write('dump_state', StateDump(None))
def on_received_episode_start(self, context: Context, data: EpisodeStart): context.write("episode_start", data=data)
def on_received_get_robot_interface_description(self, context: Context, data: RobotName): rid = RobotInterfaceDescription(robot_name=data, observations=JPGImage, commands=PWMCommands) context.write('robot_interface_description', rid)
def on_received_get_robot_interface_description(self, context: Context, data: RobotName): rid = RobotInterfaceDescription(robot_name=data, observations=Duckiebot1Observations, commands=Duckiebot1Commands) context.write("robot_interface_description", rid)
def on_received_query(self, context: Context, data: CollisionCheckQuery): collided = check_collision( Wcoll=self.params.environment, robot_body=self.params.body, robot_pose=data.pose ) result = CollisionCheckResult(collided) context.write("response", result)
def on_received_image(self, context: Context, data: JPGImage): transformed = data context.write("image", transformed)