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