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
]
