def shell_will_hit(self, shell, target, factor=1):
"""
Returns True if shell will hit rectangular object
"""
b = shell.angle
e = Vector(1, 0)
q = e.rotate(b)
center = Vector(target.x - shell.x, target.y - shell.y)
if center.scalar_product(q) < 0:
return False
a = target.angle
c1 = center + Vector(target.width / 2 * factor,
target.height / 2 * factor).rotate(a)
c2 = center + Vector(-target.width / 2 * factor,
target.height / 2 * factor).rotate(a)
c3 = center + Vector(-target.width / 2 * factor,
-target.height / 2 * factor).rotate(a)
c4 = center + Vector(target.width / 2 * factor,
-target.height / 2 * factor).rotate(a)
if sign(c1.cross_product(q)) == sign(q.cross_product(c3)):
#print("TEST", c1, c2, c3, c4, q)
return True
if sign(c2.cross_product(q)) == sign(q.cross_product(c4)):
#print("TEST", c1, c2, c3, c4, q)
return True
return False
def will_hit_precise(self, tank, target, ricochet_angle=PI/4, factor=1, side_part=1):
"""
Returns True if tank will hit rectangular object
"""
b = tank.angle + tank.turret_relative_angle
e = Vector(1, 0)
q = e.rotate(b)
tank_v = Vector(tank.x, tank.y)
c = get_unit_corners(target, factor)
hit_v = tank_v + MAX_DISTANCE * q
def will_hit_side(p1, p2):
p_mid = (p1 + p2)/2
p1_new = p_mid + (p1 - p_mid) * side_part
p2_new = p_mid + (p2 - p_mid) * side_part
intersecting = segments_are_intersecting(tank_v, hit_v, p1_new, p2_new)
angle = q.angle(p2 - p1)
safe = ricochet_angle < angle < PI - ricochet_angle
return intersecting and safe
sides = [(c[0], c[1]), (c[1], c[2]), (c[2], c[3]), (c[3], c[0])]
closest_corner = min(c, key=lambda x: x.distance(tank_v))
closer_sides = list(filter(lambda s: s[0] == closest_corner or s[1] == closest_corner, sides))
#result = will_hit_side(c1, c2) or will_hit_side(c2, c3) or will_hit_side(c3, c4) or will_hit_side(c4, c1)
result = will_hit_side(closer_sides[0][0], closer_sides[0][1]) or will_hit_side(closer_sides[1][0], closer_sides[1][1])
return result
def save_target_init_values(self, tank, x, y):
if not self.enabled:
return
dist = tank.get_distance_to(x, y)
vt = Vector(tank.speedX, tank.speedY)
tank_v = Vector(tank.x, tank.y)
pt_v = Vector(x, y)
d = pt_v - tank_v
tank_d_v = Vector(1, 0).rotate(tank.angle)
if vt.is_zero():
vd_angle = 0
else:
vd_angle = vt.angle(d)
if d.is_zero():
self.last_target_time_init_values = (0, 0, 0, 0, 0, 0)
self.last_target_time_init_values = (tank_d_v.angle(d), dist, vd_angle,
vt.length(), tank.angular_speed,
tank.crew_health /
tank.crew_max_health)
def positions(self, tank, world):
positions = []
# Current position
positions.append(Position(tank.x, tank.y, "CURRENT"))
# Forward and back direction
tank_v = Vector(tank.x, tank.y)
tank_d_v = Vector(1, 0).rotate(tank.angle)
forw = tank_v + tank_d_v * self.short_direction
back = tank_v - tank_d_v * self.short_direction
positions.append(Position(forw.x, forw.y, "FORWARD"))
positions.append(Position(back.x, back.y, "BACKWARD"))
fforw = tank_v + tank_d_v * self.long_direction
fback = tank_v - tank_d_v * self.long_direction
positions.append(Position(fforw.x, fforw.y, "FAR FORWARD"))
positions.append(Position(fback.x, fback.y, "FAR BACKWARD"))
# Low diagonal move
DIAGONAL_ANGLE = PI / 6
for a in [
DIAGONAL_ANGLE, -DIAGONAL_ANGLE, PI - DIAGONAL_ANGLE,
DIAGONAL_ANGLE - PI
]:
pt = tank_v + tank_d_v.rotate(a) * 100
positions.append(Position(pt.x, pt.y, "TURN %.0f" % degrees(a)))
# Bonuses positions
positions += [
Position(b.x, b.y, "BONUS %s" % bonus_name_by_type(b.type))
for b in world.bonuses
]
return positions
def shell_will_hit(self, shell, target, factor=1):
"""
Returns True if shell will hit rectangular object
"""
b = shell.angle
e = Vector(1, 0)
q = e.rotate(b)
center = Vector(target.x - shell.x, target.y - shell.y)
if center.scalar_product(q) < 0:
return False
a = target.angle
c1 = center + Vector(target.width/2 * factor, target.height/2 * factor).rotate(a)
c2 = center + Vector(- target.width/2 * factor, target.height/2 * factor).rotate(a)
c3 = center + Vector(- target.width/2 * factor, - target.height/2 * factor).rotate(a)
c4 = center + Vector(target.width/2 * factor, - target.height/2 * factor).rotate(a)
if sign(c1.cross_product(q)) == sign(q.cross_product(c3)):
#print("TEST", c1, c2, c3, c4, q)
return True
if sign(c2.cross_product(q)) == sign(q.cross_product(c4)):
#print("TEST", c1, c2, c3, c4, q)
return True
return False
def positions(self, tank, world):
positions = []
# Current position
positions.append(Position(tank.x, tank.y, "CURRENT"))
# Forward and back direction
tank_v = Vector(tank.x, tank.y)
tank_d_v = Vector(1, 0).rotate(tank.angle)
forw = tank_v + tank_d_v * self.short_direction
back = tank_v - tank_d_v * self.short_direction
positions.append(Position(forw.x, forw.y, "FORWARD"))
positions.append(Position(back.x, back.y, "BACKWARD"))
fforw = tank_v + tank_d_v * self.long_direction
fback = tank_v - tank_d_v * self.long_direction
positions.append(Position(fforw.x, fforw.y, "FAR FORWARD"))
positions.append(Position(fback.x, fback.y, "FAR BACKWARD"))
# Low diagonal move
DIAGONAL_ANGLE = PI/6
for a in [DIAGONAL_ANGLE, - DIAGONAL_ANGLE, PI - DIAGONAL_ANGLE, DIAGONAL_ANGLE - PI]:
pt = tank_v + tank_d_v.rotate(a) * 100
positions.append(Position(pt.x, pt.y, "TURN %.0f" % degrees(a)))
# Bonuses positions
positions += [Position(b.x, b.y, "BONUS %s" % bonus_name_by_type(b.type)) for b in world.bonuses]
return positions
def __init__(self, amb, dif, spec, ref=0):
self.baseColor = Vector((255, 255, 255))
self.otherColor = Vector((0, 0, 0))
self.amb = amb
self.dif = dif
self.spec = spec
self.checkSize = 1
self.ref = ref
def get_new_positions(self, pos, tank):
x, y = pos[:2]
tank_v = Vector(tank.x, tank.y)
p = Vector(x, y)
d = p - tank_v
p1 = tank_v + d / 2 + d.normalize().rotate(PI / 2) * SHIFT_DISTANCE
p2 = tank_v + d / 2 - d.normalize().rotate(PI / 2) * SHIFT_DISTANCE
return [(p1.x, p1.y, pos[2] + " $L"), (p2.x, p2.y, pos[2] + " $R")]
def draw_tank(tank):
draw_unit(tank)
b = tank.angle + tank.turret_relative_angle
e = Vector(1, 0)
q = e.rotate(b)
tank_v = Vector(tank.x, tank.y)
hit_v = tank_v + MAX_DISTANCE * q
pygame.draw.line(window, (255, 0, 0), (tank.x, tank.y), (hit_v.x, hit_v.y))
def draw_tank(tank):
draw_unit(tank)
b = tank.angle + tank.turret_relative_angle
e = Vector(1, 0)
q = e.rotate(b)
tank_v = Vector(tank.x, tank.y)
hit_v = tank_v + MAX_DISTANCE * q
pygame.draw.line(window, (255, 0, 0), (tank.x, tank.y), (hit_v.x, hit_v.y))
def _get_danger(self, pos, tank1, tank2):
tank1_v = Vector(tank1.x, tank1.y)
tank2_v = Vector(tank2.x, tank2.y)
tank_v = Vector(pos.x, pos.y)
d1 = (tank1_v - tank_v).normalize()
d2 = (tank2_v - tank_v).normalize()
# the more 'danger' is, the more dangerous position is. Values: [0..1]
danger = fabs(1 - d1.scalar_product(d2)) / 2
return sqrt(danger)
def positions(self, tank, world):
positions = []
# Low diagonal move
tank_v = Vector(tank.x, tank.y)
tank_d_v = Vector(1, 0).rotate(tank.angle)
for a in [self.angle, -self.angle, PI - self.angle, self.angle - PI]:
pt = tank_v + tank_d_v.rotate(a) * 100
positions.append(Position(pt.x, pt.y, "TURN %.0f" % degrees(a)))
return positions
def positions(self, tank, world):
positions = []
# Low diagonal move
tank_v = Vector(tank.x, tank.y)
tank_d_v = Vector(1, 0).rotate(tank.angle)
for a in [self.angle, - self.angle, PI - self.angle, self.angle - PI]:
pt = tank_v + tank_d_v.rotate(a) * 100
positions.append(Position(pt.x, pt.y, "TURN %.0f" % degrees(a)))
return positions
def process_bullets():
b = tank.angle + tank.turret_relative_angle
q = Vector(1, 0).rotate(b)
tank_v = Vector(tank.x, tank.y)
for shell in world.shells:
shell_v = Vector(shell.x, shell.y)
shell_speed = Vector(shell.speedX, shell.speedY)
def will_meet():
if q.collinear(shell_speed):
#TODO: may be bug
good_direction = sign(shell_speed.x) != sign(
q.x) and sign(shell_speed.y) != sign(q.y)
if good_direction:
return (True, (shell_v - tank_v).length() /
(shell_speed.length() +
INITIAL_SHELL_VELOCITY))
else:
return (False, -1)
else:
d_tank, d_shell = intersect_lines(
tank_v, q, shell_v, shell_speed.normalize())
d_tank = d_tank - 40
if d_tank < 0 or d_shell < 0:
return (False, -1)
t_tank = solve_quadratic(SHELL_ACCELERATION / 2,
INITIAL_SHELL_VELOCITY,
-d_tank)
t_shell = solve_quadratic(SHELL_ACCELERATION / 2,
shell_speed.length(),
-d_shell)
if fabs(t_tank - t_shell) < 1.5:
return (True, t_tank)
else:
return (False, -1)
wm, meet_time = will_meet()
if wm:
if shell.player_name == "evernight":
self.context.debug(
'{Shooting} Hitting bullet of ally, postpone')
move.fire_type = FireType.NONE
else:
if physics.shell_will_hit(
shell, tank, factor=0.9
) and meet_time < 20 and meet_time > 5:
self.context.debug(
'{Shooting} Possible to counter flying bullet')
move.fire_type = FireType.REGULAR
def shell_will_hit_tank_going_to(self, shell, tank, x, y, et=None):
if et is None:
et = self.estimate_time_to_position(x, y, tank)
dist = tank.get_distance_to(x, y)
v0 = hypot(shell.speedX, shell.speedY)
a = SHELL_ACCELERATION
d = tank.get_distance_to_unit(shell)
#d = shell.get_distance_to(x, y)
t = solve_quadratic(a / 2, v0, -d)
if self.shell_will_hit(shell, tank, factor=1.05) and (et > t):
return 1
#self.max_move_distance(fabs(v0), FICTIVE_ACCELERATION, 3, t) < dist):
if dist < 150:
# short distance
result = self.shell_will_hit(shell,
fictive_unit(tank, x, y),
factor=1.05)
if result:
pt_v = Vector(x, y)
tank_v = Vector(tank.x, tank.y)
dir = pt_v - tank_v
shell_speed = Vector(shell.speedX, shell.speedY)
if dir.is_zero() or shell_speed.is_zero():
return float(result)
if dir.angle(shell_speed) < PI / 8 and shell.get_distance_to(
x, y) > d:
return 0.6
return result
else:
# long distance, check if our direction is intersecting segment between shell and shell + v_shell*t
pt_v = Vector(x, y)
tank_v = Vector(tank.x, tank.y)
dir = tank_v - pt_v
shell_v = Vector(shell.x, shell.y)
shell_speed = Vector(shell.speedX, shell.speedY)
next_shell = shell_v + shell_speed * (t + 5)
if sign((shell_v - tank_v).cross_product(dir)) == sign(
dir.cross_product(next_shell - tank_v)):
return True
else:
return False
def store_shell_velocity(self, world):
if not self.enabled:
return
for shell in world.shells:
self.shell_velocity[shell.id].append(
Vector(shell.speedX, shell.speedY).length())
if world.tick % 100 == 0:
pickle.dump(self.shell_velocity, open('shells.dump', 'wb'))
def vector_is_intersecting_object(self, p, d, target, factor=1):
center = Vector(target.x - p.x, target.y - p.y)
if center.scalar_product(d) < 0:
return False
a = target.angle
w, h = target.width/2 * factor, target.height/2 * factor
c1 = center + Vector(w ,h).rotate(a)
c2 = center + Vector(- w, h).rotate(a)
c3 = center + Vector(- w, - h).rotate(a)
c4 = center + Vector(w, - h).rotate(a)
if sign(c1.cross_product(d)) == sign(d.cross_product(c3)):
return True
if sign(c2.cross_product(d)) == sign(d.cross_product(c4)):
return True
return False
def estimate_time_to_position(self, x, y, tank):
dist = tank.get_distance_to(x, y)
vt = Vector(tank.speedX, tank.speedY)
tank_v = Vector(tank.x, tank.y)
pt_v = Vector(x, y)
d = pt_v - tank_v
if d.is_zero():
return 0
tank_d_v = Vector(1, 0).rotate(tank.angle)
if vt.is_zero() or d.is_zero():
vd_angle = 0
else:
vd_angle = vt.angle(d)
if tank_d_v.is_zero() or d.is_zero():
d_angle = 0
else:
d_angle = tank_d_v.angle(d)
# Short distances fix
if dist < 100:
if fabs(d_angle) < LOW_ANGLE:
v0 = vt.projection(d)
return solve_quadratic(FICTIVE_ACCELERATION/2, v0, -dist)
#return dist/6
elif PI - fabs(d_angle) < LOW_ANGLE:
v0 = vt.projection(d)
return solve_quadratic(FICTIVE_ACCELERATION/2 * 0.75, v0, -dist)
#return dist/6 /0.75
if d.is_zero():
values = (0, 0, 0, 0, 0, 0, 0, 1)
else:
values = (
d_angle,
dist,
vd_angle,
vt.length(),
tank.angular_speed,
tank.crew_health/tank.crew_max_health,
d_angle ** 2,
1
)
return sum([x * y for (x, y) in zip(values, TIME_ESTIMATION_COEF)])
def value(self, target):
tank = self.context.tank
target_v = Vector(target.x, target.y)
tank_v = Vector(tank.x, tank.y)
penalty = 0
obstacle = self.context.world.obstacles[0]
if self.context.physics.vector_is_intersecting_object(
tank_v, target_v - tank_v, obstacle, 1.05):
penalty = self.max_value
#TODO: fix this
# if obstacle_is_attacked(self.context, target):
# penalty = self.max_value
# else:
# penalty = 0
return -penalty
def value(self, pos):
enemies = self.context.enemies
tank = self.context.tank
pos_v = Vector(pos.x, pos.y)
result = 0
obstacle = self.context.world.obstacles[0]
for enemy in enemies:
enemy_v = Vector(enemy.x, enemy.y)
if self.context.physics.vector_is_intersecting_object(
pos_v, enemy_v - pos_v, obstacle, 1.05):
if self.context.health_fraction <= 0.3 or tank.remaining_reloading_time > 100:
result += self.max_value
else:
result -= self.max_value
return result
def positions(self, tank, world):
positions = []
# Forward and back direction
tank_v = Vector(tank.x, tank.y)
tank_d_v = Vector(1, 0).rotate(tank.angle)
forw = tank_v + tank_d_v * 40
back = tank_v - tank_d_v * 40
positions.append(Position(forw.x, forw.y, "FORWARD"))
positions.append(Position(back.x, back.y, "BACKWARD"))
fforw = tank_v + tank_d_v * 80
fback = tank_v - tank_d_v * 80
positions.append(Position(fforw.x, fforw.y, "FAR FORWARD"))
positions.append(Position(fback.x, fback.y, "FAR BACKWARD"))
return positions
def getTangentVectorAt(self, t):
"""
Returns the tangent vector at value
:param t: parameter value, between 0 and 1
:return: unit tangent vector at t
"""
x = 2*(1 - t)*(self.p1.x - self.p0.x) + 2*t*(self.p2.x - self.p1.x)
y = 2*(1 - t)*(self.p1.y - self.p0.y) + 2*t*(self.p2.y - self.p1.y)
return Vector(x, y).normalize()
def test_case_4(self):
groove_area = [
Vector(0, 10),
Vector(100, 10),
Vector(0, -10),
Vector(100, -10)
]
lines = GrooveEvaluator.calculate_groove_passes(groove_area,
True,
tool_thickness=8,
tool_interlock=1)
expected_lines = [
Line(Vector(0, -6), Vector(100, -6)),
Line(Vector(0, 6), Vector(100, 6)),
Line(Vector(0, 1), Vector(100, 1))
]
self.assertListEqual(expected_lines, lines)
def shell_will_hit_tank_going_to(self, shell, tank, x, y, et=None):
if et is None:
et = self.estimate_time_to_position(x, y, tank)
dist = tank.get_distance_to(x, y)
v0 = hypot(shell.speedX, shell.speedY)
a = SHELL_ACCELERATION
d = tank.get_distance_to_unit(shell)
#d = shell.get_distance_to(x, y)
t = solve_quadratic(a/2, v0, -d)
if self.shell_will_hit(shell, tank, factor=1.05) and (et > t):
return 1
#self.max_move_distance(fabs(v0), FICTIVE_ACCELERATION, 3, t) < dist):
if dist < 150:
# short distance
result = self.shell_will_hit(shell, fictive_unit(tank, x, y), factor=1.05)
if result:
pt_v = Vector(x, y)
tank_v = Vector(tank.x, tank.y)
dir = pt_v - tank_v
shell_speed = Vector(shell.speedX, shell.speedY)
if dir.is_zero() or shell_speed.is_zero():
return float(result)
if dir.angle(shell_speed) < PI/8 and shell.get_distance_to(x, y) > d:
return 0.6
return result
else:
# long distance, check if our direction is intersecting segment between shell and shell + v_shell*t
pt_v = Vector(x, y)
tank_v = Vector(tank.x, tank.y)
dir = tank_v - pt_v
shell_v = Vector(shell.x, shell.y)
shell_speed = Vector(shell.speedX, shell.speedY)
next_shell = shell_v + shell_speed * (t + 5)
if sign((shell_v - tank_v).cross_product(dir)) == sign(dir.cross_product(next_shell - tank_v)):
return True
else:
return False
def position_is_blocked(self, x, y, tank, world):
#if tank.get_distance_to(x, y) > 400:
# return False
tank_v = Vector(tank.x, tank.y)
p = Vector(x, y)
dist = tank.get_distance_to(x, y)
if (p - tank_v).is_zero():
return False
obstacles = chain(filter(NOT_TANK(tank.id), world.tanks),
world.obstacles)
for obj in obstacles:
obj_dist = tank.get_distance_to_unit(obj)
if self.vector_is_intersecting_object(
tank_v, p - tank_v, obj, factor=1.2) and obj_dist < dist:
if not is_going_to_move(obj):
return obj
if obj_dist < max(obj.width, obj.height) + 50:
return obj
return False
def save_target_init_values(self, tank, x, y):
if not self.enabled:
return
dist = tank.get_distance_to(x, y)
vt = Vector(tank.speedX, tank.speedY)
tank_v = Vector(tank.x, tank.y)
pt_v = Vector(x, y)
d = pt_v - tank_v
tank_d_v = Vector(1, 0).rotate(tank.angle)
if vt.is_zero():
vd_angle = 0
else:
vd_angle = vt.angle(d)
if d.is_zero():
self.last_target_time_init_values = (0, 0, 0, 0, 0, 0)
self.last_target_time_init_values = (
tank_d_v.angle(d),
dist,
vd_angle,
vt.length(),
tank.angular_speed,
tank.crew_health/tank.crew_max_health
)
def will_hit_precise(self,
tank,
target,
ricochet_angle=PI / 4,
factor=1,
side_part=1):
"""
Returns True if tank will hit rectangular object
"""
b = tank.angle + tank.turret_relative_angle
e = Vector(1, 0)
q = e.rotate(b)
tank_v = Vector(tank.x, tank.y)
c = get_unit_corners(target, factor)
hit_v = tank_v + MAX_DISTANCE * q
def will_hit_side(p1, p2):
p_mid = (p1 + p2) / 2
p1_new = p_mid + (p1 - p_mid) * side_part
p2_new = p_mid + (p2 - p_mid) * side_part
intersecting = segments_are_intersecting(tank_v, hit_v, p1_new,
p2_new)
angle = q.angle(p2 - p1)
safe = ricochet_angle < angle < PI - ricochet_angle
return intersecting and safe
sides = [(c[0], c[1]), (c[1], c[2]), (c[2], c[3]), (c[3], c[0])]
closest_corner = min(c, key=lambda x: x.distance(tank_v))
closer_sides = list(
filter(lambda s: s[0] == closest_corner or s[1] == closest_corner,
sides))
#result = will_hit_side(c1, c2) or will_hit_side(c2, c3) or will_hit_side(c3, c4) or will_hit_side(c4, c1)
result = will_hit_side(closer_sides[0][0],
closer_sides[0][1]) or will_hit_side(
closer_sides[1][0], closer_sides[1][1])
return result
def value(self, target):
tank = self.context.tank
tank_v = Vector(tank.x, tank.y)
target_v = Vector(target.x, target.y)
#b = tank.angle + tank.turret_relative_angle
b = (target_v - tank_v).angle(Vector(1, 0))
target_direction = Vector(1, 0).rotate(target.angle)
target_speed = Vector(target.speedX, target.speedY)
def get_hit_time():
v0 = INITIAL_SHELL_VELOCITY
a = SHELL_ACCELERATION
d = max(0, tank.get_distance_to_unit(target) - 60)
return solve_quadratic(a / 2, v0, -d)
def max_move_distance(v0, a, max_v, t):
v0 = max(-max_v, min(v0, max_v))
if fabs(v0 + a * t) > max_v:
if a > 0:
t1 = fabs((max_v - v0) / a)
else:
t1 = fabs((-max_v - v0) / a)
t2 = t - t1
else:
t1 = t
t2 = 0
if a > 0:
return a * t1**2 / 2 + v0 * t1 + max_v * t2
else:
return a * t1**2 / 2 + v0 * t1 - max_v * t2
t = get_hit_time()
v0 = target_speed.projection(target_direction)
target_health_fraction = target.crew_health / target.crew_max_health
efficency = (1 + target_health_fraction) / 2
target_avoid_distance_forward = max_move_distance(
v0, FICTIVE_TARGET_ACCELERATION * efficency,
MAX_TARGET_SPEED * efficency, t)
target_avoid_distance_backward = max_move_distance(
v0, -FICTIVE_TARGET_ACCELERATION * 0.75 * efficency,
MAX_TARGET_SPEED * efficency, t)
target_turret_n_cos = fabs(cos(fabs(b - target.angle) + PI / 2))
var = fabs(
(target_avoid_distance_forward - target_avoid_distance_backward) *
target_turret_n_cos)
return (1 - var / 200) * self.max_value
def positions(self, tank, world):
positions = []
# Current position
positions.append(Position(tank.x, tank.y, "CURRENT"))
# Forward and back direction
tank_v = Vector(tank.x, tank.y)
tank_d_v = Vector(1, 0).rotate(tank.angle)
fforw = tank_v + tank_d_v * 80
fback = tank_v - tank_d_v * 80
positions.append(Position(fforw.x, fforw.y, "FAR FORWARD"))
positions.append(Position(fback.x, fback.y, "FAR BACKWARD"))
# Bonuses positions
positions += [
Position(b.x, b.y, "BONUS %s" % bonus_name_by_type(b.type))
for b in world.bonuses
]
return positions
def vector_is_intersecting_object(self, p, d, target, factor=1):
center = Vector(target.x - p.x, target.y - p.y)
if center.scalar_product(d) < 0:
return False
a = target.angle
w, h = target.width / 2 * factor, target.height / 2 * factor
c1 = center + Vector(w, h).rotate(a)
c2 = center + Vector(-w, h).rotate(a)
c3 = center + Vector(-w, -h).rotate(a)
c4 = center + Vector(w, -h).rotate(a)
if sign(c1.cross_product(d)) == sign(d.cross_product(c3)):
return True
if sign(c2.cross_product(d)) == sign(d.cross_product(c4)):
return True
return False
def test_case_1(self):
groove_area = [
Vector(0, 5),
Vector(100, 5),
Vector(0, -5),
Vector(100, -5)
]
lines = GrooveEvaluator.calculate_groove_passes(groove_area,
True,
tool_thickness=10,
tool_interlock=1)
expected_lines = [Line(Vector(0, 0), Vector(100, 0))]
self.assertListEqual(expected_lines, lines)
def _make_turn(self, tank, world, move):
# Precalc for estimation
self.enemies = list(filter(ALIVE_ENEMY_TANK, world.tanks))
self.allies = list(filter(ALLY_TANK(tank.id), world.tanks))
self.health_fraction = tank.crew_health / tank.crew_max_health
self.hull_fraction = tank.hull_durability / tank.hull_max_durability
self.enemies_count = len(self.enemies)
self.est_time_cache = {}
positions = []
for pg in self.position_getters:
positions += pg.positions(tank, world)
self.debug('Got %d positions' % len(positions))
if not positions:
return
for e in self.position_estimators:
e.context = self
pos_and_values = [
(pos, sum([e.value(pos) for e in self.position_estimators]))
for pos in positions
]
if self.debug_mode:
top_pos = [
item[0] for item in list(
reversed(sorted(pos_and_values, key=operator.itemgetter(
1))))[:6]
]
self.debug(' ' * 50, end='')
for e in self.position_estimators:
self.debug('%14s' % e.NAME, end='')
self.debug('%14s' % 'RESULT', end='')
self.debug('')
def out_pos(pos):
self.debug('%-50s' % (str(pos) + ' : '), end='')
res = 0
for e in self.position_estimators:
v = e.value(pos)
self.debug('%14.2f' % v, end='')
res += v
self.debug('%14.2f' % res, end='')
self.debug('')
for pos in top_pos:
out_pos(pos)
self.debug('=' * 16)
for pos in positions:
if pos.name.find('BONUS') != -1:
out_pos(pos)
self.debug('=' * 16)
for pos in positions:
if pos.name == 'FORWARD' or pos.name == 'BACKWARD' or pos.name == 'CURRENT':
out_pos(pos)
self.debug('=' * 16)
for pos in positions:
if pos.name.find('BORDER') != -1:
out_pos(pos)
next_position = None
position_iteration = 0
#average_F = sum([pos[0] for pos in pos_f])/len(pos_f)
pos_queue = PriorityQueue()
for p_v in pos_and_values:
pos_queue.put((-p_v[1] + random() * 1e-3, p_v[0]))
while True:
cur = pos_queue.get()[1]
if not self.physics.position_is_blocked(
cur.x, cur.y, tank,
world) or position_iteration >= MAX_POSITION_ITERATIONS:
next_position = cur
break
position_iteration += 1
self.debug('!!! Skipping best position, iteration %d' %
position_iteration)
if self.debug_mode:
self.debug('(blocked by %s)' % str(
self.physics.position_is_blocked(cur.x, cur.y, tank,
world)))
self.debug(
"GOING TO [%10s] (%8.2f, %8.2f); distance=%8.2f, ETA=%8.2f" %
(next_position.name, next_position.x, next_position.y,
self.tank.get_distance_to(next_position.x, next_position.y),
self.physics.estimate_time_to_position(next_position.x,
next_position.y, tank)))
self.memory.last_target_position[tank.id] = next_position
self.physics.move_to_position(next_position.x, next_position.y, tank,
move)
self.debug('=' * 16)
if self.debug_mode:
for shell in world.shells:
v0 = hypot(shell.speedX, shell.speedY)
a = SHELL_ACCELERATION
d = tank.get_distance_to_unit(shell)
tank_v = Vector(tank.x, tank.y)
shell_v = Vector(shell.x, shell.y)
shell_speed = Vector(shell.speedX, shell.speedY)
#d = shell.get_distance_to(x, y)
t = solve_quadratic(a / 2, v0, -d)
self.debug(
'SHELL [%12s] (%8.2f, %8.2f) v=%s, will hit=%s, hit time=%8.2f'
%
(shell.player_name, shell.x, shell.y, shell_speed.length(),
str(self.physics.shell_will_hit(shell, tank,
factor=1.05)), t))
self.debug('=' * 16)
def estimate_time_to_position(self, x, y, tank):
dist = tank.get_distance_to(x, y)
vt = Vector(tank.speedX, tank.speedY)
tank_v = Vector(tank.x, tank.y)
pt_v = Vector(x, y)
d = pt_v - tank_v
if d.is_zero():
return 0
tank_d_v = Vector(1, 0).rotate(tank.angle)
if vt.is_zero() or d.is_zero():
vd_angle = 0
else:
vd_angle = vt.angle(d)
if tank_d_v.is_zero() or d.is_zero():
d_angle = 0
else:
d_angle = tank_d_v.angle(d)
# Short distances fix
if dist < 100:
if fabs(d_angle) < LOW_ANGLE:
v0 = vt.projection(d)
return solve_quadratic(FICTIVE_ACCELERATION / 2, v0, -dist)
#return dist/6
elif PI - fabs(d_angle) < LOW_ANGLE:
v0 = vt.projection(d)
return solve_quadratic(FICTIVE_ACCELERATION / 2 * 0.75, v0,
-dist)
#return dist/6 /0.75
if d.is_zero():
values = (0, 0, 0, 0, 0, 0, 0, 1)
else:
values = (d_angle, dist, vd_angle, vt.length(), tank.angular_speed,
tank.crew_health / tank.crew_max_health, d_angle**2, 1)
return sum([x * y for (x, y) in zip(values, TIME_ESTIMATION_COEF)])
def getCellsForCube(self, typeString, container):
x = int(typeString)
cells = {}
index = 1
for k in range(6):
if k == 0:
normal = Vector(1, 0, 0)
up = Vector(0, 0, 1)
elif k == 1:
normal = Vector(0, 1, 0)
up = Vector(0, 0, 1)
elif k == 2:
normal = Vector(0, 0, 1)
up = Vector(1, 0, 0)
elif k == 3:
normal = Vector(-1, 0, 0)
up = Vector(0, 0, 1)
elif k == 4:
normal = Vector(0, -1, 0)
up = Vector(0, 0, 1)
elif k == 5:
normal = Vector(0, 0, -1)
up = Vector(1, 0, 0)
right = normal.cross(up).unit()
for j in range(x):
for i in range(x):
position = Locus(0,0,0).translateByVector(normal.scalarMultiply(x)).translateByVector(up.scalarMultiply(x - 1 - (2 * j))).translateByVector(right.scalarMultiply(x - 1 - (2 * i)))
neighborJunctions = []
neighborJunctions.append(position.translateByVector(right))
neighborJunctions.append(position.translateByVector(right.scalarMultiply(-1)))
neighborJunctions.append(position.translateByVector(up))
neighborJunctions.append(position.translateByVector(up.scalarMultiply(-1)))
cells[index] = Cell(index, position, normal, up, [1,1,1,1], neighborJunctions, 4, container)
index += 1
self.cells = cells
self.generateNeighbors()
def __init__(self, enemy, width):
self.enemy_v = Vector(enemy.x, enemy.y)
self.turret_v = Vector(1, 0).rotate(enemy.angle +
enemy.turret_relative_angle)
self.width = width
def get_target_data(context):
# New Decision Maker
tank = context.tank
target = context.cur_target
physics = context.physics
b = tank.angle + tank.turret_relative_angle
#e = Vector(1, 0)
#q = e.rotate(b)
tank_v = Vector(tank.x, tank.y)
target_v = Vector(target.x, target.y)
target_direction = Vector(1, 0).rotate(target.angle)
target_speed = Vector(target.speedX, target.speedY)
def get_hit_time():
v0 = INITIAL_SHELL_VELOCITY
a = SHELL_ACCELERATION
d = max(0, tank.get_distance_to_unit(target) - 60)
return solve_quadratic(a/2, v0, -d)
t = get_hit_time()
#center = Vector(target.x - tank.x, target.y - tank.y)
v0 = target_speed.projection(target_direction)
target_health_fraction = target.crew_health/target.crew_max_health
efficency = (1 + target_health_fraction)/2
target_avoid_distance_forward = physics.max_move_distance(v0, FICTIVE_TARGET_ACCELERATION * efficency, MAX_TARGET_SPEED * efficency, t)
target_avoid_distance_backward = physics.max_move_distance(v0, -FICTIVE_TARGET_ACCELERATION * BACKWARDS_FICTIVE_MULTIPLIER * efficency, MAX_TARGET_SPEED * efficency, t)
#max_pos = target_v + target_avoid_distance_forward * target_direction
#min_pos = target_v + target_avoid_distance_backward * target_direction
target_turret_n_cos = fabs(cos(fabs(b - target.angle) + PI/2))
#var = fabs((target_avoid_distance_forward - target_avoid_distance_backward) * target_turret_n_cos)
allies_targeting = inverse_dict(context.memory.target_id, target.id)
def single_attacker():
#estimate_pos = target_v + target_direction * ((target_avoid_distance_forward + target_avoid_distance_backward) / 2)
#vulnerable_width = max(90 * target_turret_n_cos, 60 * (1 - target_turret_n_cos))
target_avoid_distance_forward_new = target_avoid_distance_forward * context.memory.tank_precision[tank.id]
target_avoid_distance_backward_new = target_avoid_distance_backward * context.memory.tank_precision[tank.id]
max_pos = target_v + target_avoid_distance_forward_new * target_direction
min_pos = target_v + target_avoid_distance_backward_new * target_direction
max_pos_fu = fictive_unit(target, max_pos.x, max_pos.y)
min_pos_fu = fictive_unit(target, min_pos.x, min_pos.y)
#shoot = physics.will_hit(tank, max_pos_fu, 0.9) and physics.will_hit(tank, min_pos_fu, 0.9)
shoot_precise = physics.will_hit_precise(tank, max_pos_fu, factor=0.8, side_part=0.8) and \
physics.will_hit_precise(tank, min_pos_fu, factor=0.8, side_part=0.8) and \
physics.will_hit_precise(tank, target, factor=0.8, side_part=0.8)
shoot = shoot_precise
fabs(target_turret_n_cos)
#all_corners = get_unit_corners(max_pos_fu) + get_unit_corners(min_pos_fu)
all_corners = get_unit_corners(target)
#closest_corner = min(all_corners, key=lambda c: c.distance(tank_v))
# Instead of closest corner try to target middle of colsest side
sc1, sc2 = sorted(all_corners, key=lambda c: c.distance(tank_v))[:2]
closest_corner = 0.75 * sc1 + 0.25 * sc2
middle_position = (max_pos + min_pos)/2
w = fabs(target_turret_n_cos)**2
estimate_pos = w*middle_position + (1 - w) * closest_corner
comment = 'SINGLE, %s' % w
if obstacle_is_attacked(context, estimate_pos):
shoot = False
comment += ' blocked'
if context.debug_mode:
if shoot and tank.remaining_reloading_time == 0:
debug_data = {
"units": [min_pos_fu, max_pos_fu, target],
"tanks": [tank]
}
debug_dump(debug_data, "/".join([str(context.memory.battle_id), str(context.world.tick) + "_" + str(tank.id) + "_single"]))
return ((estimate_pos.x, estimate_pos.y), shoot, target_avoid_distance_forward, target_avoid_distance_backward, comment)
def multiple_attackers(attackers):
cnt = len(allies_targeting)
ind = index_in_sorted(allies_targeting, tank.id)
segment = (target_avoid_distance_forward - target_avoid_distance_backward)/(cnt + 1)
shift = segment * (ind + 1) + target_avoid_distance_backward
#shift *
if cnt == 2:
if ind == 0:
shift = target_avoid_distance_backward + 45
else:
shift = target_avoid_distance_forward - 45
estimate_pos = target_v + target_direction * shift
shift_fu = fictive_unit(target, estimate_pos.x, estimate_pos.y)
shoot = (physics.will_hit_precise(tank, shift_fu, factor=0.8) and
all([lambda a: a.remaining_reloading_time < MULTIPLE_MAX_TIME_DIFFERENCE or a.reloading_time - a.remaining_reloading_time < MULTIPLE_MAX_TIME_DIFFERENCE, attackers])
#and target_avoid_distance_forward - target_avoid_distance_backward < 200
)
comment = 'MULTIPLE(%d), shift=%8.2f' % (ind, shift)
if obstacle_is_attacked(context, estimate_pos):
shoot = False
comment += ' blocked'
if context.debug_mode:
if shoot and tank.remaining_reloading_time == 0 and all([context.memory.good_to_shoot.get(t.id) or t.id == tank.id for t in attackers]):
max_pos = target_v + target_avoid_distance_forward * target_direction
min_pos = target_v + target_avoid_distance_backward * target_direction
max_pos_fu = fictive_unit(target, max_pos.x, max_pos.y)
min_pos_fu = fictive_unit(target, min_pos.x, min_pos.y)
debug_data = {
"units": [min_pos_fu, max_pos_fu, target],
"tanks": attackers
}
debug_dump(debug_data, "/".join([str(context.memory.battle_id), str(context.world.tick) + "_" + str(tank.id) + "_multiple"]))
return ((estimate_pos.x, estimate_pos.y), shoot, target_avoid_distance_forward, target_avoid_distance_backward, comment)
context.memory.good_to_shoot[tank.id] = False
try_single = single_attacker()
if len(allies_targeting) == 2 and try_single[1] == False:
if tank.id in allies_targeting:
attackers = []
for t in context.world.tanks:
if t.id in allies_targeting:
attackers.append(t)
if all([lambda a: a.remaining_reloading_time < 70 or a.reloading_time - a.remaining_reloading_time < MULTIPLE_MAX_TIME_DIFFERENCE, attackers]):
try_multiple = multiple_attackers(attackers)
if try_multiple[1]:
if tank.remaining_reloading_time < 3:
context.memory.good_to_shoot[tank.id] = True
ok = all([context.memory.good_to_shoot.get(t.id) for t in attackers])
if not ok:
try_multiple = (try_multiple[0], False)
return try_multiple
return try_single
def attacked_area(self, x, y):
pt_v = Vector(x, y)
if (pt_v - self.enemy_v).scalar_product(self.turret_v) <= 0:
return 0
dist = pt_v.distance_to_line(self.enemy_v, self.turret_v)
return max(0, (self.width - dist) / self.width)
def attacked_area(self, x, y):
pt_v = Vector(x, y)
if (pt_v - self.enemy_v).scalar_product(self.turret_v) <= 0:
return 0
dist = pt_v.distance_to_line(self.enemy_v, self.turret_v)
return max(0, (self.width - dist)/self.width)
def test_case_5(self):
groove_area = [
Vector(-1, 479.1),
Vector(1001, 479.1),
Vector(-1, 469.1),
Vector(1001, 469.1)
]
lines = GrooveEvaluator.calculate_groove_passes(groove_area,
True,
tool_thickness=2.2,
tool_interlock=1)
expected_lines = [
Line(Vector(-1, 470.2), Vector(1001, 470.2)),
Line(Vector(-1, 478.0), Vector(1001, 478.0)),
Line(Vector(-1, 471.4), Vector(1001, 471.4)),
Line(Vector(-1, 476.8), Vector(1001, 476.8)),
Line(Vector(-1, 472.6), Vector(1001, 472.6)),
Line(Vector(-1, 475.6), Vector(1001, 475.6)),
Line(Vector(-1, 473.8), Vector(1001, 473.8)),
Line(Vector(-1, 474.4), Vector(1001, 474.4))
]
self.assertListEqual(expected_lines, lines)
def _make_turn(self, tank, world, move):
# Precalc for estimation
self.enemies = list(filter(ALIVE_ENEMY_TANK, world.tanks))
self.allies = list(filter(ALLY_TANK(tank.id), world.tanks))
self.health_fraction = tank.crew_health / tank.crew_max_health
self.hull_fraction = tank.hull_durability / tank.hull_max_durability
self.enemies_count = len(self.enemies)
self.est_time_cache = {}
positions = []
for pg in self.position_getters:
positions += pg.positions(tank, world)
self.debug('Got %d positions' % len(positions))
if not positions:
return
for e in self.position_estimators:
e.context = self
pos_and_values = [(pos, sum([e.value(pos) for e in self.position_estimators]))
for pos in positions]
if self.debug_mode:
top_pos = [item[0] for item in list(reversed(sorted(pos_and_values, key=operator.itemgetter(1))))[:6]]
self.debug(' ' * 50, end='')
for e in self.position_estimators:
self.debug('%14s' % e.NAME, end='')
self.debug('%14s' % 'RESULT', end='')
self.debug('')
def out_pos(pos):
self.debug('%-50s' % (str(pos) + ' : '), end='')
res = 0
for e in self.position_estimators:
v = e.value(pos)
self.debug('%14.2f' % v, end='')
res += v
self.debug('%14.2f' % res, end='')
self.debug('')
for pos in top_pos:
out_pos(pos)
self.debug('=' * 16)
for pos in positions:
if pos.name.find('BONUS') != -1:
out_pos(pos)
self.debug('=' * 16)
for pos in positions:
if pos.name == 'FORWARD' or pos.name == 'BACKWARD' or pos.name == 'CURRENT':
out_pos(pos)
self.debug('=' * 16)
for pos in positions:
if pos.name.find('BORDER') != -1:
out_pos(pos)
next_position = None
position_iteration = 0
#average_F = sum([pos[0] for pos in pos_f])/len(pos_f)
pos_queue = PriorityQueue()
for p_v in pos_and_values:
pos_queue.put( (-p_v[1] + random() * 1e-3, p_v[0]) )
while True:
cur = pos_queue.get()[1]
if not self.physics.position_is_blocked(cur.x, cur.y, tank, world) or position_iteration >= MAX_POSITION_ITERATIONS:
next_position = cur
break
position_iteration += 1
self.debug('!!! Skipping best position, iteration %d' % position_iteration)
if self.debug_mode:
self.debug('(blocked by %s)' % str(self.physics.position_is_blocked(cur.x, cur.y, tank, world)))
self.debug("GOING TO [%10s] (%8.2f, %8.2f); distance=%8.2f, ETA=%8.2f" %
(next_position.name, next_position.x, next_position.y,
self.tank.get_distance_to(next_position.x, next_position.y), self.physics.estimate_time_to_position(next_position.x, next_position.y, tank))
)
self.memory.last_target_position[tank.id] = next_position
self.physics.move_to_position(next_position.x, next_position.y, tank, move)
self.debug('=' * 16)
if self.debug_mode:
for shell in world.shells:
v0 = hypot(shell.speedX, shell.speedY)
a = SHELL_ACCELERATION
d = tank.get_distance_to_unit(shell)
tank_v = Vector(tank.x, tank.y)
shell_v = Vector(shell.x, shell.y)
shell_speed = Vector(shell.speedX, shell.speedY)
#d = shell.get_distance_to(x, y)
t = solve_quadratic(a/2, v0, -d)
self.debug('SHELL [%12s] (%8.2f, %8.2f) v=%s, will hit=%s, hit time=%8.2f' %
(shell.player_name, shell.x, shell.y, shell_speed.length(), str(self.physics.shell_will_hit(shell, tank, factor=1.05)), t) )
self.debug('=' * 16)
def test_case_7(self):
groove_area = [
Vector(0, 10),
Vector(20, 10),
Vector(0, 0),
Vector(20, 0)
]
lines = GrooveEvaluator.calculate_groove_passes(groove_area,
True,
tool_thickness=3,
tool_interlock=1)
expected_lines = [
Line(Vector(0, 1.5), Vector(20, 1.5)),
Line(Vector(0, 8.5), Vector(20, 8.5)),
Line(Vector(0, 3.5), Vector(20, 3.5)),
Line(Vector(0, 6.5), Vector(20, 6.5)),
Line(Vector(0, 5.5), Vector(20, 5.5)),
]
self.assertListEqual(expected_lines, lines)
def test_case_6(self):
groove_area = [
Vector(-1, 479.1),
Vector(451, 479.1),
Vector(-1, 469.1),
Vector(451, 469.1)
]
lines = GrooveEvaluator.calculate_groove_passes(groove_area,
True,
tool_thickness=3.2,
tool_interlock=1)
expected_lines = [
Line(Vector(-1, 470.7), Vector(451, 470.7)),
Line(Vector(-1, 477.5), Vector(451, 477.5)),
Line(Vector(-1, 472.9), Vector(451, 472.9)),
Line(Vector(-1, 475.3), Vector(451, 475.3)),
Line(Vector(-1, 475.1), Vector(451, 475.1)),
]
self.assertListEqual(expected_lines, lines)
def inverse_kinematics(self, goal, tol_limit=0.05, max_iterations=100):
'''
Preforms Inverse Kinematics on the arm using the known lengths and last positions.
This uses the FABRIK method.
See the more information section for information on the FABRIK method.
Parameters
----------
goal : arraylike or Point
desired location in the format of [x,y,z]
tol_limit : scalar (optional)
tolerance between the desired location and actual one after iteration
max_iterations : scalar (optional)
maximum itterations that the function is allowed to run
Returns
-------
q1 : scalar
rotation of the base about the z axis
q2 : scalar
rotation of first link relative to the base
q3 : scalar
rotation of the second link relative to the first link
q4 : scalar
rotation of the third link relative to the second link
x_joints : array_like
x points for all the joint locations
y_joints : array_like
y points for all the joint locations
z_joints : array_like
z points for all the joint locations
More Information
----------------
Overall algorithim:
solve for unit vectors going backwards from the desired point to the original point
of the joints, then forwards from the origin, see youtube video
https://www.youtube.com/watch?v=UNoX65PRehA&t=817s
'''
if isinstance(goal, list):
if len(goal) != 3:
raise IndexError(
"goal unclear. Need x,y,z coordinates in Point or list form."
)
goal = Point(goal[0], goal[1], goal[2])
# Find base rotation
# Initial angle of where end effector is
initial_base_roation = np.arctan2(self.y_joints[-1], self.x_joints[-1])
# Desired angle
# arctan2(y,x)
self.q1 = np.arctan2(goal.y, goal.x)
# Base rotation
base_rotation = self.q1 - initial_base_roation
# Base rotation matrix about z
z_rot = np.array([[np.cos(base_rotation), -np.sin(base_rotation), 0.0],
[np.sin(base_rotation),
np.cos(base_rotation), 0.0], [0.0, 0.0, 1.0]])
# Rotate the location of each joint by the base rotation
# This will force the FABRIK algorithim to only solve
# in two dimensions, else each joint will move as if it has
# a 3 DOF range of motion
point4 = Point(
np.dot(z_rot,
[self.x_joints[3], self.y_joints[3], self.z_joints[3]]))
point3 = Point(
np.dot(z_rot,
[self.x_joints[2], self.y_joints[2], self.z_joints[2]]))
point2 = Point(
np.dot(z_rot,
[self.x_joints[1], self.y_joints[1], self.z_joints[1]]))
point1 = Point(
np.dot(z_rot,
[self.x_joints[0], self.y_joints[0], self.z_joints[0]]))
# store starting point of the first joint
starting_point1 = point1
iterations = 0
# Make sure the desired x,y,z point is reachable
if Vector.magnitude(goal) > self.total_arm_length:
print ' desired point is likely out of reach'
for _ in range(1, max_iterations + 1):
# backwards
point3 = Vector.project_along_vector(goal, point3,
self.third_link_length)
point2 = Vector.project_along_vector(point3, point2,
self.second_link_length)
point1 = Vector.project_along_vector(point2, point1,
self.first_link_length)
# forwards
point2 = Vector.project_along_vector(point1, point2,
self.first_link_length)
point3 = Vector.project_along_vector(point2, point3,
self.second_link_length)
point4 = Vector.project_along_vector(point3, goal,
self.third_link_length)
# Solve for tolerance between iterated point and desired x,y,z,
tol = point4 - goal
# Make tolerance relative to x,y,z
tol = Vector.magnitude(tol)
iterations = iterations + 1
# Check if tolerance is within the specefied limit
if tol < tol_limit:
break
# Re-organize points into a big matrix for plotting elsewhere
self.p_joints = np.transpose([
starting_point1.as_array(),
point2.as_array(),
point3.as_array(),
point4.as_array()
])
self.x_joints = self.p_joints[0]
self.y_joints = self.p_joints[1]
self.z_joints = self.p_joints[2]
# Return the joint angles by finding the angles with the dot produt
vector21 = Vector(point2 - point1)
vector32 = Vector(point3 - point2)
vector43 = Vector(point4 - point3)
# returns -pi to pi
self.q2 = np.arctan2(
vector21.end.z, Vector.magnitude([vector21.end.x, vector21.end.y]))
# Negative sign because of dh notation, a rotation away from the previous link
# and towards the x-y plane is a negative moment about the relative z axis.
# the relative z axis of each link is out of the page if looking at the arm
# in 2D
# the x axis in dh convention is typically along the link direction.
self.q3 = -1.0 * vector21.angle(vector32)
self.q4 = -1.0 * vector32.angle(vector43)
return [
self.q1, self.q2, self.q3, self.q4, self.x_joints, self.y_joints,
self.z_joints
]
