def __init__(self, sea): # turbines 35,000 hp (26 MW), 1 auxiliary motor 325 hp (242 kW) # Ship.__init__(self, self.DRAG_FACTOR, self.MAX_TURN_RATE_HOUR, self.MAX_ACCELERATION) self.kind = '688' self.messages = [] self.sea = sea self.max_hull_integrity = 100 # in "damage points" self.hull_integrity = self.max_hull_integrity self.damages = None self.actual_deep = 60 # feet self._target_deep = 60 # feet self.message_stop = False ### Physics ### self._acceleration = Point(0, 0) self.drag_force = Point(0, 0) self._position = Point(0, 0) self._velocity = Point(0, 0) self._target_velocity = 0 self._rudder = 0 # property rudder in radians pe minute self._ship_bearing = 0 # the angle of the ship in relation to north, in radians self._turbine_level = 0 # 0 to 100% self.turbine_acceleration = Point(0, 0) self.time_elapsed = 0 # self.turbine = Turbine(self, self.MAX_ACCELERATION) self.nav = Navigation(self) self.sonar = Sonar(self)
def get_edge_coords(self): return [[ Point(0, 0), Point(self.dimensions.width, 0), ], [ Point(0, self.dimensions.height), Point(self.dimensions.width, self.dimensions.height), ]]
def get_edge_coords(self): return [[ Point(self.coords.x, self.coords.y), Point(self.coords.x + self.size.x, self.coords.y), ], [ Point(self.coords.x, self.coords.y + self.size.y), Point(self.coords.x + self.size.x, self.coords.y + self.size.y), ]]
def draw_unit_on_map(self, unit, current_map): unit_pixels = unit.get_pixels() unit_coords = unit.coords.get_int_coords() for row in range(unit.size.y): for col in range(unit.size.x): if unit_pixels[row][col]: pixel_coords = Point(unit_coords.x + col, unit_coords.y - row) if pixel_coords.is_inside(self.get_edge_coords()): current_map[unit_coords.y - row][unit_coords.x + col] = unit_pixels[row][col]
def _get_is_swing_leg_left_and_walk_trajectory_four_positions__with_current_working_set(self, index): current_walking_set = self.walking_sets[index] is_swing_leg_left = current_walking_set['is_swing_leg_left'] step_length = current_walking_set['step_length'] current_position_relative_to_initial_foot_position = current_walking_set['current_position_relative_to_initial_foot_position'] initial_foot_position = Point(get_initial_foot_position(is_left=is_swing_leg_left)) initial_position = initial_foot_position.translate(current_position_relative_to_initial_foot_position.x, current_position_relative_to_initial_foot_position.y, current_position_relative_to_initial_foot_position.z) lift_up_front_position, lift_up_front_forward_position, put_down_position\ = self._lower_limp_kinematics_engine.get_walk_trajectory_three_positions(is_swing_leg_left=is_swing_leg_left, step_length=step_length, current_position_relative_to_initial_foot_position=current_position_relative_to_initial_foot_position) return is_swing_leg_left, initial_position, lift_up_front_position, lift_up_front_forward_position, put_down_position
def walk(self): self.draw_natural_pose() # self.draw_some_points() is_swing_leg_left = True initial_position = Point(get_initial_foot_position(is_left=is_swing_leg_left)) lift_up_front_position, lift_up_front_forward_position, put_down_position = self.get_walk_trajectory_three_positions(is_swing_leg_left=True, step_length=self.step_length / 2, current_position_relative_to_initial_foot_position=Point((0, 0, 0))) lift_up_front_trajectory_section = LineSegment(initial_position, lift_up_front_position) lift_up_front_forward_trajectory_section = LineSegment(lift_up_front_position, lift_up_front_forward_position) put_down_trajectory_section = LineSegment(lift_up_front_forward_position, put_down_position) print('initial_position', initial_position) print('lift_up_front_position', lift_up_front_position) print('lift_up_front_forward_position', lift_up_front_forward_position) print('put_down_position', put_down_position) self.painter.draw_point(initial_position, color='r.') self.painter.draw_point(lift_up_front_position, color='r.') self.painter.draw_point(lift_up_front_forward_position, color='r.') self.painter.draw_point(put_down_position, color='r.') self.painter.draw_line(initial_position, lift_up_front_position, color='b-') self.painter.draw_line(lift_up_front_position, lift_up_front_forward_position, color='b-') self.painter.draw_line(lift_up_front_forward_position, put_down_position, color='b-') self._take_one_step(is_swing_leg_left, lift_up_front_trajectory_section, lift_up_front_forward_trajectory_section, put_down_trajectory_section) self.painter.show()
def get_drawing_coords(self): drawing_coords = [] for x in range(self.size.x): for y in range(self.size.y): drawing_coords.append( Point(self.coords.x + x, self.coords.y + y))
def parse_input(input_list): ast_list = [] for y, row in enumerate(input_list): for x, char in enumerate(row): if char == "#": ast_list.append(Point(x, y)) return ast_list
def a_star(connections, target): curr = Point(0, 0) open_set = {curr} prior = {} g_score = defaultdict(lambda: 10**6, {curr: 0}) f_score = {curr: man_dist(curr, target)} while open_set: last = curr open_set.remove(curr := min(open_set, key=lambda n: f_score[n])) g_score[curr] = g_score[last] + 1 if curr == target: for p1, p2 in prior.items(): assert p2 - p1 in r_directs path = [curr] while curr in prior: curr = prior[curr] path.append(curr) return len(path) - 1 else: for neighb in connections[curr]: new_g = g_score[curr] + 1 if g_score[neighb] > new_g: assert curr - neighb in r_directs prior[neighb] = curr g_score[neighb] = new_g f_score[neighb] = new_g + man_dist(neighb, target) open_set.add(neighb) return False
def _get_target_with_hip_transversal_counteracted(self): move_back_to_origin = get_translate_matrix(-self.hip_top_position.x, -self.hip_top_position.y, -self.hip_top_position.z) rotate_along_z_axis = get_rotate_matrix_along_axis('z', self.hip_transversal_radians) move_back_to_point = get_translate_matrix(self.hip_top_position.x, self.hip_top_position.y, self.hip_top_position.z) target_at_ankle__with_hip_transversal_counteracted = apply_matrix_to_vertex(self.target_at_ankle, move_back_to_point.dot(rotate_along_z_axis.dot(move_back_to_origin))) return Point(target_at_ankle__with_hip_transversal_counteracted)
def _get_support_lower_limp_angles(self, is_support_leg_left, swing_leg_foot_position): opposite_ground_position = WalkTrajectory.project_to_opposite_ground( Point(swing_leg_foot_position), is_left=is_support_leg_left) support_lower_limp_angles = self._get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect( opposite_ground_position, is_left=is_support_leg_left) return support_lower_limp_angles
def move_head(target_position, painter): target_position = Point(target_position) painter.draw_point(target_position) head_bottom_position = Point((0, 0, torso_height + neck_length)) head_top_position = Point((0, 0, torso_height + neck_length + head_height)) distance_between_head_bottom_and_target = head_bottom_position.distance_to(target_position) assert distance_between_head_bottom_and_target == head_height distance_between_head_top_and_target = head_top_position.distance_to(target_position) neck_lateral_radians = get_angle_with_3_sides_known(distance_between_head_top_and_target, head_height, head_height) def get_neck_transversal_radians(target_x, target_y): if target_x != 0: radiance = atan(target_y / target_x) else: # perpendicular if target_y > 0: radiance = half_pi elif target_y < 0: radiance = -half_pi else: radiance = 0 return radiance neck_transversal_radians = get_neck_transversal_radians(target_x=target_position[0], target_y=target_position[1]) return neck_transversal_radians, neck_lateral_radians
def get_left_leg_target_position(): # return get_leg_position((l_hip_transversal_degrees, 0, -0, -0), is_left=True) initial_foot_position = Point(initial_left_foot_position) up = initial_foot_position.translate(dz=2*foot_height) _front_up = up.translate(dx=2*foot_height) _back_up = up.translate(dx=-2*foot_height) up_out = up.translate(dy=2*foot_height) out_down = up_out.translate(dz=-foot_height) front_out_up = up_out.translate(dx=2*foot_height) _back_out_up = up_out.translate(dx=-2*foot_height) front_out_down = out_down.translate(dx=2*foot_height) _back_out_down = out_down.translate(dx=-2*foot_height) for p in [initial_foot_position, up, _front_up, _back_up, up_out, out_down, front_out_up, _back_out_up, front_out_down, _back_out_down]: painter.draw_point(p) return _back_up
def run_world_cycle(self, elapsed_time): if self.runner.collide_with(self.cactus): self.game_has_ended = True return self.runner.move(elapsed_time) self.runner.accelerate(settings.gravity, elapsed_time) self.cactus.move(elapsed_time) if self.runner.in_default_position(self.dimensions): self.runner.velocity.y = 0 self.runner.coords.y = self.dimensions.height / 2 if self.cactus.coords.x < 0 - self.cactus.size.x: self.cactus.accelerate(Point(-1, 0), 1) self.cactus.size = Point(random.randint(1, 3), random.randint(1, 4)) self.cactus.coords.x = self.dimensions.width
def parse_coordinates(self, text): try: coord = text.split(',') if len(coord) != 2: return None x = float(coord[0]) y = float(coord[1]) return Point(x, y) except ValueError: return None
def test_mov_vel_contant2(self): m1 = Submarine688() m1._velocity = Point(1, 2) m1.turn(10) self.assertAlmostEqual(m1.position.x, 10) self.assertAlmostEqual(m1.position.y, 20) m1.turn(5) m1.turn(5) self.assertAlmostEqual(m1.position.x, 20) self.assertAlmostEqual(m1.position.y, 40)
def __init__(self, draw_gui=False, intrusion=None): """ Ctor """ object.__init__(self) self._has_gui = draw_gui self._gui_size = 0 self._frame_count = 0 self._gui_output = None # Initialise background (500 x 500) self._background = [[ Rgb(DEFAULT_BG_R, DEFAULT_BG_G, DEFAULT_BG_B) for _ in range(0, 500) ] for _ in range(0, 500)] self._turtles = {} self._id_counter = 0 self._update_interval = None rospy.set_param("background_r", DEFAULT_BG_R) rospy.set_param("background_g", DEFAULT_BG_G) rospy.set_param("background_b", DEFAULT_BG_B) rospy.loginfo("Starting turtle frame with parent node %s", rospy.get_name()) trt_x = random.randrange(0, self.get_width()) trt_y = random.randrange(0, self.get_height()) self._spawn_turtle(trt_x, trt_y) # Colouring the background # Window is 500 x 500, starting bottom left at 0,0 and ending top right at 499,499 # Top left: Pastel purple self._draw_area(Rgb.pastel_purple(), Point(0, 250), Point(249, 499)) # Top right: Pastel yellow self._draw_area(Rgb.pastel_yellow(), Point(250, 250), Point(499, 499)) # Bottom left: Pastel green self._draw_area(Rgb.pastel_green(), Point(0, 0), Point(249, 249)) # Bottom right: Pastel blue self._draw_area(Rgb.pastel_blue(), Point(250, 0), Point(499, 249)) # Intrusion zone (middle): Red self._draw_red(intrusion) # Initialise GUI (if requested) self._redraw() # Initialise update timer (16 msec) self._update_interval = rospy.Duration(0.016) rospy.Timer(self._update_interval, self._update_turtles)
def get_right_leg_target_position(): # return get_leg_position((r_hip_transversal_degrees, RightHipFrontal().outward(20).degrees, 0, 0), is_left=False) initial_foot_position = Point(initial_right_foot_position) up = initial_foot_position.translate(dz=2*foot_height) _front_up = up.translate(dx=2*foot_height) _back_up = up.translate(dx=-2*foot_height) up_out = up.translate(dy=-2*foot_height) out_down = up_out.translate(dz=-foot_height) front_out_up = up_out.translate(dx=2*foot_height) _back_out_up = up_out.translate(dx=-2*foot_height) front_out_down = out_down.translate(dx=2*foot_height) _back_out_down = out_down.translate(dx=-2*foot_height) for p in [initial_foot_position, up, _front_up, _back_up, up_out, out_down, front_out_up, _back_out_up, front_out_down, _back_out_down]: painter.draw_point(p) # right... return _back_up
def _draw_red(self, intrusion): """ Draw a red area in the center based on the intrusion level. """ if intrusion is None: return if intrusion not in self.POSSIBLE_INTRUSION_LEVELS: raise ValueError( "Given value [{}] for argument \"intrusion\" is invalid". format(intrusion)) from_point = Point(0, 0) to_point = Point(0, 0) colour = Rgb() assert (len(self.POSSIBLE_INTRUSION_LEVELS) == 3) # Easy: 40 % strong_red / 316 * 316 if intrusion == self.POSSIBLE_INTRUSION_LEVELS[0]: from_point = Point(92, 92) to_point = Point(407, 407) colour = Rgb.strong_red() # Medium: 20 % med_red / 224 * 224 elif intrusion == self.POSSIBLE_INTRUSION_LEVELS[1]: from_point = Point(138, 138) to_point = Point(361, 361) colour = Rgb.med_red() # Hard: 5 % light_red / 112 * 112 elif intrusion == self.POSSIBLE_INTRUSION_LEVELS[2]: from_point = Point(194, 194) to_point = Point(305, 305) colour = Rgb.light_red() else: raise NotImplementedError( "draw_red: Intrusion level not implemented") # TODO TEMP Currently intruded means ALL is red! from_point = Point(0, 0) to_point = Point(499, 499) self._draw_area(colour, from_point, to_point)
def __init__(self, target_position, robot_model, painter, is_left): self.robot_model = robot_model self.painter = painter self.target_position = Point(target_position) self.is_left = is_left self.outer_shoulder_position = left_outer_shoulder_position if self.is_left else right_outer_shoulder_position self.draw_arm = self.robot_model.draw_left_arm if self.is_left else self.robot_model.draw_right_arm self.is_target_above_body = self.target_position.x > 0
def _get_hip_lateral(self, hip_frontal_radians, knee_lateral_radians): if self.is_left: current_ankle = Point(self.robot_model.draw_left_lower_limp(left_hip_frontal_radians=hip_frontal_radians, left_knee_lateral_radians=knee_lateral_radians, draw=False)[-2].vertex) else: current_ankle = Point(self.robot_model.draw_right_lower_limp(right_hip_frontal_radians=hip_frontal_radians, right_knee_lateral_radians=knee_lateral_radians, draw=False)[-2].vertex) opposite_side_length = current_ankle.distance_to(self.target_at_ankle__with_hip_transversal_counteracted) if is_float_equal(opposite_side_length, 0): return 0 target_at_ankle_with_hip_transversal_counteracted__radius = self.hip_bottom_position.distance_to(self.target_at_ankle__with_hip_transversal_counteracted) current_ankle_radius = self.hip_bottom_position.distance_to(current_ankle) assert is_float_equal(target_at_ankle_with_hip_transversal_counteracted__radius, current_ankle_radius), '{} {}'.format(target_at_ankle_with_hip_transversal_counteracted__radius, current_ankle_radius) assert opposite_side_length < current_ankle_radius + target_at_ankle_with_hip_transversal_counteracted__radius hip_lateral_radians = get_angle_with_3_sides_known(opposite_side_length, current_ankle_radius, target_at_ankle_with_hip_transversal_counteracted__radius) if self.is_left: return hip_lateral_radians else: return -hip_lateral_radians
def get_left_leg_target_position(): # return get_leg_position((l_hip_transversal_degrees, 0, -0, -0), is_left=True) initial_foot_position = Point(initial_left_foot_position) up = initial_foot_position.translate(dz=2 * foot_height) _front_up = up.translate(dx=2 * foot_height) _back_up = up.translate(dx=-2 * foot_height) up_out = up.translate(dy=2 * foot_height) out_down = up_out.translate(dz=-foot_height) front_out_up = up_out.translate(dx=2 * foot_height) _back_out_up = up_out.translate(dx=-2 * foot_height) front_out_down = out_down.translate(dx=2 * foot_height) _back_out_down = out_down.translate(dx=-2 * foot_height) for p in [ initial_foot_position, up, _front_up, _back_up, up_out, out_down, front_out_up, _back_out_up, front_out_down, _back_out_down ]: painter.draw_point(p) return _back_up
def walk(self): self.draw_some_points() # self.draw_robot(*self.get_squat_with_tilted_torso_lower_limp_angles()) lower_limps_angle_set_for_lift_up_front_position, lower_limps_angle_set_for_lift_up_front_forward_position, lower_limps_angle_set_for_put_down_position\ = self.get_feet_positions_set__with__both_tilted_torso_and_hip_offset_effect(is_swing_leg_left=True, step_length=self.step_length/2, current_position_relative_to_initial_foot_position=Point((0, 0, 0))) self.draw_robot(*lower_limps_angle_set_for_lift_up_front_position) self.draw_robot( *lower_limps_angle_set_for_lift_up_front_forward_position) self.draw_robot(*lower_limps_angle_set_for_put_down_position) # lower_limps_angle_set_for_lift_up_front_position, lower_limps_angle_set_for_lift_up_front_forward_position, lower_limps_angle_set_for_put_down_position\ # = self.get_lower_limps_angles_set__with__both_tilted_torso_and_hip_offset_effect(is_swing_leg_left=False, step_length=self.step_length, current_position_relative_to_initial_foot_position=Point((-self.step_length/2, 0, 0))) # self.draw_robot(*lower_limps_angle_set_for_lift_up_front_position) # self.draw_robot(*lower_limps_angle_set_for_lift_up_front_forward_position) # self.draw_robot(*lower_limps_angle_set_for_put_down_position) is_swing_leg_left = True initial_position = Point( get_initial_foot_position(is_left=is_swing_leg_left)) lift_up_front_position, lift_up_front_forward_position, put_down_position = self.get_walk_trajectory_three_positions( is_swing_leg_left=True, step_length=self.step_length / 2, current_position_relative_to_initial_foot_position=Point( (0, 0, 0))) self.painter.draw_line(initial_position, lift_up_front_position, color='b-') self.painter.draw_line(lift_up_front_position, lift_up_front_forward_position, color='b-') self.painter.draw_line(lift_up_front_forward_position, put_down_position, color='b-') self.painter.show()
def _spawn_turtle(self, trt_x, trt_y, name=None): """ Add a turtle to the field at the given coordinates. """ if name is None or name == "": name = self._create_unique_turtle_name() elif self._has_turtle(name): return "" turtle = Turtle(name, Point(trt_x, trt_y)) self._turtles[name] = turtle rospy.loginfo("New turtle [%s] at x=[%d], y=[%d]", name, trt_x, trt_y) return name
def turn(self, time_elapsed): self.time_elapsed = time_elapsed turbine_acceleration = self.MAX_ACCELERATION * self.turbine_level turbine_acceleration_x = math.cos(self._ship_bearing) * turbine_acceleration turbine_acceleration_y = math.sin(self._ship_bearing) * turbine_acceleration self.turbine_acceleration = Point(turbine_acceleration_x, turbine_acceleration_y) if self.rudder != 0: self.rotate(-1.0 * self.rudder * time_elapsed * self.speed) # diff is the difference between the angle the sub is moving and the angle of the ship is bearing # meaning the ship the turning left or right diff = self.course - self.ship_bearing # correction if the drag factor since the sub is making a turn self.drag_factor = self.DRAG_FACTOR * (1 + abs(500 * math.sin(diff))) # drag force total_drag = self.drag_factor * (self.speed ** 2) vel_angle = self._velocity.angle drag_x = math.cos(vel_angle) * total_drag drag_y = math.sin(vel_angle) * total_drag self.drag_force = Point(drag_x, drag_y) self._acceleration = self.turbine_acceleration - self.drag_force self._velocity += self._acceleration * time_elapsed self._position += self._velocity * time_elapsed # self.speed = self.speed - ( self.drag_acceleration() * time_elapsed) # deep deep_diff = self.target_deep - self.actual_deep if abs(deep_diff) > 0.1: dive_rate = min(deep_diff, self.MAX_DEEP_RATE_FEET) self.actual_deep += dive_rate * time_elapsed * 3600
def assertLowerLimpAnglesEqual(self, leg_angles, is_left=True): """ Given shoulder and elbow angles A for arm, calculate the end points P, and call inverse kinematics (IK) with P to see if IK returns A exactly. :param arm_angles: :return: """ print('\n'*6 + 'aAE', leg_angles) lower_limp_angles = to_radians(leg_angles) + (0, 0) draw_lower_limp = self.r.draw_left_lower_limp if is_left else self.r.draw_right_lower_limp expected_hip_transversal, expected_hip_frontal, expected_hip_lateral, \ expected_knee_lateral, \ expected_ankle_lateral, expected_ankle_frontal, \ expected_foot_center \ = draw_lower_limp(*lower_limp_angles, color='r-') expected_ankle = Point(expected_ankle_frontal.vertex) target = translate_3d_point(expected_ankle, dz=-foot_height) hip_transversal_radians = lower_limp_angles[0] hip_frontal_radians, hip_lateral_radians, knee_lateral_radians, ankle_lateral_radians, ankle_frontal_radians \ = InverseKinematicsLeg(hip_transversal_radians, target, self.r, self.painter, is_left).get_angles() actual_hip_transversal, actual_hip_frontal, actual_hip_lateral, \ actual_knee_lateral, \ actual_ankle_lateral, actual_ankle_frontal, \ actual_foot_center \ = draw_lower_limp(hip_transversal_radians, hip_frontal_radians, hip_lateral_radians, knee_lateral_radians, ankle_lateral_radians, ankle_frontal_radians, draw=False) actual_ankle =Point(actual_ankle_frontal.vertex) actual_foot_center = Point(actual_foot_center) self.assertTrue(actual_ankle.float_equals(expected_ankle), [actual_ankle, expected_ankle]) self.assertTrue(actual_ankle.translate(dz=-foot_height).float_equals(actual_foot_center))
def __str__(self): min_y = max_y = min_x = max_x = 0 for p in self.painted.keys(): min_y = min(min_y, p.y) min_x = min(min_x, p.x) max_y = max(max_y, p.y) max_x = max(max_x, p.x) s_list = [] for y in range(max_y, min_y - 1, -1): s_sublist = [] for x in range(min_x, max_x + 1): color = self.painted.get(Point(x, y), 0) s_sublist.append("▮" if color else " ") s_list.append("".join(s_sublist)) return "\n".join(s_list)
def run_suite(simulations=100): MAX_ROUNDS = 50 name_a = 'Orc' name_b = 'Troll' matches = [] # Run matches for i in range(simulations): orc = Creature(name_a, position=Point(0, 0)) troll = Creature(name_b, position=Point(0, 0)) cs = CombatSimulation(MAX_ROUNDS) result = cs.run(orc, troll) matches.append({'winner': result[0], 'rounds': result[1]}) # Aggregate results a_wins = 0 b_wins = 0 a_rounds = 0 b_rounds = 0 draws = 0 for match in matches: if match.get('winner', False): if match['winner'].name == name_a: a_wins += 1 a_rounds += match['rounds'] elif match['winner'].name == name_b: b_wins += 1 b_rounds += match['rounds'] else: draws += 1 print 'SIMULATION RESULTS:\n{} won {} times\n{} won {} times\n{} draws\n'.format(name_a, a_wins, name_b, b_wins, draws)
def get_right_leg_target_position(): # return get_leg_position((r_hip_transversal_degrees, RightHipFrontal().outward(20).degrees, 0, 0), is_left=False) initial_foot_position = Point(initial_right_foot_position) up = initial_foot_position.translate(dz=2 * foot_height) _front_up = up.translate(dx=2 * foot_height) _back_up = up.translate(dx=-2 * foot_height) up_out = up.translate(dy=-2 * foot_height) out_down = up_out.translate(dz=-foot_height) front_out_up = up_out.translate(dx=2 * foot_height) _back_out_up = up_out.translate(dx=-2 * foot_height) front_out_down = out_down.translate(dx=2 * foot_height) _back_out_down = out_down.translate(dx=-2 * foot_height) for p in [ initial_foot_position, up, _front_up, _back_up, up_out, out_down, front_out_up, _back_out_up, front_out_down, _back_out_down ]: painter.draw_point(p) # right... return _back_up
def update(self, dtime, background, canvas_width, canvas_height): """ Update the turtle state and position. canvas_width: Expected to be max index of the x side canvas_height: Like canvas_width, but for y """ old_pos = Point.copy(self.pos) # Movement commands are only valid for one second if (rospy.Time.now() - self._last_command_time > rospy.Duration(1.0)): self._x_vel = 0.0 self._y_vel = 0.0 self._pos_f.x += self._x_vel * dtime self._pos_f.y += self._y_vel * dtime self.pos = PointF.to_point(self._pos_f) # Clamp to screen size if (self.pos.x < 0 or self.pos.x > canvas_width or self.pos.y < 0 or self.pos.y > canvas_height): rospy.logdebug("I hit the wall! (Clamping from [x=%f, y=%f])", self.pos.x, self.pos.y) self.pos.update(x=min(max(self.pos.x, 0), canvas_width - 1), y=min(max(self.pos.y, 0), canvas_height - 1)) # Publish pose of the turtle pose = Pose() pose.x = self.pos.x pose.y = self.pos.y self._pose_pub.publish(pose) # Figure out (and publish) the color underneath the turtle colour = Color() rgb = background[self.pos.x][self.pos.y] colour.r = rgb.r colour.g = rgb.g colour.b = rgb.b self._colour_pub.publish(colour) rospy.logdebug("[%s]: pos_x: %f pos_y: %f", rospy.get_namespace(), self.pos.x, self.pos.y) return self.pos != old_pos
def connect_systems(rng, stars): routes = set() destinations = stars.keys() possible_routes = defaultdict(set) for _a, _b, _c in computeDelaunayTriangulation([Point(*point) for point in destinations]): a = destinations[_a] b = destinations[_b] c = destinations[_c] possible_routes[a].add(b) possible_routes[a].add(c) possible_routes[b].add(c) possible_routes[b].add(a) possible_routes[c].add(a) possible_routes[c].add(b) visited_systems = {rng.choice(destinations)} destinations = set(destinations) while visited_systems != destinations: plausible_routes = set() for system in visited_systems: for destination in possible_routes[system]: if destination not in visited_systems: plausible_routes.add((system, destination)) sorted_routes = sorted(list(plausible_routes), key=lambda (s, e): route_length(s, e), reverse=True) routes.add(sorted_routes.pop()) for _ in range(rng.randint(min(1, len(sorted_routes)), max(min(1, len(sorted_routes)), len(sorted_routes) / 3))): routes.add(sorted_routes.pop()) visited_systems = systems_visited(routes) for start, end in routes: stars[start].routes.add(stars[end]) stars[end].routes.add(stars[start])
def __init__(self, is_left, hip_transversal_radians, target_position, robot_model, painter): self.robot_model = robot_model self.painter = painter self.is_left = is_left if self.DEBUG: print('is_left', is_left) if self.is_left: self.hip_top_position = Point(left_hip_top_position) self.hip_bottom_position = Point(left_hip_bottom_position) else: self.hip_top_position = Point(right_hip_top_position) self.hip_bottom_position = Point(right_hip_bottom_position) if self.DEBUG: print('hip top & bottom\n\t', self.hip_top_position, '\n\t', self.hip_bottom_position) self.hip_transversal_radians = hip_transversal_radians self.target_position = Point(target_position) self.target_at_ankle = Point(translate_3d_point(target_position, dz=foot_height)) if self.DEBUG: print('target & at ankle\n\t', self.target_position, '\n\t', self.target_at_ankle) # counteract hip transversal self.target_at_ankle__with_hip_transversal_counteracted = self._get_target_with_hip_transversal_counteracted() if self.DEBUG: print('target_at_ankle__with_hip_transversal_counteracted\n\t', self.target_at_ankle__with_hip_transversal_counteracted)
class LineSegment: def __init__(self, initial, end): self.initial = Point(initial) self.end = Point(end) self._mid_point = None self._length = None def get_point_at(self, fraction): x = self.initial.x + (self.end.x - self.initial.x) * fraction y = self.initial.y + (self.end.y - self.initial.y) * fraction z = self.initial.z + (self.end.z - self.initial.z) * fraction return Point((x, y, z)) def has_point(self, p): if self.initial.x <= p.x <= self.end.x\ and self.initial.y <= p.y <= self.end.y\ and self.initial.z <= p.x <= self.end.x: return True else: return False @property def mid_point(self): if self._mid_point is None: self._mid_point = self.get_point_at(0.5) return self._mid_point @property def length(self): if self._length is None: self._length = self.initial.distance_to(self.end) return self._length
def __init__(self): # turbines 35,000 hp (26 MW), 1 auxiliary motor 325 hp (242 kW) # Ship.__init__(self, self.DRAG_FACTOR, self.MAX_TURN_RATE_HOUR, self.MAX_ACCELERATION) self.towed_array = sonar.TD16DTowedArray() self.kind = '688' self.messages = [] self.max_hull_integrity = 100 # in "damage points" self.hull_integrity = self.max_hull_integrity self.damages = None self._actual_deep = 60 # feet self._target_deep = 60 # feet ### Physics ### self._position = Point(0, 0) self._velocity = Point(0, 0) self._acceleration = Point(0, 0) self._target_speed = 0 self._target_turbine = 0 self.speed_mode = self.SPEED_MODE_SPEED self._rudder = TimedValue(0,0, self.RUDDER_CHANGE_SPEED) # property rudder in radians per minute self._ship_course = 0 # the angle of the ship in radians self._turbine_level = TimedValue(0, 0, self.TURBINE_CHANGE_SPEED) # 0 to 100% self.total_drag_acceleration = 0 self.drag_acceleration = Point(0, 0) self.acceleration_needed = 0 self.turbine_level_needed = 0 self.turbine_acceleration = 0 self.turbine_acceleration_x = 0 self.turbine_acceleration_y = 0 self.time_elapsed = 0 self.nav_mode = self.NAV_MODE_MANUAL self._destination = Point(0, 0) self.spherical_array = sonar.ANBQQ5SphericalArray() self.hull_array = sonar.ANBQQ5HullArray() self.towed_array = sonar.TD16DTowedArray()
def __init__(self, initial, end): self.initial = Point(initial) self.end = Point(end) self._mid_point = None self._length = None
class Submarine688(object): MAX_TURN_RATE_HOUR = math.radians(120) * 60 # max 120 degrees per minute, 360 in 3 minutes MAX_DEEP_RATE_FEET = 1 # 1 foot per second MAX_SPEED = 36.0 # Knots or nautical mile per hour MAX_ACCELERATION = 1.0 * 3600 # acceleration in knots/hour^2 -> max acceleration 0.1 Knots / second^2 DRAG_FACTOR = 1.0 * MAX_ACCELERATION / (MAX_SPEED ** 2) RUDDER_CHANGE_SPEED = 25.0 * 3600 # 20 degrees per second TURBINE_CHANGE_SPEED = 25.0 * 3600 # 20% per second NAV_MODE_MANUAL = 'manual' NAV_MODE_DESTINATION = 'destination' SPEED_MODE_SPEED = 'target speed' SPEED_MODE_TURBINE = 'target turbine' def __init__(self): # turbines 35,000 hp (26 MW), 1 auxiliary motor 325 hp (242 kW) # Ship.__init__(self, self.DRAG_FACTOR, self.MAX_TURN_RATE_HOUR, self.MAX_ACCELERATION) self.towed_array = sonar.TD16DTowedArray() self.kind = '688' self.messages = [] self.max_hull_integrity = 100 # in "damage points" self.hull_integrity = self.max_hull_integrity self.damages = None self._actual_deep = 60 # feet self._target_deep = 60 # feet ### Physics ### self._position = Point(0, 0) self._velocity = Point(0, 0) self._acceleration = Point(0, 0) self._target_speed = 0 self._target_turbine = 0 self.speed_mode = self.SPEED_MODE_SPEED self._rudder = TimedValue(0,0, self.RUDDER_CHANGE_SPEED) # property rudder in radians per minute self._ship_course = 0 # the angle of the ship in radians self._turbine_level = TimedValue(0, 0, self.TURBINE_CHANGE_SPEED) # 0 to 100% self.total_drag_acceleration = 0 self.drag_acceleration = Point(0, 0) self.acceleration_needed = 0 self.turbine_level_needed = 0 self.turbine_acceleration = 0 self.turbine_acceleration_x = 0 self.turbine_acceleration_y = 0 self.time_elapsed = 0 self.nav_mode = self.NAV_MODE_MANUAL self._destination = Point(0, 0) self.spherical_array = sonar.ANBQQ5SphericalArray() self.hull_array = sonar.ANBQQ5HullArray() self.towed_array = sonar.TD16DTowedArray() # self.towed_array_cable = TimedValue(0,0, self.towed_array.TOWED_ARRAY_DEPLOY_SPEED) ### TURBINE ### def get_turbine_level(self): return self._turbine_level.current_value def set_turbine_level(self, new_level): self._turbine_level.target_value = limits(new_level, -100, 100) turbine_level = property(get_turbine_level, set_turbine_level, None, "level of power in the turbine (-100% to +100%)") ### DEEP ### def get_actual_deep(self): return self._actual_deep; actual_deep = property(get_actual_deep, None, None, "Target Deep") def set_periscope_deep(self): self.target_deep = 60 def get_target_deep(self): return self._target_deep def set_target_deep(self, value): self._target_deep = limits(value, 0, 9000) target_deep = property(get_target_deep, set_target_deep, None, "Target Deep") ### Position ### def _get_position(self): return self._position def _set_position(self, value): self._position = value position = property(_get_position, _set_position, None, "Position") ### Speed ### def get_target_speed(self): return self._target_speed def set_target_speed(self, value): self._target_speed = limits(value, 0, self.MAX_SPEED) target_speed = property(get_target_speed, set_target_speed, None, "Target Speed") def get_actual_speed(self): return self._velocity.length def _set_actual_speed(self, value): self._velocity.length = limits(value, 0, self.MAX_SPEED) actual_speed = property(get_actual_speed, None, None, "Actual Speed") def all_stop(self): self.set_target_speed(0) ### Acceleration ### def _get_acceleration(self): return self._acceleration.length def _set_acceleration(self, value): self._acceleration.length = value acceleration = property(_get_acceleration, _set_acceleration, None, "Acceleration") ### Drag ### def get_drag_acceleration(self): return self.drag_force ### NAVIGATION ### def get_rubber_bearing(self): angle_deg = -1 * math.degrees(self.get_rudder()) return (90 - angle_deg) % 360 def rudder_right(self): self.rudder = self.MAX_TURN_RATE_HOUR def rudder_left(self): self.rudder = -self.MAX_TURN_RATE_HOUR def rudder_center(self): self.rudder = 0 def get_rudder(self): return self._rudder.current_value def set_rudder(self, angle): angle = limits(angle, -self.MAX_TURN_RATE_HOUR, self.MAX_TURN_RATE_HOUR) self._rudder.target_value = angle rudder = property(get_rudder, set_rudder, "Rudder") # in radians per hour def get_ship_course(self): return self._velocity.get_angle() def _set_ship_course(self, angle): self._ship_course = normalize_angle_2pi(angle) course = property(get_ship_course, _set_ship_course, "Ship Course") def get_destination(self): return self._destination def set_destination(self, destination): self._destination = destination # self.nav_mode = self.NAV_MODE_DESTINATION destination = property(get_destination, set_destination, "Ship Destination") ### Message ### # "stop" means the turn just stop because requeres pilot atention def add_message(self, module, msg, stop=False): self.messages.append("{0}: {1}".format(module, msg)) self.message_stop = self.message_stop or stop def get_messages(self): return self.messages, self.message_stop ### Sonar ### def deploy_tower_array(self): self.towed_array_cable.target_value = self.towed_array.TOTAL_LENGTH def retrive_tower_array(self): self.towed_array_cable.target_value = 0 def stop_tower_array(self): self.towed_array_cable.target_value = self.towed_array_cable.current_value def is_cavitating(self): """ Assumes the noise is proportional to speed Cavitation: Cavitation occurs when the propeller is spinning so fast water bubbles at the blades' edges. If you want to go faster, go deeper first. Water pressure at deeper depth reduces/eliminates cavitation. If you have the improved propeller upgrade, you can go about 25% faster without causing cavitation. Rule of thumb: number of feet down, divide by 10, subtract 1, is the fastest speed you can go without cavitation. For example, at 150 feet, you can go 14 knots without causing cavitation. 150/10 = 15, 15-1 = 14. You can get the exact chart at the Marauders' website. (url's at the end of the document) # cavitation doesn't occur with spd < 7 max_speed_for_deep = max((self.actual_deep / 10) - 1, 7) cavitating = max_speed_for_deep < self.speed if cavitating and not self.cavitation: self.add_message("SONAR", "COMM, SONAR: CAVITATING !!!", True) self.cavitation = cavitating :return: sound in in decibels """ max_speed_for_deep = max((self.actual_deep / 10) - 1, 7) return max_speed_for_deep < self.actual_speed def clear_messages(self): self.messages = [] self.message_stop = False # non-liner noise: # for 0 < speed < 15 : linear from 40 to 60 # for speed > 15 : linear with factor of 2db for each knot # ref: http://fas.org/man/dod-101/navy/docs/es310/uw_acous/uw_acous.htm NOISE_RANGE1 = linear_scaler([0, 15], [40, 60]) NOISE_RANGE2 = linear_scaler([15, 35], [60, 100]) # def self_noise(self, freq): # returns # # if self.speed <= 15: # # noise = self.NOISE_RANGE1(self.speed) # # else: # # noise = self.NOISE_RANGE2(self.speed) # # # cavitation doesn't occur with spd < 7 # # max_speed_for_deep = max((self.actual_deep / 10) - 1, 7) # # cavitating = max_speed_for_deep < self.speed # # return noise + (30 if self.cavitation else 0) + random.gauss(0, 0.4) # # ''' # Above 10-20 kts, flow noise becomes the dominant factor and significantly increases # with speed (@1.5-2 dB/KT) # ''' # # # return noise + (30 if cavitating else 0) + random.gauss(0, 0.4) ACUSTIC_PROFILE = { 2: 112.0, 4: 108.0, 5: 115.0, 8: 110.0 } def get_self_noise(self): # returns """ """ # s = Sound() # # s.add_frequencs(self.ACUSTIC_PROFILE) # # base_sound_level = 100 # db - "Very quite submarine" # s.add_logdecay(base_sound_level, 1, base_sound_level, 100) # s.add_logdecay(base_sound_level, 100, base_sound_level - 20, 1000) # # # # machine noise # # 5 - 400Hz, proportional to turbine level # turbine_noise = 75 + math.log10((abs(self.turbine.get_level())+0.01)) * 40 # # s.add_logdecay(turbine_noise, 1, turbine_noise - 10, 400) # # # flow noise # flow_nose = (abs(self.get_speed()) * 6.0) # # # s.add_logdecay(noise_30hz,5,noise_30hz,30) # 120db @ 30Hz - 100db @ 300 db # s.add_logdecay(flow_nose, 100, flow_nose-20, 1000) # 120db @ 30Hz - 100db @ 300 db # # return s.add_noise(0.5) return 100 # def get_sea_noise(self): # returns # return self.sea.get_sea_noise(self.actual_deep) # if self.speed <= 15: # noise = self.NOISE_RANGE1(self.speed) # else: # noise = self.NOISE_RANGE2(self.speed) # logfreq = math.log10(freq) # if self.is_cavitating(): # base = [0] # else: # base = [150] # # # if freq <= 100: # base.append(noise) # # if freq > 100: # base.append(noise - 20 * math.log10(freq/100)) # return sum_of_decibels(base) + random.gauss(0, 1) def turn(self, time_elapsed, messages): self.time_elapsed = time_elapsed # update timed values self._rudder.update(time_elapsed) self._turbine_level.update(time_elapsed) self.towed_array.update(time_elapsed) # ship_moviment_angle is the angle the ship is moving ship_moviment_angle = self._velocity.get_angle() # ship_rotation_angle is the angle the ship pointing ship_course_angle = self._ship_course # drifting_angle_diff is the difference between the angle the sub is moving and the angle of the ship is bearing # meaning the ship the turning left or right drifting_angle_diff = normalize_angle_2pi(ship_moviment_angle - ship_course_angle) self.turbine_acceleration = self.MAX_ACCELERATION * self.turbine_level / 100 self.turbine_acceleration_x = math.cos(ship_course_angle) * self.turbine_acceleration self.turbine_acceleration_y = math.sin(ship_course_angle) * self.turbine_acceleration self.turbine_acceleration = Point(self.turbine_acceleration_x, self.turbine_acceleration_y) if self.rudder != 0: # rotate the ship # the ship rotates proportional to it's speed angle_to_rotate = (self.rudder * time_elapsed) * (self.actual_speed / self.MAX_SPEED) new_angle = ship_course_angle + angle_to_rotate self._ship_course = normalize_angle_pi(new_angle) # correction if the drag factor since the sub is making a turn self.drag_factor = self.DRAG_FACTOR * (1 + abs(300 * math.sin(drifting_angle_diff))) # speed if self.speed_mode == self.SPEED_MODE_SPEED: if self.actual_speed != self.target_speed: self.acceleration_needed = self.drag_factor * (self.target_speed**2) self.turbine_level_needed = 100.0 * self.acceleration_needed / self.MAX_ACCELERATION # adjust turbines diff_speed = self.target_speed - self.actual_speed diff_turbine = self.turbine_level - self.turbine_level_needed # diff*10 gives more burst to make the change in speed faster self.turbine_level = self.turbine_level_needed + limits(diff_speed, -1, 1) * 10 elif self.speed_mode == self.SPEED_MODE_TURBINE: # fixed turbine level self.acceleration_needed = 0 self.turbine_level_needed = self._target_turbine self.turbine_level = self._target_turbine # drag force self.total_drag_acceleration = -1.0 * self.drag_factor * ((self.actual_speed) ** 2) drag_x = math.cos(ship_moviment_angle) * self.total_drag_acceleration drag_y = math.sin(ship_moviment_angle) * self.total_drag_acceleration self.drag_acceleration = Point(drag_x, drag_y) self._acceleration = self.turbine_acceleration + self.drag_acceleration # convert seconds to hours self._velocity += self._acceleration * time_elapsed self._position += self._velocity * time_elapsed # deep deep_diff = self.target_deep - self.actual_deep if abs(deep_diff) > 0.1: dive_rate = min(deep_diff, self.MAX_DEEP_RATE_FEET) self.actual_deep += dive_rate * time_elapsed * 3600 if self.nav_mode == self.NAV_MODE_DESTINATION: self.angle_to_destination = self.position.get_angle_to(self.destination) self.angle_difference = normalize_angle_pi(self.angle_to_destination - self.course) self.rudder = self.angle_difference * 500.0 # TODO: when the sub reaches the destination, it should stop def __str__(self): return "Sub: {status} deep:{deep:.0f}({sdeep})".format(status='', deep=self.actual_deep, sdeep=self.target_deep)
class InverseKinematicsLeg(): DEBUG = False def __init__(self, is_left, hip_transversal_radians, target_position, robot_model, painter): self.robot_model = robot_model self.painter = painter self.is_left = is_left if self.DEBUG: print('is_left', is_left) if self.is_left: self.hip_top_position = Point(left_hip_top_position) self.hip_bottom_position = Point(left_hip_bottom_position) else: self.hip_top_position = Point(right_hip_top_position) self.hip_bottom_position = Point(right_hip_bottom_position) if self.DEBUG: print('hip top & bottom\n\t', self.hip_top_position, '\n\t', self.hip_bottom_position) self.hip_transversal_radians = hip_transversal_radians self.target_position = Point(target_position) self.target_at_ankle = Point(translate_3d_point(target_position, dz=foot_height)) if self.DEBUG: print('target & at ankle\n\t', self.target_position, '\n\t', self.target_at_ankle) # counteract hip transversal self.target_at_ankle__with_hip_transversal_counteracted = self._get_target_with_hip_transversal_counteracted() if self.DEBUG: print('target_at_ankle__with_hip_transversal_counteracted\n\t', self.target_at_ankle__with_hip_transversal_counteracted) def get_angles(self): if self.DEBUG: self.painter.draw_point(self.target_position, color='y.') if self.DEBUG: self.painter.draw_point(self.target_at_ankle, color='y.') knee_lateral_radians, knee_interior_radians, knee_exterior_radians = self._get_knee_lateral() if self.DEBUG: print('knee_lateral_radians', degrees(knee_lateral_radians)) hip_frontal_radians = self._get_hip_frontal() if self.DEBUG: print('hip_frontal_radians', degrees(hip_frontal_radians)) hip_lateral_radians = - self._get_hip_lateral(hip_frontal_radians, knee_lateral_radians) if self.DEBUG: print('hip_lateral_radians', degrees(hip_lateral_radians)) ankle_lateral_radians = -(abs(hip_lateral_radians) - knee_exterior_radians) if not self.is_left: ankle_lateral_radians *= -1 if self.DEBUG: print('ankle_lateral_radians', ankle_lateral_radians) ankle_frontal_radians = hip_frontal_radians if self.DEBUG: print('ankle_frontal_radians', ankle_frontal_radians) if self.DEBUG: print('fd (hip_transversal, hip_frontal, hip_lateral, knee_lateral)') if self.DEBUG: print(' ', to_degrees((self.hip_transversal_radians, hip_frontal_radians, hip_lateral_radians, knee_lateral_radians))) return hip_frontal_radians, hip_lateral_radians, knee_lateral_radians, ankle_lateral_radians, ankle_frontal_radians def _get_target_with_hip_transversal_counteracted(self): move_back_to_origin = get_translate_matrix(-self.hip_top_position.x, -self.hip_top_position.y, -self.hip_top_position.z) rotate_along_z_axis = get_rotate_matrix_along_axis('z', self.hip_transversal_radians) move_back_to_point = get_translate_matrix(self.hip_top_position.x, self.hip_top_position.y, self.hip_top_position.z) target_at_ankle__with_hip_transversal_counteracted = apply_matrix_to_vertex(self.target_at_ankle, move_back_to_point.dot(rotate_along_z_axis.dot(move_back_to_origin))) return Point(target_at_ankle__with_hip_transversal_counteracted) def _get_knee_lateral(self): d1 = self.hip_bottom_position.distance_to(self.target_at_ankle__with_hip_transversal_counteracted) if self.DEBUG: print('d1', d1, self.hip_bottom_position, self.target_at_ankle__with_hip_transversal_counteracted) assert is_float_equal(d1, leg_length) or d1 < leg_length, '{} {} {}'.format(d1, leg_length, d1-leg_length) if is_float_equal(d1, leg_length): knee_interior_radians = pi else: knee_interior_radians = get_angle_with_3_sides_known(d1, upper_leg_length, lower_leg_length) knee_exterior_radians = pi - knee_interior_radians if self.DEBUG: print('knee interior & exterior angles', degrees(knee_interior_radians), degrees(knee_exterior_radians)) if self.is_left: knee_lateral_radians = knee_exterior_radians else: knee_lateral_radians = -knee_exterior_radians return knee_lateral_radians, knee_interior_radians, knee_exterior_radians def _get_hip_frontal(self): target_at_ankle_with_hip_transversal_counteracted__y_offset_to_hip_bottom = abs(self.target_at_ankle__with_hip_transversal_counteracted.y - self.hip_bottom_position.y) target_at_ankle_with_hip_transversal_counteracted__z_offset_to_hip_bottom = abs(self.target_at_ankle__with_hip_transversal_counteracted.z - self.hip_bottom_position.z) hip_frontal_radians = atan( target_at_ankle_with_hip_transversal_counteracted__y_offset_to_hip_bottom / target_at_ankle_with_hip_transversal_counteracted__z_offset_to_hip_bottom) if self.is_left: return -hip_frontal_radians else: return hip_frontal_radians def _get_hip_lateral(self, hip_frontal_radians, knee_lateral_radians): if self.is_left: current_ankle = Point(self.robot_model.draw_left_lower_limp(left_hip_frontal_radians=hip_frontal_radians, left_knee_lateral_radians=knee_lateral_radians, draw=False)[-2].vertex) else: current_ankle = Point(self.robot_model.draw_right_lower_limp(right_hip_frontal_radians=hip_frontal_radians, right_knee_lateral_radians=knee_lateral_radians, draw=False)[-2].vertex) opposite_side_length = current_ankle.distance_to(self.target_at_ankle__with_hip_transversal_counteracted) if is_float_equal(opposite_side_length, 0): return 0 target_at_ankle_with_hip_transversal_counteracted__radius = self.hip_bottom_position.distance_to(self.target_at_ankle__with_hip_transversal_counteracted) current_ankle_radius = self.hip_bottom_position.distance_to(current_ankle) assert is_float_equal(target_at_ankle_with_hip_transversal_counteracted__radius, current_ankle_radius), '{} {}'.format(target_at_ankle_with_hip_transversal_counteracted__radius, current_ankle_radius) assert opposite_side_length < current_ankle_radius + target_at_ankle_with_hip_transversal_counteracted__radius hip_lateral_radians = get_angle_with_3_sides_known(opposite_side_length, current_ankle_radius, target_at_ankle_with_hip_transversal_counteracted__radius) if self.is_left: return hip_lateral_radians else: return -hip_lateral_radians