Force and Compliance Controls

Introduction to Robotics, H. Harry Asada 1 Chapter 9 Force and Compliance Controls A class of simple tasks may need only trajectory control where the robot end-effecter is moved merely along a prescribed time trajectory. However, a number of complex tasks, including assembly of parts, manipulation of tools, and walking on a terrain, entail the control of physical interactions and mechanical contacts with the environment. Achieving a task goal often requires the robot to comply with the environment, react to the force acting on the end-effecter, or adapt its motion to uncertainties of the environment. Strategies are needed for performing those tasks. Force and compliance controls are fundamental task strategies for performing a class of tasks entailing the accommodation of mechanical interactions in the face of environmental uncertainties. In this chapter we will first present hybrid position/force control: a basic principle of strategic task planning for dealing with geometric constraints imposed by the task environment. An alternative approach to accommodating interactions will also be presented based on compliance or stiffness control. Characteristics of task compliances and force feedback laws will be analyzed and applied to various tasks. 9.1 Hybrid Position/Force Control 9.1.1 Principle To begin with let us consider a daily task. Figure 9.1.1 illustrates a robot drawing a line with a pencil on a sheet of paper. Although we humans can perform this type of task without considering any detail of hand control, the robot needs specific control commands and an effective control strategy. To draw a letter, “A”, for example, we first conceive a trajectory of the pencil tip, and command the hand to follow the conceived trajectory. At the same time we accommodate the pressure with which the pencil is contacting the sheet of paper. Let o-xyz be a coordinate system with the z-axis perpendicular to the sheet of paper. Along the x and y axes, we provide positional commands to the hand control system. Along the z-axis, on the other hand, we specify a force to apply. In other words, controlled variables are different between the horizontal and vertical directions. The controlled variable of the former is x and y coordinates, i.e. a position, while the latter controlled variable is a force in the z direction. Namely, two types of control loops are combined in the hand control system, as illustrated in Figure 9.1.2. z O y Fz x Figure 9.1.1 Robot drawing a line with a pencil on a sheet of paper Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada Position Reference + 2 Position Control Robot Force Reference + Controlled Variables Force Control Figure 9.1.2 Position and force control loops The above example is one of the simplest tasks illustrating the need for integrating different control loops in such a way that the control mode is consistent with the geometric constraint imposed to the robot system. As the geometric constraint becomes more complex and the task objective is more involved, an intuitive method may not suffice. In the following we will obtain a general principle that will help us find proper control modes consistent with both geometric constraints and task objectives. Let us consider the following six-dimensional task to derive a basic principle behind our heuristics and empiricism. Example 9.1 Shown below is a task to pull up a peg from a hole. We assume that the peg can move in the vertical direction without friction when sliding in the hole. We also assume that the task process is quasi-static in that any inertial force is negligibly small. A coordinate system O-xyz, referred to as C-frame, is attached to the task space, as shown in the figure. The problem is to find a proper control mode for each of the axes: three translational and three rotational axes. ωz vz vy ωy vx ωx Figure 9.1.3 Pulling up a peg from a hole Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 3 The key question is how to assign a control mode, position control or force control, to each of the axes in the C-frame in such a way that the control action may not conflict with the geometric constraints and physics. M. Mason addressed this issue in his seminal work on hybrid position/force control. He called conditions dictated by physics Natural Constraints, and conditions determined by task goals and objectives Artificial Constraints. Table 9.1.1 summarizes these conditions. From Figure 9.1.3 it is clear that the peg cannot be moved in the x and y directions due to the geometric constraint. Therefore, the velocities in these directions must be zero: vx = 0, v y = 0 . Likewise, the peg cannot be rotated about the x and y axes. Therefore, the angular velocities are zero: ω x = 0, ω y = 0 . These conditions constitute the natural constraints in the kinematic domain. The remaining directions are linear and angular z axes. Velocities along these two directions can be assigned arbitrarily, and may be controlled with position control mode. The reference inputs to these position control loops must be determined such that the task objectives may be satisfied. Since the task is to pull up the peg, an upward linear velocity must be given: vz = V > 0 . The orientation of the peg about the z-axis, on the other hand, doesn’t have to be changed. Therefore, the angular velocity remains zero: ω z = 0 . These reference inputs constitute the artificial constraints in the kinematic domain. Table 9.1.1 Natural and artificial constraints of the peg-in-the-hole problem Kinematic Static vx = 0 Natural Constraints vy = 0 fz = 0 ωx = 0 ωy = 0 τz = 0 fx = 0 Artificial Constraints vz = V > 0 fy = 0 ωz = 0 τx = 0 τy = 0 In the statics domain, forces and torques are specified in such a way that the quasi-static condition is satisfied. This means that the peg motion must not be accelerated with any unbalanced force, i.e. non-zero inertial force. Since we have assumed that the process is frictionless, no resistive force acts on the peg in the direction that is not constrained by geometry. Therefore, the linear force in the z direction must be zero: f z = 0 . The rotation about the z axis, too, is not constrained. Therefore, the torque about the z axis must be zero: τ z = 0 . These conditions are dictated by physics, and are called the natural constraints in the statics domain. The remaining directions are geometrically constrained. In these directions, forces and torques can be assigned arbitrarily, and may be controlled with force control mode. The reference inputs to these control loops must be determined so as to meet the task objectives. In this task, it is not required to push the peg against the wall of the hole, nor twist it. Therefore, the reference inputs are set to Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 4 zero: f x = 0, f y = 0, τ x = 0, τ y = 0 . These constitute the artificial constraints in the statics domain. In the above example, it is clear that the axes involved in the natural constraints and the artificial constraints are orthogonal to each other in both kinematic and static domains. Moreover, the axes involved in the natural kinematic constraints and the artificial static constraints are the same, and the ones listed in the natural static constraints and the artificial kinematic constraints are the same. These relationships are rather obvious in the above example where the direction of each C-frame axis is aligned with the direction along which each control mode, position or force, is assigned. If such a C-frame exists, these orthogonality properties are simply the consequence of the following assumptions and rule: • • • Each C-frame axis must have only one control mode, either position or force, The process is quasi-static and friction less, and The robot motion must conform to geometric constraints. In general, the axes of a C-frame are not necessarily the same as the direction of a separate control mode. Nevertheless, the orthogonality properties hold in general. We prove this next. Let V6 be a six-dimensional vector space, and Va ⊂ V 6 be an admissible motion space, that is, the entire collection of admissible motions conforming to the geometric constraints involved in a given task. Let Vc be a constraint space that is the orthogonal complement to the admissible motion space: Vc = Va ⊥ (9.1.1) Let F ∈ V 6 be a six-dimensional endpoint force acting on the end-effecter, and ∆p ∈ V 6 be an infinitesimal displacement of the end-effecter. The work done on the end-effecter is given by ∆Work = FT ∆p (9.1.2) Decomposing each vector to the one in the admissible motion space and the one in the constraint space, F = Fa + Fc ; Fa ∈Va , Fc ∈Vc (9.1.3) ∆p = ∆p a + ∆p c ; ∆p a ∈Va , ∆p c ∈Vc and substituting them to eq.(2) yield ∆Work = (Fa + Fc )T (∆p a + ∆p c ) = Fa ∆p a + Fa ∆p c + Fc ∆p a + Fc ∆p c T T T T = Fa ∆p a + Fc ∆p c T since Fa ⊥ ∆p c , T (9.1.4) Fc ⊥ ∆p a by definition. For the infinitesimal displacement ∆p to be a virtual displacement δp , its component in the constraint space must be zero: ∆p c = 0 . Then, ∆p a = δp becomes a virtual displacement, and eq.(4) reduces to virtual work. Since the system is in a static equilibrium, the virtual work must vanish for all virtual displacements δp a . Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 5 δWork = Fa T δp a = 0, ∀δp a (9.1.5) This implies that any force and moment in the admissible motion space must be zero, i.e. the natural static constraints: 0 = Fa ∈Va (9.1.6) Furthermore, the properties of artificial static constraints can be derived from eqs.(4) and (5). Since in eq.(4) ∆p c = 0 , the static equilibrium condition holds, although Fc ∈ Vc takes an arbitrary value. This implies that to meet a task goal we can assign arbitrary values to the force and moment in the constraint space, i.e. the artificial static constraints. arbitrary : Fc ∈Vc (9.1.7) Converting infinitesimal displacements to velocities, p a , p c , we can obtain the natural and artificial kinematic constraints: arbitrary : p a ∈Va , 0 = p c ∈Vc (9.1.8) Table 9.1.2 summarizes the above results. Table 9.1.2 Mason’s Principle of Hybrid Position/Force Control Natural Constraints Artificial Constraints Kinematic Static 0 = p c ∈Vc arbitrary : p a ∈Va 0 = Fa ∈Va arbitrary : Fc ∈Vc The reader will appreciate Mason’s Principle when considering the following exercise problem, in which the partition between admissible motion space and constraint space cannot be described by a simple division of C-frame axes. Rather the admissible motion space lies along an axis where a translational axis and a rotational axis are coupled. Exercise 9.2 (The same as PS) 9.1.2 Architecture of Hybrid Position/Force Control System Based on Mason’s Principle, a hybrid position/force control system can be constructed in such a way that the robot control system may not have a conflict with the natural constraints of the task process, while performing the task towards the task goal. Figure 9.1.5 shows the block diagram of a hybrid position/force control system. The upper half of the diagram is position control loops, where artificial kinematic constraints are provided as reference inputs to the system and are compared with the actual position of the end-effecter. The lower half of the diagram is force control loops, where artificial static constraints are provided as reference inputs to the Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 6 feedback loops and are compared with the actual force and moment at the end-effecter. Note that feedback signals are described in an appropriate C-frame attached to the end-effecter. If the feedback signals are noise-less and the C-frame is perfectly aligned with the actual task process, the position signal of the feedback loop must lie in the admissible motion space, and the force being fed back must lie in the constraint space. However, the feedback signals are in general corrupted with sensor noise and the C-frame may be misaligned. Therefore, the position signal may contain some component in the constraint space, and some fraction of the force signal may be in the admissible motion space. These components are contradicting with the natural constraints, and therefore should not be fed back to the individual position and force controls. To filter out the contradicting components, the feedback errors are projected to their own subspaces, i.e. the positional error ep mapped to the admissible motion space Va and the force feedback error ef mapped to the constraint space Vc. In the block diagram these filters are shown by projection matrices, Pa and Pc :   e p = Pa e p , e f = Pc e f (9.1.9) When the C-frame axes are aligned with the directions of the individual position and force control loops, the projection matrices are diagonal, consisting of only 1 and 0 in the diagonal components. In the case of the peg-in-the-hole problem, they are: Pa = diag (1 1 0 1 1 0), Pc = diag (0 0 1 0 0 1) (9.1.10) In case of Example 9.2 where the C-frame axes are not the directions of the individual position and force control loops, the projection matrices are not diagonal. Position Feedback _ + Pa Position Reference Inputs  ep J −1 Position/velocity control compensator + Robot Task Environment + Force Reference Inputs + _ Pc  ef JT Force/torque control compensator Force Feedback Figure 9.1.4 Block diagram of hybrid position/force control system Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada  7  These feedback errors, e p and e f , are in the C-frame, hence they must be converted to the joint space in order to generate control commands to the actuators. Assuming that the positional error vector is small and that the robot is not at a singular configuration, the position feedback error in joint coordinates is given by  e q = J −1 e p (9.1.11) where J is the Jacobian relating the end-effecter velocities in the C-frame to joint velocites. The force feedback error in the joint coordinates, on the other hand, is obtained based on the duality principle:  eτ = J T e f (9.1.12) These two error signals in the joint coordinates are combined after going through dynamic compensation in the individual joint controls. 9.2 Compliance Control 9.2.1 Task strategy Use of both position and force information is a unique feature in the control of robots physically interacting with the environment. In hybrid position/force control, separation was made explicitly between position and force control loops through projections of feedback signals onto admissible motion space and constraint space. An alternative to this space separation architecture is to control a relationship between position and force in the task space. Compliance Control is a basic control law relating the displacement of the end-effecter to the force and moment acting on it. Rather than totally separating the task space into subspaces of either position or force control, compliance control reacts to the endpoint force such that a given functional relationship, typically a linear map, is held between the force and the displacement. Namely, a functional relationship to generate is given by ∆p = CF (9.2.1) where C is an m x m Compliance Matrix, and ∆pand F are endpoint displacement and force represented in an m-dimensional, task coordinate system. Note that the inverse to the compliance matrix is a stiffness matrix: K = C −1 (9.2.2) if the inverse exists. The components of the compliance matrix, or the stiffness matrix, are design parameters to be determined so as to meet task objectives and constraints. Opening a door, for example, can be performed with the compiance illustrated in Figure 9.2.1. The trajectory of the doorknob is geometrically constrained to the circle of radius R centered at the door hinge. The robot hand motion must comply to the constrained doorknob trajectory, although the trajectory is not exactly known. The robot must not break the doorknob, although the conceived trajectory is different from the actual trajectory. This task requirement can be met by assigining a small stiffness, i.e. a high compliance, to the radial direction perpendicular to the trajectory. As illustrated in the figure, Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 8 such a small spring constant generates only a small restoring force in response to the discrepancy between the actual doorknob trajectory and the reference trajectory of the robot hand. Along the direction tangent to the doorknob trajectory, on the other hand, a large stiffness, or a small compliance, is assigned. This is to force the doorknob to move along the trajectory despite friction and other resistive forces. The stiffness matrix is therefore given by ⎛ kx K = ⎜⎜ ⎝0 0⎞ ⎟ ; k x << 1, k y >> 1 k y ⎟⎠ (9.2.3) with reference to the task coordinate system O-xy. Using this stiffness with which the doorknow is held, the robot can open the door smoothly and dexterously, although the exact trajectory of the doorknob is not known. y k y >> 1 O x k x << 1 R Doorknob Trajectory Figure 9.2.1 Door opening with compliance control 9.2.2 Compliance control synthesis Now that a desired compliance is given, let us consider the method of generating the desired compliance. There are multiple ways of synthesizing a compliance control system. The simplest method is to accommodate the proportional gains of joint feedback controls so that desired restoring forces are generated in proportion to discrepancies between the actual and reference joint angles. As shown in Figure 9.2.2, a feedback control error ei is generated when a disturbance force or torque acts on the joint. At steady state a ststic balance is made, as an actuator torque τ i proportional to the control error ei cancels out the disturbance torque. The proportionality constant is determined by the position feedback gain ki, when friction is neglected. Therefore a desired stiffness or compliance can be obtained by tuning the position feedback gain. Compliance synthesis is trivial for single joint control systems. For general n degree-offreedom robots, however, multiple feedback loops must be coordinated. We now consider how to generate a desired m x m compliance or stiffness matrix specified at the endpoint by tuning joint feedback gains. Department of Mechanical Engineering Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 9 Disturbance Force/torque + ei PD Control Actuators τi + Robot Mechanis + qri (t ) qi Figure 9.2.2 Single joint position feedback control system Theorem Let J be the Jacobian relating endpoint velocity p ∈ R m x1 to joint velocities q ∈ R n x1 , and τ ∈ R n x1 be joint torques associated with joint coordinates q. Let ∆p ∈ R m x1 be a m x 1 vector of the endpoint displacement measured from a nominal position p , and F ∈ R m x1 be the endpoint force associated with the endpoint displacement ∆p . Let Kp be a desired endpoint stiffness matrix defined as: F = K p ∆p (9.2.4) The necessary condition for joint feedback gain Kq to generate the endpoint stiffness Kp is given by K q = JT K p J (9.2.5) assuming no friction at the joints and linkage mechanisms. Proof Using the Jacobian and the duality principle as well as eq.(4), τ = J T F = J T K p ∆p = J T K p J ⋅ ∆q (9.2.6) Using eq.(5), the above relationship reduces to τ = K q ∆q (9.2.7) This implies that Kq is the joint feedback gain matrix. Example 9.2.1 Consider a two-link, planar robot arm with absolute joint angles and joint torques, as shown in Figure 9.2.3. Obtain the joint feedback gain matrix producing the endpoint stiffness Kp : ⎛k 0 ⎞ ⎟⎟ K p = ⎜⎜ 1 ⎝ 0 k2 ⎠ Department of Mechanical Engineering (9.2.8) Massachusetts Institute of Technology Introduction to Robotics, H. Harry Asada 10 Assuming that the link length is 1 for both links, the Jacobian is given by ⎛ − s − s2 ⎞ ⎟ J = ⎜⎜ 1 c2 ⎟⎠ ⎝ c1 (9.2.9) From eq.(5), kq3 ⎞ ⎟ k q 2 ⎟⎠ ⎛ k q1 K q = J T K p J = ⎜⎜ ⎝ kq3 (9.2.10) where k q1 = k1s12 + k 2c12 k q 2 = k1s22 + k 2c22 (9.2.11) k q 3 = k1s1s2 + k 2c1c2 Note that the joint feedback gain matrix Kq is symmetric and that the matrix Kq degenerates when the robot is at a singular configuration. If it is non-singular, then −1 −1 −1 ∆p = J∆q = JK q τ = JK q J T F = J (J T K p J ) −1 J T F = K p F = CF (9.2.12) Therefore, the obtained joint feedback gain provides the desired endpoint stiffness given by eq.(8), or the equivalent compliance. F y ∆p φ2 φ1 τ2 τ1 x O Figure 9.2.3 Two link robot Department of Mechanical Engineering Massachusetts Institute of Technology