Method of moving and orienting a tool along a curved path

A method for controlling motion of a function element carried by a wrist of a manipulator and having associated therewith a workpoint and a tool vector, the motion being described by an arc defining a path to be traversed by the workpoint, by a desired line of approach of the tool vector, and by a velocity of the workpoint, the arc being defined by input signals representing coordinates of three reference positions on the arc including the arc end positions, the line of approach being defined by input signals representing the orientation of the wrist at the reference positions, the velocity of the workpoint being defined by further input signals, the manipulator including movable members for effecting motion of the wrist and a servomechanism circuit for controlling the motion of the movable members, the method comprising the steps of:

• (a) calculating work plane orientation angles in response to the input signals representing the arc reference positions, the work plane orientation angles defining the attitude of a plane in which the arc lies;
• (b) producing arc intermediate position signals in response to the work plane orientation angles and the input signals defining the arc reference positions and the velocity of the workpoint, the arc intermediate position signals representing an intermediate position on the arc to which the workpoint is to be moved in a predetermined time;
• (c) producing intermediate wrist orientation signals in response to the input signals representing the wrist orientation and the tool vector, the intermediate wrist orientation signals defining the attitude of the function element at the arc intermediate position;
• (d) producing machine coordinate signals in response to the intermediate wrist orientation signals and the arc intermediate position signals, the machine coordinate signals defining the motion of the machine movable members between their current positions and the positions defined by the arc intermediate position signals and the intermediate wrist orientation signals;
• (e) applying the machine coordinate signals to the servomechanism circuit to move the work point to the arc intermediate position and to move the function element to the attitude defined by the intermediate orientation signal;
• and
• (f) iterating the steps (b) through (e) to move the workpoint through the arc while maintaining the tool vector on the desired line of approach.

A method for controlling motion of a function element mounted to a faceplate of a wrist of a manipulator and having associated therewith a workpoint and a tool vector, the motion being described by an arc defining a path to be traversed by the workpoint, by a desired line of approach of the tool vector, and by a velocity of the workpoint, the arc being defined by input signals representing coordinates of three reference positions on the arc including the arc end positions, and further input signals defining the desired line of approach at each arc reference position, the velocity of the workpoint being defined by further input signals, and the tool vector being defined by further input signals, the manipulator including movable members for effecting motion of the wrist and a servomechanism circuit for controlling the motion of the movable members, the method comprising the steps of:

• (a) calculating work plane orientation angles in response to the input signals representing the arc reference positions, the work plane orientation angles defining the attitude of a plane in which the arc lies;
• (b) producing process angle signals in response to the input signals defining the line of approach of the tool vector and the input signals defining the tool vector and in response to the work plane orientation angles, the process angle signals defining the attitude of the desired line of approach of the tool vector at each reference position;
• (c) producing arc intermediate position signals in response to the input signals defining the reference positions and the velocity of the workpoint and in response to the work plane orientation angles, the arc intermediate position signals representing an intermediate position on the arc to which the workpoint is to be moved in a predetermined time;
• (d) producing intermediate wrist orientation signals in response to the process angle signals and the work plane orientation angles, the intermediate wrist orientation signals defining the attitude of the tool vector at the position represented by the arc intermediate position signals;
• (e) producing machine coordinate signals in response to the intermediate wrist orientation signals and the arc intermediate position signals, the machine coordinate signals defining the motion of the movable machine members between their current positions and the positions defined by the arc intermediate position signals and the intermediate wrist orientation signals;
• (f) applying the machine coordinate signals to the servomechanism circuit to move the workpoint to the arc intermediate position and to move the function element to the attitude defined by the intermediate wrist orientation signals;
• and
• (g) iterating steps (c) through (f) to move the workpoint through the arc while maintaining the tool vector on the desired line of approach.

A method for controlling motion of a function element mounted to a face plate of a wrist of a manipulator and having associated therewith a workpoint and a tool vector, the motion being described by an arc defining a path to be traversed by the workpoint, by a desired line of approach of the tool vector, and by a velocity of the workpoint, the arc being defined by input signals representing coordinates of three reference positions on the arc including the arc end positions, and further input signals representing process angles and tool vector orientation angles together defining the line of approach of the tool vector at the arc reference positions, the velocity of the workpoint being defined by further input signals, and the tool vector being defined by still further input signals, the manipulator including movable members for effecting motion of the wrist and a servomechanism circuit for controlling the motion of the movable members. The method comprising the steps of;

• (a) producing arc intermediate position signals in response to the input signals defining the reference positions and the velocity of the workpoint, the arc intermediate position signals representing an intermediate position on the arc to which the workpoint is to be moved in a predetermined time;
• (b) producing intermediate wrist orientation signals in response to the input signals representing process angles and the arc intermediate position signals, the intermediate wrist orientation signals defining the attitude of the function element at the position represented by the arc intermediate position signals, the intermediate wrist orientation signals being produced by;
• (i) calculating process angles at the intermediate position;
• (ii) calculating a process angle orientation matrix in response to the process angles, the process angle orientation matrix defining a transformation from a frame of reference at the workpoint and including the desired line of approach to the frame of reference at the workpoint parallel to a coordinate system from which the input signals are referenced;
• (iii) calculating a tool vector orientation matrix in response to the input signals representing tool vector orientation angles, the tool vector orientation matrix defining a transformation from a frame of reference at the workpoint and including the tool vector to a frame of reference at the workpoint parallel to the coordinate system from which the input signals are referenced;
• (iv) calculating a wrist orientation matrix in response to the process angle orientation matrix and the tool vector matrix;
• and
• (v) calculating the wrist orientation angles in response to the wrist orientation matrix;
• (c) producing machine coordinate signals in response to the intermediate wrist orientation signals and the arc intermediate position signals, the machine coordinate signals defining the motion of the machine members between their current positions and the positions defined by the arc intermediate position signals and the intermediate wrist orientation signals;
• (d) applying the machine coordinate signals to the servo mechanism circuit to move the workpoint to the arc intermediate position and to move the function element to the attitude defined by the intermediate wrist orientation signals;
• and
• (e) iterating steps (a) through (d) to move the workpoint through the arc while maintaining the tool vector on the desired line of approach.

A method for controlling motion of a function element mounted to a face plate of a wrist of a manipulator and having associated therewith a workpoint and a tool vector, the motion being described by an arc defining a path to be traversed by the workpoint, by a desired line of approach of the tool vector, and by a velocity of the workpoint, the arc being defined by input signals representing coordinates of three reference positions on the arc including the arc end positions, and further input signals representing orientation angles defining the line of approach of the tool vector at the arc reference positions, the velocity of the workpoint being defined by further input signals, and the tool vector being defined by still further input signals, the manipulator including movable members for effecting motion of the wrist and a servomechanism circuit for controlling the motion of the movable members, the method comprising the steps of:

• (a) producing process angle signals representing angles of the desired line of approach of the tool vector at each reference position by:
• (i) calculating a wrist orientation matrix in response to the input signals representing the orientation angles, the wrist orientation matrix defining a transformation from a frame of reference at the workpoint and including an axis parallel to a normal to the face plate to a frame of reference at the workpoint parallel to a coordinate system from which the input signals are referenced;
• (ii) calculating a tool vector orientation matrix in response to the input signals defining the tool vector, the tool vector orientation matrix defining a transformation from a frame of reference at the workpoint and including the tool vector to a frame of reference at the workpoint parallel to the coordinate system from which the input signals are referenced;
• (iii) calculating a process angle orientation matrix in response to the wrist orientation matrix and the tool vector orientation matrix;
• (iv) calculating the process angles in response to the process angle orientation matrix;
• and
• (v) iterating the steps (i) through (iv) for each arc reference positions;
• (b) producing arc intermediate position signals in response to the input signals defining the reference positions and the velocity of the workpoint, the arc intermediate position signals representing an intermediate position on the arc to which the workpoint is to be moved in a predetermined time;
• (c) producing intermediate wrist orientation signals in response to the process angle signals and the arc intermediate position signals, the intermediate wrist orientation signals defining the attitude of the function element at the positions represented by the arc intermediate position signals;
• (d) producing machine coordinate signals in response to the intermediate wrist orientation signals and the arc intermediate position signals, the machine coordinate signals defining the motion of the machine members between there current positions and the positions defined by the arc intermediate position signals and the intermediate wrist orientation signals;
• (e) applying the machine coordinate signals to the servo mechanism circuit to move the workpoint to the arc intermediate position and to move the function element to the attitude defined by the intermediate wrist orientation signals;
• and
• (f) iterating steps (b) through (e) to move the workpoint through the arc while maintaining the tool vector on the desired line of approach.

An apparatus for controlling motion of a function element mounted to a faceplate of a wrist of a manipulator and having associated therewith a workpoint and a tool vector, the motion being described by an arc defining a path to be traversed by the workpoint, by a desired line of approach of the tool vector and by a velocity of the workpoint, the arc being defined by input signals representing coordinates of three reference positions on the arc including the arc end positions, the line of approach being defined by further input signals, the velocity of the workpoint being defined by further input signals, the manipulator including movable members for effecting motion of the wrist and a servomechanism circuit for controlling the motion of the movable members, the apparatus comprising:

• (a) means for calculating work plane orientation angles in response to the input signals representing the arc reference positions, the work plane orientation angles defining the attitude of a plane in which the arc lies;
• (b) means for producing arc intermediate position signals in response to the input signals defining the arc reference positions, the velocity of the workpoint, and the work plane orientation angles, the arc intermediate position signals representing an intermediate position on the arc to which the workpoint is to be moved;
• (c) means for producing machine coordinate signals in response to the arc intermediate position signals, the machine coordinate signals defining the motion of the machine movable members between their current positions and the positions defined by the arc intermediate position signals.