def forward_kinematics_spherical_wrist(joint_values, points):
"""Forward kinematics function for spherical wrist robots.
Parameters
----------
joint_values : list of float
A list of 6 joint values in radians.
points : list of point
A list of 4 points specifying the robot's joint positions.
Returns
-------
:class:`compas.geometry.Frame`
Notes
-----
This is a modification of the *Lobster* tool for solving the inverse
kinematics problem for a spherical wrist robots.
https://www.grasshopper3d.com/forum/topics/lobster-reloaded?groupUrl=lobster&
This is the instruction on how to determine the 4 points.
http://archive.fabacademy.org/archives/2017/fablabcept/students/184/assets/lobster_ik.pdf
Check that p2, p3, and p4 are all in the XZ plane (y coordinates are zero)!
"""
p1, p2, p3, p4 = points
a1, a2, a3, a4, a5, a6 = joint_values
af1 = Frame.worldZX()
af1.point = p1
af2 = Frame.worldZX()
af2.point = p2
af3 = Frame.worldYZ()
af3.point = p3
af4 = Frame.worldZX()
af4.point = p3
af5 = Frame.worldYZ()
af5.point = p3
af6 = Frame.worldYZ()
af6.point = p4
axis_frames = [af1, af2, af3, af4, af5, af6]
# TODO: this is slow
Rot1 = Rotation.from_axis_and_angle((0, 0, 1), -1 * a1)
for i in range(0, 6):
axis_frames[i].transform(Rot1)
Rot2 = Rotation.from_axis_and_angle(axis_frames[0].zaxis,
a2 + math.pi / 2,
point=axis_frames[0].point)
for i in range(1, 6):
axis_frames[i].transform(Rot2)
Rot3 = Rotation.from_axis_and_angle(axis_frames[1].zaxis,
a3 - math.pi / 2,
point=axis_frames[1].point)
for i in range(2, 6):
axis_frames[i].transform(Rot3)
Rot4 = Rotation.from_axis_and_angle(axis_frames[2].zaxis, a4 * -1.0,
axis_frames[2].point)
for i in range(3, 6):
axis_frames[i].transform(Rot4)
Rot5 = Rotation.from_axis_and_angle(axis_frames[3].zaxis, a5,
axis_frames[3].point)
for i in range(4, 6):
axis_frames[i].transform(Rot5)
Rot6 = Rotation.from_axis_and_angle(axis_frames[4].zaxis, -1.0 * a6,
axis_frames[4].point)
for i in range(5, 6):
axis_frames[i].transform(Rot6)
return axis_frames[5]
def forward_kinematics_spherical_wrist(p1, p2, p3, p4, joint_values):
"""Forward kinematics function for spherical wrist robots.
Parameters
----------
p1 : Point
p2 : Point
p3 : Point
p4 : Point
joint_values : list of float
List of 6 joint values in radians.
Returns
-------
:class:`compas.geometry.Frame`
"""
a1, a2, a3, a4, a5, a6 = joint_values
af1 = Frame.worldZX()
af1.point = p1
af2 = Frame.worldZX()
af2.point = p2
af3 = Frame.worldYZ()
af3.point = p3
af4 = Frame.worldZX()
af4.point = p3
af5 = Frame.worldYZ()
af5.point = p3
af6 = Frame.worldYZ()
af6.point = p4
axis_frames = [af1, af2, af3, af4, af5, af6]
Rot1 = Rotation.from_axis_and_angle((0, 0, 1), -1 * a1)
for i in range(0, 6):
axis_frames[i].transform(Rot1)
Rot2 = Rotation.from_axis_and_angle(axis_frames[0].zaxis,
a2 + math.pi / 2,
point=axis_frames[0].point)
for i in range(1, 6):
axis_frames[i].transform(Rot2)
Rot3 = Rotation.from_axis_and_angle(axis_frames[1].zaxis,
a3 - math.pi / 2,
point=axis_frames[1].point)
for i in range(2, 6):
axis_frames[i].transform(Rot3)
Rot4 = Rotation.from_axis_and_angle(axis_frames[2].zaxis, a4 * -1.0,
axis_frames[2].point)
for i in range(3, 6):
axis_frames[i].transform(Rot4)
Rot5 = Rotation.from_axis_and_angle(axis_frames[3].zaxis, a5,
axis_frames[3].point)
for i in range(4, 6):
axis_frames[i].transform(Rot5)
Rot6 = Rotation.from_axis_and_angle(axis_frames[4].zaxis, -1.0 * a6,
axis_frames[4].point)
for i in range(5, 6):
axis_frames[i].transform(Rot6)
return axis_frames[5]
