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]