Haptic Simulation for Fixture Loading Planning
Proceedings of the 2005 IEEE International Conference on Robotics and Biomimetics June 29 - July 3, 2005, Hong Kong and Macau
Haptic Simulation for Fixture Loading Planning Tong Liu and Michael Yu Wang Department of Automation and Computer-Aided Engineering, The Chinese University of Hong Kong, Shatin, N.T., Hong Kong (e-mail: {tliu, yuwang}@acae.cuhk.edu.hk)
Abstract— The paper presents a haptic-guided fixture loading planning system. The system consists three parts, a multi-body contact dynamics solver based on time-stepping and GaussSeidel method, a planning scheme based on a criteria of nonjamming condition that can monotonously increase the contacts with locators, and a haptic simulation system. Index Terms— Assembly planning, jamming, multi-body contact dynamics, haptics.
I. I NTRODUCTION Workpiece inserting and extracting is one of the basic operations in robotic manipulation and automatic manufacture, such as part assembly and disassembly, fixture loading and unloading [1], [2]. Planning the workpiece inserting scheme is much different from trajectory planning. While the goal of the trajectory planning is to find a collision-free path from the initial configuration to the desired final configuration, workpiece inserting involves not only motion in free space, but also motion of a workpiece requiring physical interaction (e.g. contacts) with its environment [3], [4]. Furthermore, the final goal of a inserting scheme is usually not defined by a position in three dimensional space, but by contact constraints with other part of the assembly (for example, in fixture loading, the final position of a workpiece are determined by contacts with all the locators). In this paper, we will study one example of inserting scheme, fixture loading. We will present a system that can plan the fixture loading scheme interactively with a haptic device. A fixture is a device that can immobilize and localize workpieces [5]. The precise positioning is achieved by contacts of the workpiece with all locators. Six locators are required to localize a workpiece in three dimensional space application, and three locators are required for two dimensional applications. Fixture loading is to move the workpiece from a free space to the final position that is in contact with all the locators. Many recent researches in fixturing are concerned with the the fixture layout design which aims at force closure and reducing the locating error [6], [7], [8]. But fixture loading planning is also as important as the fixture layout design. Without a proper loading scheme, the workpiece may not be in contact with locators and positioning cannot be guaranteed [9]. Although it is commonly recognized that automating this task is of both theoretical and practical importance, many real assembly and fixture loading is still performed manually [2]. A major reason is that the relationship between the motion of a workpiece under frictional contacts and forces exerted on it
0-7803-9315-5/05/$20.00 ©2005 IEEE
190
is very complicated and difficult to be used in the planning and control of manipulators [10], [2]. Similar problems appear in grasping planning, such as enveloping grasp and rolling [11]. The situation is made even worse by the imprecision of most of the robotic manipulators. Since automatic assembly has been studied for many years, we first make an overview of the related results. One method to facilitate the inserting operation is to enhance the compliance of the manipulator with a compliant device. A famous example is Whitney’s remote center compliance (RCC) [12]. The remote compliance center decomposes the translational motion and the rotational motion, and if a contact point is close to it, no rotational motion will occur, therefore jamming is preventable. However, RCC devices can only be used in simple inserting tasks, such as peg-in-hole. For complex assembly tasks such as fixture loading, the workpiece needs more maneuverability and designing a compliance device will be unrealistic. To enhance the flexibility of a robotic system to different assembly tasks and make complex inserting operations possible, motion planning approaches are more promising [13]. A simplified version of this problem is to assume that the workpiece is in the vicinity of the final position [2], [14], and the position error is negligible. By this assumption, the geometric configuration will be unchanged during the operation, so that the planning is usually simplified into the operation on a linear system. In [2], a polyhedral convex cone is computed that external wrenches in the cone will only increase the number of contacts. For three dimensional application, the method needs a linearization of the friction cone. In [14], an admittance matrix is designed to map the contact forces to the motion of the body. This can be viewed as an active version of RCC, because the compliance is guaranteed by the control law. However, for inserting tasks like fixture loading, the vicinity assumption is not realistic. This is because usually before reaching the vicinity, the position error of the manipulator is already large enough to form contact(s) with some locators. The contact forces will deviate the workpiece from the desired motion, or even immobilize the workpiece. Therefore, A more general problem is to plan and control the overall scheme from the free space to the final full contact position. One class of approaches to this problem rely on an extension of the motion planning in free space. That is, the collision-free path is generated first, then a patch of a refined plan is used in place where the path is prune to collision [13], [15]. A
different approach is so called LMT method, by Lozano-Perez, Mason, and Taylor[16], where the planning starts from the final position and search for the preimage that includes the initial position and results in this final position. One issue in the planning of fixture loading is to deal with the contact forces. If there is no friction, all contact forces are perpendicular to the contact surfaces at the contact points. The mapping of contact forces to the body frame is dual to the mapping of body velocity[9] to contact velocities. Therefore, a problem involving frictionless contact forces can always be converted to a problem related to velocity. However, when there are frictional forces between the contacted surfaces, the problem of solving the contact forces and the motion of the workpiece will be indeterminate. Usually friction is modelled by Coulomb’s friction model. With this model, the contact forces are always within the friction cones, and if rolling contact occurs, the direction of the contact force at the contact point will be anywhere within the friction cone. Therefore, the duality between the contact forces and velocity will no longer exist. Because of indeterministic contact forces, the motion of the manipulated body is also difficult to determine. That is why many inserting schemes, such as RCC or LMT methods require sliding contacts. A companion issue to indeterministic contact forces is the uncertainty. This is not critical when planning motion in free spaces, mainly because the system dynamics does not change and can be estimated well. However, when contact involved, a large deviation from planned path can be generated from a small initial error. The contact forces may also be large enough to resist motion and result in mechanical failure. Since fully automatic planning for fixture loading is difficult, human interaction can be a promising way to solve the problem. Two strategies are available: physical prototypes and virtual prototypes. Physical prototypes are generally time consuming and costly. On the other hand, haptic interaction and virtual reality enable simultaneous feeling of contact force and vision feedback, and thus make realistic simulation feasible. One can feel with his fingers or hands the resultant force from contacts and motion of the workpiece resulted from the applied force. With these information and human experience, he can easily find the appropriate force to push the workpiece to the right contact state. Incorporating the haptic ’hints’, the planning can be made robust [17]. On the other hand, for real-time haptic ‘display’ of the contact force, the simulation must be performed with a high update rate (≥ 1000Hz) [18]. The speed and accuracy of the simulation is also a key problem in haptic interaction. This paper will give an outline of such a haptic simulation based fixture inserting planning system. In this system, the transition of contact states and exerted force are planned instead of the free-space path. The system is composed of three parts: multi-body contact dynamics engine, automatic planning subsystem, and haptic simulation interface. The multi-body contact dynamics engine is to compute the corresponding motion of the workpiece and contact forces when manipulator applying forces on the workpiece. Automatic planning subsys-
191
tem is to find a trajectory to a subgoal and find appropriate exerted forces for the workpiece to have such a trajectory. II. M ULTI - BODY C ONTACT DYNAMICS In a rigid-body model, the interaction between bodies is approximated as a collision or a unilateral contact, and the experimental laws (such as the restitution law and Coulomb’s friction law) are used to describe these phenomena. A major disadvantage of this approximation is the discontinuity in velocities which results in the impulsive contact forces. These discontinuous quantities are no longer ordinary functions of time but rather distributions or measures, and their associated differential equations must be treated as differential inclusions. Here, we use a method based on an extension of explicit time-stepping scheme and an application of the differential inclusion process introduced by J. J. Moreau[19]. From a differential kinematic analysis of contacts, we derive a set of transfer equations in the velocity-based time-stepping formulation. In applying the Gauss-Seidel iterative scheme, the transfer equations are combined with the Signorini conditions and Coulomb’s friction law. The contact forces are properly resolved in each iteration, without resorting to any linearization of the friction cone. The following is a brief introduction to this method. The mathematical model includes three major components: contact kinematics of smooth-rigid bodies, dynamics of motion discretized with a time-stepping scheme, and appropriate contact laws governing the non-penetration, collision and Coulomb’s friction conditions. The contact kinematics refers to the kinematics relationship between the internal geometry and external coordinate frame, and the transformation between body motion and contact point motion. The following contact differential equation describes the relationship between the internal geometry and external coordinate frame. Vlg lf = Gφ˙
(1)
where, lf and lg are local frames fixed with respect to the body frames, and at the time of interest they coincide with the contact frame, Vlg lf is the body velocity of frame lf with respect to lg , φ = [uf , vf , ug , vg , d, ψ]T is the meshing parameters. The transformation between body motion and contact point motion is Vlg lf = Adglf f (Vwf − Adgf g Vwg ),
(2)
where, Adgab is the adjoint transformation of the transformation matrix gab as defined in [20]. The contact dynamics of time-stepping is derived by discretizing Newton-Euler Equations. The final form is given as follows. Pnc h vi+1 = (3) c=1 Fc )i + vi m (Fext + ωi+1 =
−1
T Ri+1 I b Ri+1 · Pnc Pnc Tc )i h ((Text + c=1 pc × Fc + c=1
+Ri I b
−1
RiT ωi )
(4)
where the contact forces Fc and torques Tc (c = 1, · · · , nc ) are understood to be unknowns and to be solved for. The contact transfer equations is to connect the contact velocity (of the next time-step) with the contact forces. It is a linear transformation when current configuration is decided. Following is its formula. vˆj = α ˆ j + Bjj Fˆj Pnc α ˆ j = αj + k=1,k6=j Bjk Fˆk (j = 1, · · · , nc )
(5)
where, Fˆj vˆj
= [Fx , Fy , Fz , Tz ]T = [vx , vy , vz , ωz ]T
(6) (7)
Contact laws are empirical formulae to describe the physical behavior of two rigid bodies in unilateral contact. In general, if there is a normal relative velocity with contact, there will be a collision and the two bodies cannot penetrate into each other and may maintain their contact or sperate. This process is governed by the so called Signorini conditions. When two bodies are in contact, the friction forces at their contact point are usually described by Coulomb’s law. Signorini condition can be written as vz ≤ 0; Fz ≤ 0; vzT Fz = 0 ,
(8)
or, in a compact form, as 0 ≥ vz ⊥ Fz ≤ 0. Coulomb’s friction law is given as Fx2 + Fy2 ≤ µ2 Fz2
(9)
Fy2 Fx2 Tz2 + + ≤ µ2 Fz2 , e2x e2y e2r
(10)
or,
Together with the contact transfer equations, the contact forces can be resolved from contact laws when there is only one contact point. For multiple contacts, we use a Gauss-Seidel scheme to resolve multiple contacts. Gauss-Seidel scheme is to use an iterative process to find a solution for the contact dynamics defined by the transfer equation (5), the Signorini conditions (8), and Coulomb’s law (9) or (10). One such approach is to use the Gauss-Seidel iteration method first suggested by Moreau [19], [21], who has developed a sound mathematical justification based on the theory of convex analysis and differential inclusion. It is essentially a method of propagation of contact among the multiple, simultaneous contacts. At any step of the iteration, only one contact is updated and all other contacts are assumed to have been resolved previously. Starting from an initial guess of all of the contact forces in the transfer equation (5), one can choose to update the contact forces of any single contact, for example, Fˆj . Therefore, the only unknowns in the transfer equation (5) are the contact force Fˆj and the contact velocity vˆj of this contact, while α ˆ j in the equations is already given. Thus, this transfer equation is then resolved by applying the contact laws for determining Fˆj as to be discussed before.
192
III. N ON -JAMMING C ONDITION During the workpiece loading (or insertion) process, it is unrealistic and difficult to form all six contacts simultaneously. It is a usual strategy to form some contacts first and then to try to form another contact by applying a force on the workpiece while keeping the established contacts from separation. During this latter step, it is possible that the workpiece becomes immobilized by the first formed contacts because of friction forces at the contacts if the applied force results in rolling motion at the frictional contacts. Such immobilization is defined as jamming in this paper, that is, immobilization involving any tangential motion constraint at a frictional contact. For the task of loading a workpiece into the fixture, the goal is to reach the final immobilization of the workpiece by the normal constraints only. While in the process of reaching the goal, any jamming must be avoided. If the workpiece is jammed, the direction and magnitude of the applied force has to be adjusted such that the tangential constraints must be made ineffective. Furthermore, the applied force should also drive the workpiece closer to the final goal. Haptic simulation with force feedback can simplify the task of searching such applying forces, because human can apply his global perception and experience to the task, which is unavailable for the automatic planning [17], [22]. However, when the number of contacts increases, it is not obvious to find the non-jamming applying force even though we know the expected motion of the workpiece. In this case, a scheme to find the non-jamming applied force that result in the expected motion is necessary for a successful planning. In this section, we will analyze the conditions of non-jamming, and to present a scheme to find such non-jamming applying forces. The conclusion drawn in this section will give a sufficient and necessary condition for the jamming criteria, which does not rely on the linearization of the friction cone and treats the two dimensional and three dimensional problems in a unified way. Following are the summery of related results of contact kinematics. Rolling is defined as the tangential relative velocity of the contact points of two bodies is zero, and relative rotational velocity is in the tangent plane. Although degrees of freedom (DOF) of a body in contact with another body are five, DOF of a body rolling on one contact point are only two; and DOF of a body rolling on two contact points is one, that is, rolling about an axis that is coincide with the line crossing the two contact points. If there are more than two contact points, in general (that is, the contacts do not lie on the same line), there will be no motion if you want all the contacts be rolling. The DOFs of a body with n contacts are 6 − n. When the number of the contacts is greater than or equal to 6, the body is immovable while maintaining all the contacts. In case of contacts less than three, there will be only one solution of the contact forces given an exerted force on the body, and the body motion can be computed easily when know all the forces. However, if the contact number is greater or equal to three, it is known as a static indeterminant problem
with the number of the unknown variables greater than the numbers of the equations. Therefore, we only consider this case in the following discussions. Consider a general case, that is, the body has 3 or more contacts, and the normal directions of the contacts do not intersect in one point. In this case, if the body can move while maintaining all the contacts, the only way is all sliding contacts. However, when the body is in motion, finite forces cannot stop the motion abruptly. Therefore, jamming can only occur when the body is, by some chance, at rest. Correspondingly, we define no jamming condition for an exerted wrench as that the wrench cannot be balanced by static contact forces, at the same time, existing contacts are maintained. We first set up the mechanical framework for this problem. For convenience, the body frame of the workpiece is set to coincide with the inertia frame. Let Rf li = [xi , yi , zi ], and T z1 −z1T p∧ f l1 .. G = ... , . T T ∧ zn −zn pf ln T x1 −xT1 p∧ f l1 .. Hx = ... , . T T ∧ xn −xn pf ln T y1 −y1T p∧ f l1 .. Hy = ... , . T T ∧ yn −yn pf ln mI3×3 0 . (11) M= 0 I body Then, the force balance equation can be written as e T F fz fxT fyT = + GT HxT HyT Te T T . (12) M aTuf αuf
For frictional contacts of rigid-bodies, the non-penetration condition and Coulomb’s friction law must be satisfied. Let Gi be the ith row of G. If Gi a < 0, we have fxi = fyi = fzi = 0, corresponding to separation. If Gi a = 0, fxi2 + fyi2 ≤ µ2i fzi2 . Furthermore, if the contact is sliding, fxi = kxi fzi and fyi = kyi fzi
(13)
where, aiy ai , kyi = µ q . kxi = µ q x i2 i2 ai2 ai2 x + ay x + ay
(14)
The above equations refers to that the relative acceleration of the contact points is opposite to the direction of the friction forces (because fz ≤ 0). The no jamming condition is in fact two conditions, that is the exerted force must be resistant to separation, and also
193
resistant to all rolling contacts. For the condition of resistance to separation, we need the following conditions. First, suppose all the contact points can be penetrated freely (that is there is no forces on the contact points), we require e F −1 ≥ 0. (15) GM Te That is the gaps of the contacts should be reduced. Second, we suppose that some contact points do not exist, and let C be the set of points where there is contact, and S be the set of points where there is no contact. Therefore, we require e h i C F fz T T S −1 C C G M + G ≤ 0, H Te ftC e h i fC F T T z GC M −1 + GC = 0. (16) HC e T ftC However, if all the contacts exits, we have e fz T F T G H GM −1 + =0 Te ft
(17)
Here we only consider the case when rank(G) = n, that is, the normal vectors of the contact points are not intersected at one point. As discussed before, n ≥ 3 and the body has a motion while maintaining contacts, ati 6= 0. Therefore, e T fz F T G H H i M −1 + = ait 6= 0 (18) Te ft and fxi = kxi fzi and fyi = kyi fzi
(19)
where, kxi = µ q
aix i2 ai2 x + ay
6= 0 and kyi = µ q
aiy
6= 0
(20)
i2 ai2 x + ay
Now we conclude with the following theorem. With it, we can determine whether the applied force to the body results in jamming, and can further find a no jamming exerted force. The detailed proof will be found in later papers. Theorem 1: For a given applied force [F eT , T eT ]T satisfying satisfying (15), if it results in a contact force vector [fzT , ftT ]T satisfying (17), (18), (19) simultaneously is found, then the applied force [F eT , T eT ]T will definitely result in sliding motion of the body at all of the contacts with the contact forces [fzT , ftT ]T . There are two remarks for this theorem. First, since the state of all sliding contacts is a sufficient condition for non-jamming, the [F eT , T eT ]T satisfying (15), (18), and (19) will definitely result in non-jamming. Second, for n ≥ 3 and considering frictional moment, it is impossible for any one of the contacts to become rolling unless the configuration constraints are satisfied. If the configuration constraints are not satisfied, then the condition of non-jamming is equivalent to having sliding on all of contact points. Therefore, the condition for an applied force to make
all sliding contacts is a necessary and sufficient condition for non-jamming for n ≥ 3. This theorem can serve as guideline for finding the applied force to make all contacts sliding.
No. CUHK4376/02E), and the Natural Science Foundation of China (NSFC) (Young Overseas Investigator Collaboration Award 50128503).
IV. H APTIC - BASED I NSERTING P LANNING
[1] Matthew T. Mason. Mechanics of Robotic Manipulation. The MIT Press, 2001. [2] J. C. Trinkle and D. Balkcom. Computing wrench cones for planar contact tasks. In IEEE International Conf. on Robotics and Automation, pages 869 – 875, May 2002. [3] Joseph M. Schimmels and Michael A. Peshkin. Force-assembly with friction. IEEE Transaction on Robotics and Automation, 10(4):465–479, August 1994. [4] Devin Balkcom and J. C. Trinkle. Computing wrench cones for planar rigid body contact tasks. The International Journal of Robotics Research, 21(12):1053–1066, December 2002. [5] Paul D. Campbell. Basic Fixture Design. Industrial Press, New York, 1994. [6] H. Asada and A. By. Kinematic analysis of workpart fixturing for flexible assembly with automatically reconfigurable fixtures. IEEE Transaction on Robotics and Automation, RA-1(2):86 – 94, 1985. [7] J. S. Carlson. Quadratic sensitivity analysis of fixturing and locating schemes for rigid parts. ASME Journal of Manufactring Science and Engineering, 123(3):462 – 472, 2001. [8] M. Y. Wang and T. Liu. Fixture kinematic analysis based on the full contact model of rigid bodies. In Proceedings of IEEE International Conference on Intelligent Robots and Systems (IROS), pages 1602 – 1607, October 2002. [9] Joseph M. Schimmels and Michael A. Peshkin. Force-assemblability: Insertion of a workpiece into a fixture guided by contact forces alone. In IEEE International Conf. on Robotics and Automation, pages 1296 – 1301, April 1991. [10] J.S. Pang and J.C. Trinkle. Complementarity formulations and existence of solutions of dynamic multi-rigid-body contact problems with coulomb friction. Mathematical Programming, 73:139 – 154, 1996. [11] Jonghoon Park, Wan-Kyun Chung, and Makoto Kaneko. Active-external enveloping grasping: Dynamical-balance based motion analysis. In IEEE International Conf. on Intelligent Robots and Systems, pages 1566 – 1571, October 2002. [12] D. E. Whitney and J. L. Nevins. The remote center compliance: What can it do? Robotics Today, pages 135–139, Summer 1979. [13] Jan Rosell Gratac`os. Fine-Motion Planning for Robotic Assembly under Modelling and Sensing Uncertainties. Ph. d. thesis, Universitat Polit`echica de catalunya, June 1998. [14] Joseph M. Schimmels and Michael A. Peshkin. Admittance matrix design for force-guided assembly. IEEE Transaction on Robotics and Automation, 8(2):213 – 227, April 1992. [15] Jing Xiao. Planning motion compliant to complex contact states. International Journal of Robotics Research, 20(6):446–465, June 2001. [16] T. Lozano-Perez, M. T. Mason, and R. H. Taylor. Automatic synthesis of fine-motion strategies for robots. International Journal of Robotics Research, 3(1):3–24, 1984. [17] Nancy M. Amato and O. Burchan Bayazit et al. Providing haptic ’hints’ to automatic motion planners. Technical report 98-026, Department of Computer Science Texas A&M University, November 1998. [18] Mandayam A. Srinivasan and Cagatay Basdogan. Haptics in virtual environments: Taxonomy, research status, and challenges. Computer and Graphics, 21(4):393 – 404, 1997. [19] J. J. Moreau and Michel Jean. Numerical treatment of contact and friction: the contact dynamics method. Engineering Systems Design and Analysis, 4:201–208, 1996. [20] R. M. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, Boca Raton, FL, 1994. [21] J. J. Moreau. Numerical aspects of the sweeping process. Computer Methods in Applied Mechanics and Engineering, 177(1999):329–349, 1999. [22] D. D. Nelson, D. E. Johnson, and E. Cohen. Haptic rendering of surfaceto-surface sculpted model interaction. In 8th Annual Symp. on Haptic Interfaces for Virtual Environment and Teleoperator Systems, November 1999.
In this section, we briefly introduce the planning scheme for fixture loading operation. As discussed before, fixture loading has its specialties, like there is only 6 locators in a general three dimensional case; surfaces are contacted only on one point, and the final state is at the point when all 6 contacts are formed. Considering these specialties, we divide the planning scheme into two steps. The central point of the scheme is that the planned motion should make the contact points increasing monotonously. The first step is a coarse plan, which is similar to LMT. That is, we define the set of states that can be recognizably (with force sensors) attained, and select the pre-images that connect the final state to the initial state. The difference from our method and the standard LMT is that in standard LMT, the way from one state to another is only a simple operation, with polyhedral bodies, it may work well; however, for fixturing with curved surfaces, one simple operation usually cannot fulfil the task. Therefore, we have a second step, which is to determine the strategy from one state to another. Although in general cases, LMT is computationally ineffective, for fixture loading, it is very simple, since there are only point contacts. Our recognizable states only refer to the number of contact points and with which locator the workpiece contacts. This step can be done automatically, or by user assignment. For the second step, in each state transition, we plan two things: the body velocity and the exerted force by the manipulator. When contact points are less than three, we require the body in rolling contact with the locators to approach another locator. That means the contact forces are in the friction cones. When contact points increase to three or more, rolling contact cannot occur, therefore, we require the body have a no jamming motion while maintain the contacts. The contact velocities and forces can be computed with the method introduced in the former section. Haptic simulation are used in the second step so that the user can suggest a body velocity when planning cannot process, and the user can also use haptic simulation to verify the planned scheme off-line. V. C ONCLUSION AND F URTHER W ORK The paper presents a haptic-guided fixture loading planning system. The system is based on a multi-body contact dynamics solver. An criteria of no jamming condition is given, and the planning scheme that can monotonously increase the contacts with locators is briefly introduced. ACKNOWLEDGMENT The research work is supported in part by the Hong Kong Research Grants Council (Projects No. CUHK4217/01E and
194
R EFERENCES
Haptic Simulation for Fixture Loading Planning Tong Liu and Michael Yu Wang Department of Automation and Computer-Aided Engineering, The Chinese University of Hong Kong, Shatin, N.T., Hong Kong (e-mail: {tliu, yuwang}@acae.cuhk.edu.hk)
Abstract— The paper presents a haptic-guided fixture loading planning system. The system consists three parts, a multi-body contact dynamics solver based on time-stepping and GaussSeidel method, a planning scheme based on a criteria of nonjamming condition that can monotonously increase the contacts with locators, and a haptic simulation system. Index Terms— Assembly planning, jamming, multi-body contact dynamics, haptics.
I. I NTRODUCTION Workpiece inserting and extracting is one of the basic operations in robotic manipulation and automatic manufacture, such as part assembly and disassembly, fixture loading and unloading [1], [2]. Planning the workpiece inserting scheme is much different from trajectory planning. While the goal of the trajectory planning is to find a collision-free path from the initial configuration to the desired final configuration, workpiece inserting involves not only motion in free space, but also motion of a workpiece requiring physical interaction (e.g. contacts) with its environment [3], [4]. Furthermore, the final goal of a inserting scheme is usually not defined by a position in three dimensional space, but by contact constraints with other part of the assembly (for example, in fixture loading, the final position of a workpiece are determined by contacts with all the locators). In this paper, we will study one example of inserting scheme, fixture loading. We will present a system that can plan the fixture loading scheme interactively with a haptic device. A fixture is a device that can immobilize and localize workpieces [5]. The precise positioning is achieved by contacts of the workpiece with all locators. Six locators are required to localize a workpiece in three dimensional space application, and three locators are required for two dimensional applications. Fixture loading is to move the workpiece from a free space to the final position that is in contact with all the locators. Many recent researches in fixturing are concerned with the the fixture layout design which aims at force closure and reducing the locating error [6], [7], [8]. But fixture loading planning is also as important as the fixture layout design. Without a proper loading scheme, the workpiece may not be in contact with locators and positioning cannot be guaranteed [9]. Although it is commonly recognized that automating this task is of both theoretical and practical importance, many real assembly and fixture loading is still performed manually [2]. A major reason is that the relationship between the motion of a workpiece under frictional contacts and forces exerted on it
0-7803-9315-5/05/$20.00 ©2005 IEEE
190
is very complicated and difficult to be used in the planning and control of manipulators [10], [2]. Similar problems appear in grasping planning, such as enveloping grasp and rolling [11]. The situation is made even worse by the imprecision of most of the robotic manipulators. Since automatic assembly has been studied for many years, we first make an overview of the related results. One method to facilitate the inserting operation is to enhance the compliance of the manipulator with a compliant device. A famous example is Whitney’s remote center compliance (RCC) [12]. The remote compliance center decomposes the translational motion and the rotational motion, and if a contact point is close to it, no rotational motion will occur, therefore jamming is preventable. However, RCC devices can only be used in simple inserting tasks, such as peg-in-hole. For complex assembly tasks such as fixture loading, the workpiece needs more maneuverability and designing a compliance device will be unrealistic. To enhance the flexibility of a robotic system to different assembly tasks and make complex inserting operations possible, motion planning approaches are more promising [13]. A simplified version of this problem is to assume that the workpiece is in the vicinity of the final position [2], [14], and the position error is negligible. By this assumption, the geometric configuration will be unchanged during the operation, so that the planning is usually simplified into the operation on a linear system. In [2], a polyhedral convex cone is computed that external wrenches in the cone will only increase the number of contacts. For three dimensional application, the method needs a linearization of the friction cone. In [14], an admittance matrix is designed to map the contact forces to the motion of the body. This can be viewed as an active version of RCC, because the compliance is guaranteed by the control law. However, for inserting tasks like fixture loading, the vicinity assumption is not realistic. This is because usually before reaching the vicinity, the position error of the manipulator is already large enough to form contact(s) with some locators. The contact forces will deviate the workpiece from the desired motion, or even immobilize the workpiece. Therefore, A more general problem is to plan and control the overall scheme from the free space to the final full contact position. One class of approaches to this problem rely on an extension of the motion planning in free space. That is, the collision-free path is generated first, then a patch of a refined plan is used in place where the path is prune to collision [13], [15]. A
different approach is so called LMT method, by Lozano-Perez, Mason, and Taylor[16], where the planning starts from the final position and search for the preimage that includes the initial position and results in this final position. One issue in the planning of fixture loading is to deal with the contact forces. If there is no friction, all contact forces are perpendicular to the contact surfaces at the contact points. The mapping of contact forces to the body frame is dual to the mapping of body velocity[9] to contact velocities. Therefore, a problem involving frictionless contact forces can always be converted to a problem related to velocity. However, when there are frictional forces between the contacted surfaces, the problem of solving the contact forces and the motion of the workpiece will be indeterminate. Usually friction is modelled by Coulomb’s friction model. With this model, the contact forces are always within the friction cones, and if rolling contact occurs, the direction of the contact force at the contact point will be anywhere within the friction cone. Therefore, the duality between the contact forces and velocity will no longer exist. Because of indeterministic contact forces, the motion of the manipulated body is also difficult to determine. That is why many inserting schemes, such as RCC or LMT methods require sliding contacts. A companion issue to indeterministic contact forces is the uncertainty. This is not critical when planning motion in free spaces, mainly because the system dynamics does not change and can be estimated well. However, when contact involved, a large deviation from planned path can be generated from a small initial error. The contact forces may also be large enough to resist motion and result in mechanical failure. Since fully automatic planning for fixture loading is difficult, human interaction can be a promising way to solve the problem. Two strategies are available: physical prototypes and virtual prototypes. Physical prototypes are generally time consuming and costly. On the other hand, haptic interaction and virtual reality enable simultaneous feeling of contact force and vision feedback, and thus make realistic simulation feasible. One can feel with his fingers or hands the resultant force from contacts and motion of the workpiece resulted from the applied force. With these information and human experience, he can easily find the appropriate force to push the workpiece to the right contact state. Incorporating the haptic ’hints’, the planning can be made robust [17]. On the other hand, for real-time haptic ‘display’ of the contact force, the simulation must be performed with a high update rate (≥ 1000Hz) [18]. The speed and accuracy of the simulation is also a key problem in haptic interaction. This paper will give an outline of such a haptic simulation based fixture inserting planning system. In this system, the transition of contact states and exerted force are planned instead of the free-space path. The system is composed of three parts: multi-body contact dynamics engine, automatic planning subsystem, and haptic simulation interface. The multi-body contact dynamics engine is to compute the corresponding motion of the workpiece and contact forces when manipulator applying forces on the workpiece. Automatic planning subsys-
191
tem is to find a trajectory to a subgoal and find appropriate exerted forces for the workpiece to have such a trajectory. II. M ULTI - BODY C ONTACT DYNAMICS In a rigid-body model, the interaction between bodies is approximated as a collision or a unilateral contact, and the experimental laws (such as the restitution law and Coulomb’s friction law) are used to describe these phenomena. A major disadvantage of this approximation is the discontinuity in velocities which results in the impulsive contact forces. These discontinuous quantities are no longer ordinary functions of time but rather distributions or measures, and their associated differential equations must be treated as differential inclusions. Here, we use a method based on an extension of explicit time-stepping scheme and an application of the differential inclusion process introduced by J. J. Moreau[19]. From a differential kinematic analysis of contacts, we derive a set of transfer equations in the velocity-based time-stepping formulation. In applying the Gauss-Seidel iterative scheme, the transfer equations are combined with the Signorini conditions and Coulomb’s friction law. The contact forces are properly resolved in each iteration, without resorting to any linearization of the friction cone. The following is a brief introduction to this method. The mathematical model includes three major components: contact kinematics of smooth-rigid bodies, dynamics of motion discretized with a time-stepping scheme, and appropriate contact laws governing the non-penetration, collision and Coulomb’s friction conditions. The contact kinematics refers to the kinematics relationship between the internal geometry and external coordinate frame, and the transformation between body motion and contact point motion. The following contact differential equation describes the relationship between the internal geometry and external coordinate frame. Vlg lf = Gφ˙
(1)
where, lf and lg are local frames fixed with respect to the body frames, and at the time of interest they coincide with the contact frame, Vlg lf is the body velocity of frame lf with respect to lg , φ = [uf , vf , ug , vg , d, ψ]T is the meshing parameters. The transformation between body motion and contact point motion is Vlg lf = Adglf f (Vwf − Adgf g Vwg ),
(2)
where, Adgab is the adjoint transformation of the transformation matrix gab as defined in [20]. The contact dynamics of time-stepping is derived by discretizing Newton-Euler Equations. The final form is given as follows. Pnc h vi+1 = (3) c=1 Fc )i + vi m (Fext + ωi+1 =
−1
T Ri+1 I b Ri+1 · Pnc Pnc Tc )i h ((Text + c=1 pc × Fc + c=1
+Ri I b
−1
RiT ωi )
(4)
where the contact forces Fc and torques Tc (c = 1, · · · , nc ) are understood to be unknowns and to be solved for. The contact transfer equations is to connect the contact velocity (of the next time-step) with the contact forces. It is a linear transformation when current configuration is decided. Following is its formula. vˆj = α ˆ j + Bjj Fˆj Pnc α ˆ j = αj + k=1,k6=j Bjk Fˆk (j = 1, · · · , nc )
(5)
where, Fˆj vˆj
= [Fx , Fy , Fz , Tz ]T = [vx , vy , vz , ωz ]T
(6) (7)
Contact laws are empirical formulae to describe the physical behavior of two rigid bodies in unilateral contact. In general, if there is a normal relative velocity with contact, there will be a collision and the two bodies cannot penetrate into each other and may maintain their contact or sperate. This process is governed by the so called Signorini conditions. When two bodies are in contact, the friction forces at their contact point are usually described by Coulomb’s law. Signorini condition can be written as vz ≤ 0; Fz ≤ 0; vzT Fz = 0 ,
(8)
or, in a compact form, as 0 ≥ vz ⊥ Fz ≤ 0. Coulomb’s friction law is given as Fx2 + Fy2 ≤ µ2 Fz2
(9)
Fy2 Fx2 Tz2 + + ≤ µ2 Fz2 , e2x e2y e2r
(10)
or,
Together with the contact transfer equations, the contact forces can be resolved from contact laws when there is only one contact point. For multiple contacts, we use a Gauss-Seidel scheme to resolve multiple contacts. Gauss-Seidel scheme is to use an iterative process to find a solution for the contact dynamics defined by the transfer equation (5), the Signorini conditions (8), and Coulomb’s law (9) or (10). One such approach is to use the Gauss-Seidel iteration method first suggested by Moreau [19], [21], who has developed a sound mathematical justification based on the theory of convex analysis and differential inclusion. It is essentially a method of propagation of contact among the multiple, simultaneous contacts. At any step of the iteration, only one contact is updated and all other contacts are assumed to have been resolved previously. Starting from an initial guess of all of the contact forces in the transfer equation (5), one can choose to update the contact forces of any single contact, for example, Fˆj . Therefore, the only unknowns in the transfer equation (5) are the contact force Fˆj and the contact velocity vˆj of this contact, while α ˆ j in the equations is already given. Thus, this transfer equation is then resolved by applying the contact laws for determining Fˆj as to be discussed before.
192
III. N ON -JAMMING C ONDITION During the workpiece loading (or insertion) process, it is unrealistic and difficult to form all six contacts simultaneously. It is a usual strategy to form some contacts first and then to try to form another contact by applying a force on the workpiece while keeping the established contacts from separation. During this latter step, it is possible that the workpiece becomes immobilized by the first formed contacts because of friction forces at the contacts if the applied force results in rolling motion at the frictional contacts. Such immobilization is defined as jamming in this paper, that is, immobilization involving any tangential motion constraint at a frictional contact. For the task of loading a workpiece into the fixture, the goal is to reach the final immobilization of the workpiece by the normal constraints only. While in the process of reaching the goal, any jamming must be avoided. If the workpiece is jammed, the direction and magnitude of the applied force has to be adjusted such that the tangential constraints must be made ineffective. Furthermore, the applied force should also drive the workpiece closer to the final goal. Haptic simulation with force feedback can simplify the task of searching such applying forces, because human can apply his global perception and experience to the task, which is unavailable for the automatic planning [17], [22]. However, when the number of contacts increases, it is not obvious to find the non-jamming applying force even though we know the expected motion of the workpiece. In this case, a scheme to find the non-jamming applied force that result in the expected motion is necessary for a successful planning. In this section, we will analyze the conditions of non-jamming, and to present a scheme to find such non-jamming applying forces. The conclusion drawn in this section will give a sufficient and necessary condition for the jamming criteria, which does not rely on the linearization of the friction cone and treats the two dimensional and three dimensional problems in a unified way. Following are the summery of related results of contact kinematics. Rolling is defined as the tangential relative velocity of the contact points of two bodies is zero, and relative rotational velocity is in the tangent plane. Although degrees of freedom (DOF) of a body in contact with another body are five, DOF of a body rolling on one contact point are only two; and DOF of a body rolling on two contact points is one, that is, rolling about an axis that is coincide with the line crossing the two contact points. If there are more than two contact points, in general (that is, the contacts do not lie on the same line), there will be no motion if you want all the contacts be rolling. The DOFs of a body with n contacts are 6 − n. When the number of the contacts is greater than or equal to 6, the body is immovable while maintaining all the contacts. In case of contacts less than three, there will be only one solution of the contact forces given an exerted force on the body, and the body motion can be computed easily when know all the forces. However, if the contact number is greater or equal to three, it is known as a static indeterminant problem
with the number of the unknown variables greater than the numbers of the equations. Therefore, we only consider this case in the following discussions. Consider a general case, that is, the body has 3 or more contacts, and the normal directions of the contacts do not intersect in one point. In this case, if the body can move while maintaining all the contacts, the only way is all sliding contacts. However, when the body is in motion, finite forces cannot stop the motion abruptly. Therefore, jamming can only occur when the body is, by some chance, at rest. Correspondingly, we define no jamming condition for an exerted wrench as that the wrench cannot be balanced by static contact forces, at the same time, existing contacts are maintained. We first set up the mechanical framework for this problem. For convenience, the body frame of the workpiece is set to coincide with the inertia frame. Let Rf li = [xi , yi , zi ], and T z1 −z1T p∧ f l1 .. G = ... , . T T ∧ zn −zn pf ln T x1 −xT1 p∧ f l1 .. Hx = ... , . T T ∧ xn −xn pf ln T y1 −y1T p∧ f l1 .. Hy = ... , . T T ∧ yn −yn pf ln mI3×3 0 . (11) M= 0 I body Then, the force balance equation can be written as e T F fz fxT fyT = + GT HxT HyT Te T T . (12) M aTuf αuf
For frictional contacts of rigid-bodies, the non-penetration condition and Coulomb’s friction law must be satisfied. Let Gi be the ith row of G. If Gi a < 0, we have fxi = fyi = fzi = 0, corresponding to separation. If Gi a = 0, fxi2 + fyi2 ≤ µ2i fzi2 . Furthermore, if the contact is sliding, fxi = kxi fzi and fyi = kyi fzi
(13)
where, aiy ai , kyi = µ q . kxi = µ q x i2 i2 ai2 ai2 x + ay x + ay
(14)
The above equations refers to that the relative acceleration of the contact points is opposite to the direction of the friction forces (because fz ≤ 0). The no jamming condition is in fact two conditions, that is the exerted force must be resistant to separation, and also
193
resistant to all rolling contacts. For the condition of resistance to separation, we need the following conditions. First, suppose all the contact points can be penetrated freely (that is there is no forces on the contact points), we require e F −1 ≥ 0. (15) GM Te That is the gaps of the contacts should be reduced. Second, we suppose that some contact points do not exist, and let C be the set of points where there is contact, and S be the set of points where there is no contact. Therefore, we require e h i C F fz T T S −1 C C G M + G ≤ 0, H Te ftC e h i fC F T T z GC M −1 + GC = 0. (16) HC e T ftC However, if all the contacts exits, we have e fz T F T G H GM −1 + =0 Te ft
(17)
Here we only consider the case when rank(G) = n, that is, the normal vectors of the contact points are not intersected at one point. As discussed before, n ≥ 3 and the body has a motion while maintaining contacts, ati 6= 0. Therefore, e T fz F T G H H i M −1 + = ait 6= 0 (18) Te ft and fxi = kxi fzi and fyi = kyi fzi
(19)
where, kxi = µ q
aix i2 ai2 x + ay
6= 0 and kyi = µ q
aiy
6= 0
(20)
i2 ai2 x + ay
Now we conclude with the following theorem. With it, we can determine whether the applied force to the body results in jamming, and can further find a no jamming exerted force. The detailed proof will be found in later papers. Theorem 1: For a given applied force [F eT , T eT ]T satisfying satisfying (15), if it results in a contact force vector [fzT , ftT ]T satisfying (17), (18), (19) simultaneously is found, then the applied force [F eT , T eT ]T will definitely result in sliding motion of the body at all of the contacts with the contact forces [fzT , ftT ]T . There are two remarks for this theorem. First, since the state of all sliding contacts is a sufficient condition for non-jamming, the [F eT , T eT ]T satisfying (15), (18), and (19) will definitely result in non-jamming. Second, for n ≥ 3 and considering frictional moment, it is impossible for any one of the contacts to become rolling unless the configuration constraints are satisfied. If the configuration constraints are not satisfied, then the condition of non-jamming is equivalent to having sliding on all of contact points. Therefore, the condition for an applied force to make
all sliding contacts is a necessary and sufficient condition for non-jamming for n ≥ 3. This theorem can serve as guideline for finding the applied force to make all contacts sliding.
No. CUHK4376/02E), and the Natural Science Foundation of China (NSFC) (Young Overseas Investigator Collaboration Award 50128503).
IV. H APTIC - BASED I NSERTING P LANNING
[1] Matthew T. Mason. Mechanics of Robotic Manipulation. The MIT Press, 2001. [2] J. C. Trinkle and D. Balkcom. Computing wrench cones for planar contact tasks. In IEEE International Conf. on Robotics and Automation, pages 869 – 875, May 2002. [3] Joseph M. Schimmels and Michael A. Peshkin. Force-assembly with friction. IEEE Transaction on Robotics and Automation, 10(4):465–479, August 1994. [4] Devin Balkcom and J. C. Trinkle. Computing wrench cones for planar rigid body contact tasks. The International Journal of Robotics Research, 21(12):1053–1066, December 2002. [5] Paul D. Campbell. Basic Fixture Design. Industrial Press, New York, 1994. [6] H. Asada and A. By. Kinematic analysis of workpart fixturing for flexible assembly with automatically reconfigurable fixtures. IEEE Transaction on Robotics and Automation, RA-1(2):86 – 94, 1985. [7] J. S. Carlson. Quadratic sensitivity analysis of fixturing and locating schemes for rigid parts. ASME Journal of Manufactring Science and Engineering, 123(3):462 – 472, 2001. [8] M. Y. Wang and T. Liu. Fixture kinematic analysis based on the full contact model of rigid bodies. In Proceedings of IEEE International Conference on Intelligent Robots and Systems (IROS), pages 1602 – 1607, October 2002. [9] Joseph M. Schimmels and Michael A. Peshkin. Force-assemblability: Insertion of a workpiece into a fixture guided by contact forces alone. In IEEE International Conf. on Robotics and Automation, pages 1296 – 1301, April 1991. [10] J.S. Pang and J.C. Trinkle. Complementarity formulations and existence of solutions of dynamic multi-rigid-body contact problems with coulomb friction. Mathematical Programming, 73:139 – 154, 1996. [11] Jonghoon Park, Wan-Kyun Chung, and Makoto Kaneko. Active-external enveloping grasping: Dynamical-balance based motion analysis. In IEEE International Conf. on Intelligent Robots and Systems, pages 1566 – 1571, October 2002. [12] D. E. Whitney and J. L. Nevins. The remote center compliance: What can it do? Robotics Today, pages 135–139, Summer 1979. [13] Jan Rosell Gratac`os. Fine-Motion Planning for Robotic Assembly under Modelling and Sensing Uncertainties. Ph. d. thesis, Universitat Polit`echica de catalunya, June 1998. [14] Joseph M. Schimmels and Michael A. Peshkin. Admittance matrix design for force-guided assembly. IEEE Transaction on Robotics and Automation, 8(2):213 – 227, April 1992. [15] Jing Xiao. Planning motion compliant to complex contact states. International Journal of Robotics Research, 20(6):446–465, June 2001. [16] T. Lozano-Perez, M. T. Mason, and R. H. Taylor. Automatic synthesis of fine-motion strategies for robots. International Journal of Robotics Research, 3(1):3–24, 1984. [17] Nancy M. Amato and O. Burchan Bayazit et al. Providing haptic ’hints’ to automatic motion planners. Technical report 98-026, Department of Computer Science Texas A&M University, November 1998. [18] Mandayam A. Srinivasan and Cagatay Basdogan. Haptics in virtual environments: Taxonomy, research status, and challenges. Computer and Graphics, 21(4):393 – 404, 1997. [19] J. J. Moreau and Michel Jean. Numerical treatment of contact and friction: the contact dynamics method. Engineering Systems Design and Analysis, 4:201–208, 1996. [20] R. M. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, Boca Raton, FL, 1994. [21] J. J. Moreau. Numerical aspects of the sweeping process. Computer Methods in Applied Mechanics and Engineering, 177(1999):329–349, 1999. [22] D. D. Nelson, D. E. Johnson, and E. Cohen. Haptic rendering of surfaceto-surface sculpted model interaction. In 8th Annual Symp. on Haptic Interfaces for Virtual Environment and Teleoperator Systems, November 1999.
In this section, we briefly introduce the planning scheme for fixture loading operation. As discussed before, fixture loading has its specialties, like there is only 6 locators in a general three dimensional case; surfaces are contacted only on one point, and the final state is at the point when all 6 contacts are formed. Considering these specialties, we divide the planning scheme into two steps. The central point of the scheme is that the planned motion should make the contact points increasing monotonously. The first step is a coarse plan, which is similar to LMT. That is, we define the set of states that can be recognizably (with force sensors) attained, and select the pre-images that connect the final state to the initial state. The difference from our method and the standard LMT is that in standard LMT, the way from one state to another is only a simple operation, with polyhedral bodies, it may work well; however, for fixturing with curved surfaces, one simple operation usually cannot fulfil the task. Therefore, we have a second step, which is to determine the strategy from one state to another. Although in general cases, LMT is computationally ineffective, for fixture loading, it is very simple, since there are only point contacts. Our recognizable states only refer to the number of contact points and with which locator the workpiece contacts. This step can be done automatically, or by user assignment. For the second step, in each state transition, we plan two things: the body velocity and the exerted force by the manipulator. When contact points are less than three, we require the body in rolling contact with the locators to approach another locator. That means the contact forces are in the friction cones. When contact points increase to three or more, rolling contact cannot occur, therefore, we require the body have a no jamming motion while maintain the contacts. The contact velocities and forces can be computed with the method introduced in the former section. Haptic simulation are used in the second step so that the user can suggest a body velocity when planning cannot process, and the user can also use haptic simulation to verify the planned scheme off-line. V. C ONCLUSION AND F URTHER W ORK The paper presents a haptic-guided fixture loading planning system. The system is based on a multi-body contact dynamics solver. An criteria of no jamming condition is given, and the planning scheme that can monotonously increase the contacts with locators is briefly introduced. ACKNOWLEDGMENT The research work is supported in part by the Hong Kong Research Grants Council (Projects No. CUHK4217/01E and
194
R EFERENCES
