def _inverse_kinematics(self, target):
"""Calculate the necessary angle for a target, in the YZ plane.
Uses the circle intersection method."""
# Circle Centers
c0 = Coord(0, -BASE_RADIUS, 0);
c1 = Coord(0, target.y - EFFECTOR_RADIUS, target.z);
# Distance between both circles
dist = c0.get_abs_distance(c1);
# Cirle radius
r0 = BICEP_LENGTH;
r1 = math.sqrt(FOREARM_LENGTH**2 - target.x**2) + JOINT_OFFSET;
# Check valid distance between the two
if 0 >= dist or dist > r0 + r1:
print "Invalid position - Potentially outside of reach"
# raise Exception();
# Calculate middle point
a = (r0**2 - r1**2 + dist**2) / (2 * dist);
# Calculate the angle and return
new_angle = 180 * (math.atan2(c1.z, -c1.y + c0.y) + math.acos(a/r0)) / math.pi;
print "Calculated Angle: ", new_angle;
return new_angle;
