The mathematics#
The BrachioGraph object contains two trigonometric methods, to translate x/y co-ordinates of the pen into angles of the
motors and vice-versa. Using the example illustrated below, the arms are both 9cm long and the pen is at x=4, y=10
.
Translating co-ordinates to angles#
hypotenuse = math.sqrt(x**2+y**2)
if hypotenuse > self.inner_arm + self.outer_arm:
raise Exception(f"Cannot reach {hypotenuse}; total arm length is {self.inner_arm + self.outer_arm}")
hypotenuse_angle = math.asin(x/hypotenuse)
inner_angle = math.acos(
(hypotenuse**2+self.inner_arm**2-self.outer_arm**2)/(2*hypotenuse*self.inner_arm)
)
outer_angle = math.acos(
(self.inner_arm**2+self.outer_arm**2-hypotenuse**2)/(2*self.inner_arm*self.outer_arm)
)
shoulder_motor_angle = hypotenuse_angle - inner_angle
elbow_motor_angle = math.pi - outer_angle
return (math.degrees(shoulder_motor_angle), math.degrees(elbow_motor_angle))

The xy_to_angles()
method receives x and y co-ordinates as arguments. First we find a line from the origin (the
shoulder motor) to the pen, and its angle from the y-axis:
hypotenuse = math.sqrt(x ** 2 + y ** 2)
hypotenuse_angle = math.asin(x/hypotenuse)
Given x=4, y=10
, hypotenuse
is 10.77, and its angle from the y-axis (hypotenuse_angle
) is 21.8 degrees
(0.38 radians).
The hypotenuse line, the inner arm and the outer arm form a second triangle. All their lengths are known, so we can find the angle between the line of the hypotenuse of the first triangle and the inner arm:
inner_angle = math.acos(
(hypotenuse ** 2 + self.inner_arm ** 2 - self.outer_arm ** 2) / (2 * hypotenuse * self.inner_arm)
)
which is 53.25 degrees. The hypotenuse_angle
minus the inner_angle
gives us the angle of the shoulder motor
(from the y-axis):
shoulder_motor_angle = hypotenuse_angle - inner_angle
in other words, -31.45 degrees. So now we know what angle to set the shoulder motor to.
And similarly, we can find the angle at the elbow, between the inner and outer arms:
outer_angle = math.acos(
(self.inner_arm ** 2 + self.outer_arm ** 2 - hypotenuse ** 2) / (2 * self.inner_arm * self.outer_arm)
)
The angle of the outer arm relative to the inner arm is 180 degrees (or π radians) minus the outer_angle
:
elbow_motor_angle = math.pi - outer_angle
Finally we convert the angle values to degrees and return them:
return (math.degrees(shoulder_motor_angle), math.degrees(elbow_motor_angle))
Translating angles to co-ordinates#
Obtaining angles from co-ordinates is essentially the reverse process, in angles_to_xy()
. This method isn’t
actually used in the BrachioGraph
, but can be useful when experimenting.