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 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_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 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 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 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 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 __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 _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 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_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 __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 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 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 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 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 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 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 __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 __init__(self, coords, size):
self.coords = coords
self.size = size
self.velocity = Point(0, 0)
