M100RAK Robotic Arm Kit User Guide
RobotShop Inc. www.RobotShop.com
Calculating Angles Based on x, y, z
This is just one way of calculating the joint angles based on coordinate
(x,y,z) of the end of the gripper. Note that the equations need to be
calculated in RADIANS (not degrees) and then converted back to degrees. We
also suggest including constraints to ensure the given point can be reached
by the arm.
L is the length of the shoulder axis to the elbow axis
M is the length of the elbow axis to the wrist axis
N is the length of the wrist axis to the end of the gripper (or the desired
point)
R = [(x^2) + (y^2)]^(1/2)
Represents the radius from the axis of rotation of the base to x,y
s = R N
Since the arm has four degrees of freedom, there are infinite solutions
possible for the arm to reach point (x,y,z). We will therefore introduce an
artificial constraint and keep the gripper at a specific angle to the
horizontal, and calculate for a new coordinate (x
1
, y
1
, z
1
) of the wrist axis.
Q = [(s^2) + (z^2)]^(1/2)
This is the distance between the shoulder axis and the wrist axis
f = atan2(z, s)
This is the angle between the horizontal and the line Q. The atan function
would return two angles whereas the atan2 function determines the correct
angle based on the x and y coordinate. The actual height is h + z which can
be taken into account when inputting
g = acos[ ((L^2)+(Q^2)-(M^2)) / (2*L*Q)]
This is the angle between line Q and link L using the law of cosines.
Use the equations above to find angles a, b, c and d:
a = f + g
This is angle a aboe.
b = acos[((M^2)+(L^2)-(Q^2)) / (2*L*M)]
This is angle b aboe using the law of cosines.
c = -b - a + 2*pi
This is angle c. Angle c is kept horizontal to the (x,y) plane.
d = math.atan2(x, y)
This is he angle d of he base.