Patent Document ID: 4680519
Application ID: 06778830
Patent Flag: 1

Claim One:
1. In an articulated manipulator of the type having at least a first, a second, and a third link respectively associated with a first, a second, and a third axis of rotation of motion about an offset wrist, each link and motion being associated with a first, a second, and a third joint, a method of determining a first, a second, and a third joint angle, respectively, for each of the axis of rotation to provide control data for activating actuators being associated with a corresponding joint, to permit positioning of an end of the manipulator at a desired point, said method comprising the steps of: (A) obtaining data identifying a desired end point position and orientation, the data including an end point position vector and an end point approach orientation vector; (B) multiplying the end point approach orientation vector by the length of the first link; (C) producing a first set of coordinates, by subtracting the result of step (B) of multiplying from the end point position vector; (D) determining a first vector from the first set of coordinates along the first axis of rotation with a length of the first link; (E) setting initially a second set of coordinates equal to the first set of coordinates; (F) determining from the second set of coordinates a second vector along the third axis of rotation, with a length of the third link; (G) calculating the cross-product of the first vector and second vector, to produce a third vector; (H) multiplying the third vector by the length of the second link; (I) computing an estimate of the second set of coordinates by subtracting the result of step (H) of multiplying, from the first set of coordinates; (J) repeating steps (F) of determining, through steps (I) of computing, until the difference between any two successive estimations is less than a predetermined minimum value; (K) determining said joint angles from the second set of coordinates; (L) generating position commands from said joint angles; and (M) sending the position commands to the actuator associated with a manipulator joint to control the manipulator joint.