function [theta45, star45] = invwrist(T35)
%INVWRIST Inverse wrist kinematics for A150.
% [THETA45, STAR45] = INVWRIST(T35) returns the
% Denavit-Hartenberg parameters THETA45 (theta4 and
% theta5) given the pose of the end effector frame (frame 5)
% relative to the wrist center frame (frame 3). STAR45
% are the joint angles (theta4* and theta5*).
%
% If no solution exists then THETA45 and STAR45 will
% be empty.

theta5 = atan2(-T35(3, 1), -T35(3, 2)) * 180/pi;
theta4 = atan2(T35(1, 3), -T35(2, 3)) * 180/pi;

if abs(theta4 - 90) > 110
    theta45 = [];
    star45 = [];
    return;
end

theta45 = [theta4 (theta5 + 180)];
star45 = [(theta4 - 90) theta5];

