Robot System Employing Force/Position Control

The present invention relates to industrial robotic systems of the type including a robot device, which is typically six-jointed, and computer-driven control and displacement units connected for effecting rotation of each joint.

Many industrial assembly operations require that one part contact and then slide with respect to a second part until the plane of contact changes abruptly and the first part drops into place in the second part. This kind of task is easily accomplished if the one part is restricted to straight line motion with a constant force applied to this part in the direction of the straight line motion. This is an example of force/position control in which force is controlled in a given direction and position is controlled in a plane perpendicular to the force direction.

This type of control is simple to achieve with a cartesian coordinate robot when the straight line motion is aligned with one of the robot axes. However, with jointed robots, force/position control is more difficult since in general all six joints must be controlled simultaneously in order to effect straight line motion of the robot manipulator.

The basic components of a six-jointed robot are illustrated in simplified form in FIG. 1. This device includes a fixed base 2, five linking members 4, 6, 8, 10 and 12 and an end member 14 which carries or guides the part to be assembled and is normally provided with some type of gripping mechanism.

Base 2 and members 4, 6, 8, 10, 12 and 14 are coupled together by pivot joints 21, 22, 23, 24, 25 and 26 each having a joint motor. Pivot joints 22, 23 and 25 have transverse axes and joints 21, 24 and 26 have longitudinal axes.

Each member can thus be pivoted about a respective joint axis from a reference angular position, the angular deviation from the reference position being known as the joint angle for that joint. The joint angles for joints 21-26 are designated .theta..sub.1, .theta..sub.2, .theta..sub.3, .theta..sub.4, .theta..sub.5 and .theta..sub.6.

The precise position and orientation of member 14 is a function of the six joint angles. The position of member 14 can be defined in terms of the x, y and z coordinates of a reference point on member 14 relative to a fixed coordinate system. The orientation of member 14 can be defined by the orientation of a selected plane of member 14 containing the reference point and in terms of three Euler angles .phi., .theta. and .PSI. which designate the orientation of the selected plane relative to the fixed coordinate system. These angles are designated on the basis of the orientation of a local coordinate system associated with the selected plane relative to the fixed coordinate system, as will be explained with reference to FIGS. 2A-2D in which the reference point on member 14 is designated R.

In FIG. 2A, a fixed plane 52 parallel to the x-y plane of the fixed coordinate system is congruent with the selected plane 54 of member 14. Plane 54 is associated with coordinates x', y', z' congruent with fixed coordinates x, y, z, respectively. Plane 54 can have any orientation relative to plane 52, defined by angles .phi., .theta. and .PSI.. If, to arrive at the final orientation shown in FIG. 2D, plane 54 is rotated about congruent axes z, z', an angle .phi. is formed between axes x and x'. Then rotation of plane 54 about axis x' forms angle .theta. between axes z and z'. Finally, rotation of plane 54 about axis z' forms angle .PSI. between axis x' and the line of intersection of planes 24 and 54.

Each set of values for x, y, z, .phi., .theta., .PSI. of the reference point R and plane 54 of member 14 produces a unique set of values for the coordinates .theta..sub.1 -.theta..sub.6.

Force/position control is a current research topic in robot control. A typical method which has been proposed is as follows. A projection operator P.sub.F defines the direction in which one wants to control the force and a complementary projection operator P.sub.P defines the directions in which one wants to control position. A feedback loop is then implemented in that sensors measure the six joint angles .theta..sub.1, .theta..sub.2, .theta..sub.3, .theta..sub.4, .theta..sub.5, and .theta..sub.6, and a nonlinear transformation on these angles is performed to yield the three position coordinates x, y and z and the three Euler angles .phi., .theta., and .PSI.. Three angle corrections

are computed where .phi..sub.R, .theta..sub.R and .PSI..sub.R are desired or setpoint values. Three position corrections

are also computed where x, y and z are actual coordinate values and x.sub.R, y.sub.R and z.sub.R denote a point on the straight line of desired motion. Then ##EQU1## defines the desired projection of the position correction. A force error ##EQU2## is also computed using the measured force vector, the desired force vector and the projection operator. PID (Proportional, Integral and Derivative) controllers are applied to the six error quantities .DELTA.f, .DELTA.u, .DELTA.v, .DELTA..phi., .DELTA..theta. and .DELTA..PSI. to produce six correction signals e.sub.1, e.sub.2, e.sub.3, e.sub.4, e.sub.5 and e.sub.6. A second nonlinear transformation is then applied to these six signals to produce correction current signals .DELTA.I.sub.1, .DELTA.I.sub.2, .DELTA.I.sub.3, .DELTA.I.sub.4, .DELTA.I.sub.5 and .DELTA.I.sub.6 to the joint motor armatures.

One of the main difficulties with the approach just described is the large amount of computation needed to evaluate the projection operators P.sub.F and P.sub.P as well as the two nonlinear transformations at each time step. The method also ignores additional dynamic coupling among the joints induced by the object, held by the robot gripper, and contacting a hard surface with a certain normal.

It is an object of the present invention to effect an improved force/position control for a jointed robot which is to execute a straight line motion.

A further object of the invention is to reduce the computation required for such control.

Another object of the invention is to provide some decoupling of the feedback paths when the object being positioned by the robot arm contacts a hard surface.

The above and other objects are achieved, according to the present invention, in a robotic assembly system including a robotic device composed of a base member, a plurality of movable members and joint means coupling the members in a sequence extending from the base member such that a first one of the movable members is supported by the base member and is rotatable relative to the base member about a respective axis and every other one of the movable members is supported by, and rotatable about a respective axis relative to, a respective preceding movable member in the sequence, with the last movable member in the sequence being an end member for supporting a workpiece which is to be brought to a selected position relative to a fixed body by movement along a reference plane associated with the body, the selected position corresponding to a set of reference angular positions each of which defines an angular position of a respective movable member about the respective axis thereof, computer driven displacement means coupled to the joint means for rotating each movable member about the respective axis thereof in a manner to bring the workpiece to the selected position while displacing the workpiece generally along the selected plane and pressing the workpiece against the selected plane with a force approaching a selected value, first monitoring means associated with the end member and coupled to the displacement means for producing a signal representative of the force with which the workpiece is being pressed against the reference surface, and second monitoring means coupled to the joint means and the displacement means for providing signals indicating the current angular position of each movable member about the respective axis thereof, by the improvement wherein the displacement means comprise means coupled to the joint means for effecting rotation of only one of the movable members in direct response to the signal produced by the first monitoring means.

FIG. 1 is a simplified elevational view of a robot arm of the type to which the present invention is applied.

FIGS. 2A-2D are diagrams illustrating the establishment of a coordinate system defining the position and orientation of a robot end member.

FIG. 3 is a block diagram of a unit of a position control system for controlling the robot of FIG. 1.

FIG. 4 is a linkage diagram illustrating control of robot movement according to the present invention.

FIG. 5 is a block diagram of a robot control system according to the present invention.

FIGS. 6A-6D are block diagrams of components of the system of FIG. 5.

FIG. 7 is a vector diagram illustrating control techniques employed according to the present invention.

FIG. 8 is a vector diagram illustrating further aspects of the control technique according to the present invention.

FIG. 3 shows a typical position control subsystem for one joint. Each joint is controlled by a PID controller 30 that servos each joint angle .theta..sub.i to its respective reference angle value .theta..sub.iR using feedback. PID controller 30 is usually implemented by a microprocessor computer operating at a high sampling rate of 1000 Hz or higher. Operating at a much lower rate and in an open loop fashion is a supervisory controller (not shown) that converts the desired values for the position and orientation coordinates x, y, z, .phi., .theta., and .PSI. (not shown) to joint reference coordinates, or angles, .theta..sub.1R, .theta..sub.2R, .theta..sub.3R, .theta..sub.4R, .theta..sub.5R and .theta..sub.6R.

The supervisory controller is implemented by another microprocessor which computes the nonlinear transformation needed to cause the object carried by the robot to follow a path dictated by time varying world, or fixed, coordinates. The amount of computation involved in the nonlinear transformations usually determines the lower bandwidth of the output signals from the supervisory controller.

In order to give smooth operation, each joint controller 30 interpolates the control signals issued by the supervisory controller.

Controller 30 is followed by an amplifier 32 which supplies an armature current signal I.sub.AD to the associated joint motor and attached robot member 34. The position .theta..sub.i of the associated joint is then fed back to controller 30.

The invention builds on this conventional control design in a simple way: An automatically selected joint is freed from position control and instead is servoed to force control. Displacements of this selected joint from a reference angle generated first and second order position corrections for the other five joints to insure straight line motion of the gripped object.

FIG. 4 helps to illustrate how one of the joints is selected for force control. For simplicity, a two dimensional model is used for a robot possessing only two joints, 41 and 42. Point P is the contact point between an object gripped by robot end member 14 (FIG. 1) and a mating part. Surface 44 is the constraint surface which limits the motion of the gripped object for small displacements of joints 41 and 42. Surface 44 has a normal vector N. Line L is the desired straight line movement direction of the gripped part under position/force control. With joint 42 locked, motion about joint 41 generates arc A1 possessing tangent vector V1 and point P. Similarly, arc A2 with tangent vector V2 at point P is generated if joint 41 is locked and joint 42 is allowed to move. The joint whose tangent vector forms the smallest angle with the surface normal vector N is selected as the joint to be freed from position control and placed under force control. Optimum implementation of the invention is achieved when line L is aligned with N.

From FIG. 1 it is seen that the arcs generated by joints 21, 22 and 23 have much less curvature (due to longer radii) than those generated by wrist joints 24, 25 and 26. Thus in a practical system using a robot similar in geometry to that of FIG. 1, one would normally select the force controlled joint from among joints 21, 22 and 23. Choosing these joints permits the closest approximation to straight line motion using the control scheme described here.

FIG. 5 shows an embodiment of a PID controller subsystem for any selected one of joints 21, 22 or 23 when force control is implemented according to the invention. A motor in unit 34 is driven by the desired armature current I.sub.AD produced by amplifier 32 operating on a signal I.sub.AD '. Signal I.sub.AD ' is selected by a mode selector 56, which can be a signal-controlled switch, to be either a signal I.sub.P if the joint is under position control or a signal I.sub.F if the joint is under force control. Signal M from a master controller sets the particular mode of operation for each selected joint.

The PID position controller 30 for joint i operates in a conventional way in response to comparison of the actual angle .theta.i with the reference angle .theta..sub.iR biased by a signal .DELTA..theta..sub.iR via an adder 58. The production of the signal .DELTA..theta..sub.iR for each joint is one of the main elements of this invention. There is a position biasing component 60 for each joint which produces .DELTA..theta..sub.iR as a function of a signal .DELTA..theta. which is the difference between .theta..sub.i.sbsb.f.sub.R and .theta..sub.i.sbsb.f where i.sub.f is the index, or identity, of the joint under force control. For each joint ##EQU3## since

Position controller 30 also is constructed to produce the signal

For i=i.sub.f this reduces to .theta..sub.iR -.theta..sub.i since .DELTA..theta..sub.iR is zero. The coefficients A.sub.i and B.sub.i which are set by a signal C are a function of both the joint index and the reference angles .theta..sub.iR of all of the joints. The signals .DELTA..theta..sub.iR bias the angle reference positions of all of the position controlled joints so that a straight line is followed by the gripped object when the force controlled joint moves.

For wrist joints 24, 25 and 26, mode selector 56 is absent since these joints are always position controlled. When a joint is force controlled, the mode selector sets I.sub.AD ' to I.sub.F and disconnects the PID position controller.

where I.sub.F ' is the output of a PI force controller 62 and I.sub.Bias is a constant. I.sub.F ' and I.sub.Bias are combined in an adder 64 just before a joint is switched from position to force control, a command S from the master controller causes the current value of I.sub.AD of the selected joint to be sampled by a sample and hold circuit 66 so that I.sub.Bias is set equal to this sampled value and held at this value.

The force controller acts on a difference value .DELTA.f-f.sub..omega. formed by an adder 68. A velocity restraint circuit 70 operates on .theta..sub.i to produce f.sub..omega. whenever a threshold value .omega..sub.o for the joint is exceeded. This negative feedback signal f.sub..omega. limits the velocity of rotation of the joint. The limit .omega..sub.o is set by the master controller.

where f.sub.R is a reference value and f.sub.m is a filtered force version of a measured contact force value f.sub.m. Measured contact force value f.sub.m is produced by a force sensor associated with end member 14 or its gripping mechanism. An adaptive clipping and filtering circuit 74 operates on the measured force f.sub.m to produce the signal f.sub.m. Circuit 74 receives a signal F.sub.o which is a force limit on the amount of clipping and is sent by the master controller. Signal f.sub.m is sent as a negative signal to an adder 76 which also receives f.sub.R and produces signal .DELTA.f that is sent to a joint selector 78. A joint selector signal JS from the master controller is coordinated with the mode selector M so that .DELTA.f is sent to the joint under force control via joint selector 78.

Details of the different components of FIG. 5 are shown in FIG. 6. FIG. 6A shows three components that make up the spike removal and low pass filtering circuit 74. A low pass filter 80 operates on the measured contact force f.sub.m to produce a low pass signal f.sub.L. An adder 82 subtractively combines f.sub.m and f.sub.L to form a high pass signal

which is then operated on by a clipping circuit 84. A clipping limit F.sub.o is set by the master controller. An adder 86 forms a signal

where f.sub.c is the output of clipping circuit 84. Hence ##EQU4## As long as instantaneous changes in f.sub.m do not exceed F.sub.o, f'.sub.m equals f.sub.m at all frequencies. When spikes occur, changes in f'.sub.m are limited to F.sub.o. The clipping circuit is necessary since at the moment of contact between two components that are being mated a large change in force is likely to be sensed by a force torque sensor on the wrist of the robot. Finally filter 88 operates on f'.sub.m to produce low pass filtered signal f.sub.m. Filter 88 reduces the bandwidth of the force feedback path and helps to decouple interactions between the position feedback paths and the force feedback path.

The velocity restraint circuit 70 is shown in FIG. 6B and is composed of two components. The circuit 90 described by the Laplace transform operator s/(1+Ts) operates on .theta..sub.i to produce a smoothed velocity signal .omega.. The next component 92 has an adjustable dead band specified by .omega..sub.o from the master controller. The output signal from the high gain adjustable dead band circuit 92 f.sub..omega. and is used to apply high negative feedback to the joint when .omega..sub.o is exceeded.

FIG. 6C shows the position bias processing circuit 60 in which equation 5a is implemented for each joint under position control. Such a circuit provides second order correction to the position setting of the five joints as a function of the single force controlled joint. Each pair (A.sub.i, B.sub.i) is computed specifically for each joint i. The master controller sets these coefficients by the signal C.

The circuit shown in FIG. 6D produces difference signal .DELTA..theta.. The joint selector signal JS from the master controller is applied to a joint selector 94 to select the force controlled joint from among .DELTA..theta..sub.1, .DELTA..theta..sub.2 or .DELTA..theta..sub.3 to produce .DELTA..theta.'. Then an adjustable filter 96 described by the transfer function G(s) operates on .DELTA..theta.' to produce .DELTA..theta.. A signal K from the master controller supplies the coefficients that determine the rational transfer function G(s). Adjustable filter 96 is used to decouple a certain mode of interaction between the force controlled joint and the position controlled joints.

The master controller receives from a higher lever controller the desired part position and orientation signals x, y, z, .phi., .theta. and .PSI.. Also desired straight line parameters x.sub.p, y.sub.p, zp, .phi..sub.p and .omega..sub.p are received from a higher level controller. From the robot, the master controller receives the joint signals .theta..sub.1, .theta..sub.2, .theta..sub.3, .theta..sub.4, .theta..sub.5 and .theta..sub.6.

For both all joint position control and position/force control, the master controller sends out signals representing the reference angles .theta..sub.1R, .theta..sub.2R, .theta..sub.3R, .theta..sub.4R, .theta..sub.5R and .theta..sub.6R. It updates the coefficient sets {A.sub.i } and {B.sub.i } as well as the parameters in set {K} for G(s) at each major time step. When switching the modes of control or changing the force controlled joint, it sends out signals F.sub.o, .omega..sub.o, JS, set {M} and set {S} to the appropriate joint controller components.

The nonlinear joint transformations from world coordinates to joint coordinates are the same as used by the known position only joint control systems. The first computation done by the master controller to implement the invention is the selection of the force controlled joint in response to a command from the higher level controller to switch from position control to position/force control.

Referring to FIG. 7, a line can be specified in a number of ways. For example, a vector

uses a fixed point described by a vector V.sub.o through which the line passes and a direction vector .DELTA.V.sub.o giving the direction of the line. Vector V is a point on the line for a specific value of the single free parameter .alpha.. Equation 13 needs six parameters to specify the line, three for V.sub.o and three for .DELTA.V.sub.o. The minimum number of parameters to specify a straight line is five when vector .DELTA.V.sub.o is a unit length vector specified by two angles such that

where .DELTA.x.sub.o, .DELTA.y.sub.o, and .DELTA.z.sub.o, are the components of .DELTA.V.sub.o. Hence, xp, yp, zp give V.sub.o and .phi.p, .PSI.p give .DELTA.V.sub.o according to equations 14, 15 and 16 to specify the desired straight line motion of the part. The normal vector of the constraint surface is N with components x.sub.N, y.sub.N and z.sub.N. For most assembly operations N would be aligned with .DELTA.V.sub.o.

In order to select the most appropriate joint, a number of vector operations must be performed by the master controller involving the geometry of the robot. Let

R.sub.i be the unit vector that is: (a) tangent, at contact point P, to the arc described by rotation of point P about the axis of joint i with all other joints locked; and (b) perpendicular to a line parallel to that axis; where i is the index of one of the joints which may be placed under force control; and

i.sub.f be that index for which the vector dot product of R.sub.i with N has the maximum value, the vector dot product being:

(Magnitude of R.sub.i).times.(Magnitude of N).times.the cosine of the angle between R.sub.i and N.

This is the index of that joint whose pivotal movement causes the path of P to be closest to N.

The signal JS is then produced to implement force control on joint i.sub.f.

Next a reference point must be chosen. This is done by the master controller by first obtaining the most recent values for .theta..sub.1, .theta..sub.2, .theta..sub.3, .theta..sub.4, .theta..sub.5 and .theta..sub.6. Then, using a nonlinear transformation involving the geometry of the robot and these angles, the present value x'.sub.p, y'.sub.p, z'.sub.p of the fixed point in the gripped object is computed. As shown in FIG. 7, this present value is represented by point P' which is defined by vector V'.sub.p having components x'.sub.p, y'.sub.p and z'.sub.p. In general V'.sub.p, will not lie on the desired straight line L described by V.sub.o and .DELTA.V.sub.o. It is desired to translate P back to the straight line L while remaining in a plane having normal N and passing through the V'.sub.p. Since the equation of such a plane is

where x.sub.N, y.sub.N and z.sub.N are the components of normal vector N and x.sub.L, y.sub.L and z.sub.L are the components of vector V.sub.L, and since point V.sub.L must also satisfy equation 13, one finds ##EQU5## and

gives the desired point on the straight line. Using the components of V.sub.L, namely x.sub.L, y.sub.L, z.sub.L and the angles .phi., .theta. and .PSI. of the desired pose, or orientation, as defined earlier herein, the master controller can compute updated values of .theta..sub.1R, .theta..sub.2R, .theta..sub.3R, .theta..sub.4R, .theta..sub.5R and .theta..sub.6R by the nonlinear joint transformations.

As previously indicated, when the fixed, or reference, point of the gripped object is point P, described be vector V.sub.L, a unique solution results for .theta..sub.1R, .theta..sub.2R, .theta..sub.3R, .theta..sub.4R, .theta..sub.5R and .theta..sub.6R.

FIG. 8 shows two points along line L which are located at respectively opposite sides of point P, are spaced from point P by respective vector distances .epsilon..DELTA.Vo and -.epsilon..DELTA.Vo, and are described by respective vectors V.sub.La and V.sub.Lb, where:

The nonlinear joint transformations can be applied to points V.sub.La and V.sub.Lb by the master controller to form shifted joint reference angles .theta..sub.iRa and .theta..sub.iRb. It is assumed that the two points described by V.sub.La and V.sub.Lb are selected such the 0<.epsilon.<<1.

Then, for each of the points spaced along line L from P, there can be computed, for each joint, reference angle difference values:

With the joint under force control being i.sub.f, the master controller computes, for each joint i.noteq.i.sub.f : ##EQU6## while

These coefficients A.sub.i, B.sub.i implement a second order correction in the set points .theta..sub.iR for i.noteq.i.sub.f as .theta..sub.i.sbsb.f varies about its reference angle so that the point P traces a curve that differs from a straight line in proportion to (.DELTA..theta..sub.i.sbsb.f).sup.3.

The last computation done is the evaluation of a coefficient set {K} for each joint i describing the associated rational transfer function G(s). This transfer function serves to decouple the force controlled joint from each position controlled joint. The coupling between the sets for two such joints can be explained as follows. When the gripped object is constrained to move on a plane having a certain normal N, motion of the position controlled joints results in movement of the force controlled joint. Motion in the force controlled joint changes .DELTA..theta..sub.i.sbsb.f which in turn closes a feedback loop by implementing position corrections .DELTA..theta..sub.iR, i.noteq..sub.f, which activate all the PID position controllers. Using only linear terms, let H(s) be the transfer function relating a change in .DELTA..theta. to a change in .DELTA..theta..sub.i.sbsb.f due to the five joints on position control and the planar constraint on point B.

But

so that a feedback path is clearly evident. Combining equations (25) and (26) yields: ##EQU7## The system can be decoupled by requiring the transfer function in equation 27 to be constant at all frequencies. In steady state G(0)=1 so that one obtains the constraint ##EQU8## as the desired transfer function. It is easy to show that with this definition of G(s) the poles of the system with loop gain G(s)H(s) are the poles of H(s). Hence closing the feedback loop through G(s) does not change the system dynamics.

In general the transfer function H(s) will be high order; a low frequency approximation to H(s) can be made for the purpose of computing G(s).

Let ##EQU9## be a low frequency rational approximation to H(s). Substitution of equation 30 into 29 yields: ##EQU10## The master controller selects a suitable approximation to H(s) from a stored set of approximations as a function of I.sub.f and point P and then computes the c.sub.i 's using equation 32. Signal set {K} consists of sets [b.sub.i ], [c.sub.i ] which are sent by the master controller to adjustable filter 96 of FIG. 6d.

A practical embodiment of the invention could employ a Unimation brand Model 560 data processing system as the control system and a Puma brand robot arm as the robot device.

Summary of Variables associated with robotic system:

x, y, z, .phi., .theta., .PSI. define the desired position and orientation, or pose, of the reference point on the gripped object. These are computed in advance on the basis of the physical dimensions of the parts to be mated and are supplied to the master controller;

x.sub.p, y.sub.p, z.sub.p, .phi..sub.p, .PSI..sub.p define the straight line along which force control is to be applied. They are computed in advance on the basis of the physical dimensions of the parts to be mated and are supplied to the master controller;

.theta..sub.1, .theta..sub.2, . . . .theta..sub.6 are the actual robot joint angles which are indicated by angle encoders connected to each joint axis;

.theta..sub.1R, .theta..sub.2R, . . . .theta..sub.3R, are the desired joint angles, corresponding to the desired position and pose of the reference point on the gripped object. These angles are computed by the master controller based on the robot-specific nonlinear transformation on V.sub.L ;

{A.sub.i }, {B.sub.i } are computed by the master controller using equations 20-24;

Fo, w.sub.o are supplied to the master controller, which outputs them to appropriate components of the control system;

{a.sub.i }, {b.sub.i } computed in advance by robot manufacturer;

JS is computed by the master controller based on equations 15 and 16;

M, S are generated by the master controller as a function of JS, each time JS changes state.

Major advantages of the invention over conventional force/position control are:

Speed. The invention allows signal processing tasks to be separated according to the bandwidth requirements of the tasks. FIGS. 5 and 6 show the division of tasks into high, medium and low bandwidth. The highest bandwidth is required by the joints on position control. The invention retains standard PID position controllers with only three additional high speed tasks.

The first high speed task is the position bias processing for each joint as shown in FIG. 5. This function is very simple: it requires evaluation of the second order polynomial of equation 5a. The dedicated microprocessors that implement position control for each joint can quickly perform this calculation.

The second high speed task is the implementation of the velocity restraint and the PI force controller 62. Since the implementation of position control and the implementation of force control are mutually exclusive modes, the PID joint microprocessor 30 can easily implement the velocity restraint and the force control functions when not in the position control mode.

The third high speed task is the implementation of the adjustable filter 96 of FIG. 6D whose transfer function is G(S). It is unlikely that this filter would ever be more than the sixth order; a second order filter would probably suffice in most cases. This task could be assigned to a seventh microprocessor to obtain the necessary bandwidth.

Intermediate bandwidth tasks are associated with the processing of the measured contact force. Most assembly operations require much less precision and much less bandwidth in force control than in position control. One can take advantage of this fact by using the lower bandwidth of the force feedback loop to decouple interactions between the force feedback loop and the position feedback loop. The spike removal and filtering circuits of FIG. 6a can be assigned to the seventh microprocessor implementing G(s) or assigned to a separate microprocessor.

Finally the lowest bandwidth tasks are those of the master controller. These tasks would be implemented in a separate microprocessor. Some of these tasks, such as the nonlinear joint transformations, the updating of the sets {A.sub.i }, {B.sub.i } and the computation of the reference angles, require considerable computation and are necessarily low bandwidth. Other computations such as joint selection, mode selection and the sampling of specific armature currents are only needed infrequently.

The position bias processing circuits of FIGS. 5 and 6 overcome the main difficulty of other approches where projection operators must be calculated at the highest frequency. The position bias circuits provide high bandwidth processing of .DELTA..theta. to produce the signals .DELTA..theta..sub.iR to implement first and second order corrections to straight line motion. These circuits enable the master controller to perform the updating of the sets {A.sub.i } and {B.sub.i } at a much lower bandwidth while still retaining the benefits of high bandwidth position servoing.

Stability. The adjustable filter 96 removes a major coupling between the force and position controllers when the gripped object is constrained to move in a plane. Be explicitly taking account of this coupling, the invention avoids potential stability problems.

It will be understood that the above description of the present invention is susceptible to various modifications, changes, and adaptations, and the same are intended to be comprehended within the meaning and range of equivalents of the appended claims.