BOSTON UNIVERSITY COLLEGE OF ENGINEERING. Dissertation MODELING BY MANIPULATION ENHANCING ROBOT PERCEPTION THROUGH CONTACT STATE ESTIMATION

Transcription

1 BOSTON UNIVERSITY COLLEGE OF ENGINEERING Dissertation MODELING BY MANIPULATION ENHANCING ROBOT PERCEPTION THROUGH CONTACT STATE ESTIMATION by THOMAS DEBUS B.S., University o Versailles, 1995 M.S., Boston University, 2000 Subitted in partial ulillent o the requireents or the degree o Doctor o Philosophy 2005

2

3 Acknowledgeents First and oreost, I would like to thank Proessor Pierre E. Dupont. His guidance and support have been invaluable, and his constant encourageents have allowed e to stay positive and deterined in y research. Above all, he showed aith in y work and y ideas; without hi, I could not have written this thesis. I would like to express y gratitude to Proessor Robert D. Howe or his enthusias, great insights and support. Not only did he contribute greatly with his ideas and knowledge, but he also opened the doors to his biorobotics laboratory. I would also like to thank Proessor John B. Baillieul and Proessor David A. Castañon or being y coittee ebers. Thanks to y any riends and colleagues at Boston University over the years. In particular I want to thank y ellow ebers o the Intelligent Mechatronics Laboratory. Special thanks to Je Stoll or the any valuable discussions about contact state estiation. I would also like to extend thanks to the entire sta o the Aerospace and Mechanical Engineering or their help and kindness. Finally, I would like to thank y other and brother or their unconditional love, encourageents, and support or any years. This dissertation is dedicated to the. The unding or this research was generously provided by the National Science Foundation. iii

4 MODELING BY MANIPULATION ENHANCING ROBOT PERCEPTION THROUGH CONTACT STATE ESTIMATION (Order No. ) THOMAS DEBUS Boston University College o Engineering, 2005 Major Proessor: Pierre E. Dupont, Ph.D., Associate Proessor o Aerospace and Mechanical Engineering. ABSTRACT Autonoous task perorance is the ultiate goal o robotics. To this end, enhancing robot perception, the ability o a robot to recognize and odel its environent, is essential. This thesis presents design tools or contact-based perceptual systes applicable to anipulation tasks which can be described by sequences o contact states between rigid objects. The undaental coponent o such systes is a contact state estiator. This estiator uses sensor data collected as objects are anipulated to deterine the sequence o actual contact states ro a network o possible contact states. In the process, it also estiates the paraeters, e.g., geoetric paraeters, o the individual contact state odels. In this thesis, a rigorous approach to contact state estiator design is proposed which involves characterizing our properties o a given contact state network, set o sensors, and associated contact state odels. These properties are: (1) the distinguishability o contact states, (2) the observability o each contact state inside the state network, (3) the iv

5 identiiability o each contact state odel s paraeters, and (4) the excitability o the sensor inputs to perit estiation o all the paraeters associated with a contact state odel. The irst two properties address the easibility o contact state estiation, while the last two address the easibility o estiating the paraeters o the individual contact states. The ajor contribution o this thesis is the developent o a uniied analytic approach to testing the distinguishability o contact states and the identiiability o their paraeters. The testing ethod is applicable to any contact state odel regardless o the chosen sensing odality. The concept o contact observability is also introduced as a orward projection o the paraeter history associated with the execution o the task. The eect o the sensor signals on the paraeters is analyzed by studying the invertibility o an excitability atrix representing the relationship between the structure o the contact states and the sensor signals. Finally, an ipleentation o a contact state estiator is presented using a hidden Markov odel to cobine a nonlinear least squares estiator with prior inoration ro a contact state network. v

6 Table o Contents 1. Introduction Contact State Estiator Design o a Contact State Estiator Ipleentation o a Contact State Estiator Assuptions Outline o the Thesis Prior Work Contact State Estiator Task Representation Contact State Modeling Task Encoding Contact State Identiiability and Distinguishability Data Excitation Contact State Observability Paraeter Estiation Contact State Estiation Thesis Objectives vi

7 3. Topology and Modeling o Contact States Contact State Topology Single Contact State Multiple Contact States Contact State Modeling Contact State Paraeterization Pose Representation Wrench and Twist Representations Relationship Between the Pose and Twist Equations A Contact State Modeling Exaple Pose Equation Reciprocity Equation or the Measured Twist Reciprocity Equation or the Measured Wrench Suary Identiiability and Distinguishability Testing Contact State Distinguishability and Identiiability Deinitions Taylor Series Testing o Distinguishability and Identiiability Distinguishability Testing Identiiability Testing Exaples Exaple 1 Distinguishability o Polygonal Vertex Edge Contacts Exaple 2 Identiiability o a Polygonal Vertex Edge Contact Exaple 3 Identiiability o a Polyhedral Vertex Face Contact Exaple 4 Distinguishability o Polyhedral Vertex Face Contacts Coputational Coplexity analysis Identiiability Testing Distinguishability Testing Suary vii

8 5. Contact State Observability and Task Encoding Task Representation Contact Topology Task Decoposition Contact State Observability Distinguishable State Graph Observability Conjecture Task Encoding Contact State Network Selection o the Probability Transition Matrix Suary Paraeter Estiation and Data Excitation Estiation Algorith: Iplicit Solution The Levenberg-Marquardt Algorith Estiator ipleentation Excitation Condition Estiation Algorith: Explicit Solution Estiation Through Dierentiation Estiator Ipleentation Excitation Condition Suary Contact State Estiation Using a Hidden Markov Model The Hidden Markov Model Fraework HMM Structure HMM Based Segentation HMM Ipleentation Experiental Contact State Estiation Syste Coniguration Contact State Equations viii

9 7.2.3 Excitation Analysis Contact State Network HMM Ipleentation Sensitivity o Contact State Estiation to Robot Path Alternate Forulation o the Observation Probability Distribution Suary Conclusion Thesis Contributions Future Research Directions Appendix A Appendix B Bibliography Vita ix

10 List o Tables Table 3.1: Planar single contacts between a anipulated object and a ixed object.. 47 Table 3.2: Spatial single contacts between a anipulated objects and a ixed object.48 Table 3.3: Degenerate contact states Table 3.4: Double contacts or polygons Table 4.1: Bounds on the nuber o equations needed or identiiability testing Table 4.2: Bounds on the nuber o equations needed or distinguishability testing.99 Table 7.1: Identiable paraeters x

11 List o Figures Figure 1.1: Robot classiication...2 Figure 1.2: Peg-in-hole insertion decoposed into a sequence o contact states...4 Figure 1.3: Perceptual syste applicable to anipulation tasks...5 Figure 1.4: The seven design steps o a contact state estiator...8 Figure 1.5: Ipleentation scheatic o a contact state estiator...11 Figure 2.1: Exaple o orward projection associated with a our-state graph...37 Figure 3.1: Sequence o three contact states used to place a block in a corner...45 Figure 3.2: An edge-edge contact and its topologically equivalent single contact...50 Figure 3.3: Exaple o raes assignents or our dierent types o contacts...54 Figure 3.4: Constraint coniguration o a robot due to a planar vertex-edge contact...57 Figure 3.5: Kineatic closure equation...59 Figure 3.6: Representation o a vertex-edge contact. a) Representation o the contact in the Cartesian space or one coniguration, b) Representation o the contact in the Cartesian space or ultiple conigurations, and c) Representation o the contact in the coniguration space...68 Figure 3.7: Single vertex-edge contact between two polygons...70 Figure 4.1: Contact between two polygons. a) Contact between a vertex o the anipulated object and an edge o the ixed object, and b) Contact between a vertex o the ixed object and an edge o the anipulated object...82 Figure 4.2: Vertex Face contact state...86 xi

12 Figure 4.3: Two vertex ace contact states in which the location o the contact point is the sae on the anipulated object but on two orthogonal aces o the environent object...90 Figure 5.1: Block-in-corner assebly task Figure 5.2: Contact state topology o a block-in-corner assebly task Figure 5.3: Contact state graph representing a block-in-corner assebly task Figure 5.4: Distinguishable contact state graph Figure 5.5: Exaple o a contact sequence that perits estiation o all anipulated and environent object odel paraeters Figure 5.6. Forward projection on the paraeter history. a) Contact state sequence in which the block is rotated clockwise and then slid toward the botto edge o the ixed corner, and b) The paraeter history is cobined with the knowledge o the objects shape resulting in a 2D odel o the anipulated object and its environent Figure 5.7: Distinguishable state network Figure 6.1: The two parts o the ipleentation o a contact state estiator: ultiple odel estiation and contact state estiation Figure 6.2: Probe oving on an inclined line. a) The probe is pivoting around the contact point resulting in a poor exciting path. b) The probe is sliding with a constant orientation resulting in a poor exciting path. c) The probe is sliding and rotating resulting in an exciting path Figure 6.3: Estiation using a sliding window estiation schee Figure 6.4: Contact s residual pd by Monte Carlo siulation. a) A siulated coniguration o a vertex-to-edge contact, b) Contact s residual pd corresponding to objects o ixed sizes and locations, and c) Contact s residual pd corresponding to objects o dierent sizes and locations Figure 6.5: Sensor path associated with a vertex-edge contact Figure 6.6: Noise-ree explicit and iplicit paraeter estiations Figure 6.7: Noisy explicit and iplicit paraeter estiations Figure 7.1: Ipleentation scheatic o the Viterbi algorith Figure 7.2: Possible sequence o contact states during peg-in-hole insertion Figure 7.3: Experiental apparatus - PHANToM 1.5, cylindrical peg and orientable hole xii

13 Figure 7.4: Multi-pass estiation. a) Initial estiation residuals, b) Estiation residuals coputed using paraeter estiates propagated ro C 2 Figure 7.5: Contact state networks. a) Four-state network based on the contact states o Fig. 7.2, b) Two-state network or distinguishing contact state C ro all other possible contact states Figure 7.6: State estiation o the ost likely insertion state sequence Figure 7.7: Segentation o an atypical state sequence xiii

14 Chapter 1 Introduction Robotic systes are used in a variety o anipulation tasks ranging ro coputer chip assebly to undersea connector ating. A vast ajority o these systes can be classiied into three categories: a) teleoperated systes, b) sei-autonoous systes, and c) autonoous systes, as illustrated in Fig Each o these systes is deined by two capabilities. The irst is perception, which includes sensing and odeling the environent in which the robot operates. The second is action, which includes planning the sequence o otor coands to accoplish the task and controlling their execution. The discriinating actor between these three systes is whether the huan or the achine is in charge o perception and action. Teleoperated and sei-autonoous systes are coonly used to peror anipulation tasks in poorly known environents since huans can adapt and react quickly to unplanned situations (e.g., space exploration, nuclear aterial handling, undersea oil developent). Autonoous 1

15 systes, on the other hand, are currently restricted to assebly tasks in well-odeled environents (e.g., part ating assebly) since achine perception is liited in poorly known environents. (a) TELOPERATION (b) SEMI-AUTONOMY (c) AUTONOMY GOAL HUMAN ACTION Planning, control ROBOT + ENVIRONMENT GOAL HUMAN / MACHINE ACTION Planning, control ROBOT + ENVIRONMENT GOAL MACHINE ACTION Planning, control ROBOT + ENVIRONMENT HUMAN PERCEPTION Sensing, Modeling HUMAN / MACHINE PERCEPTION Sensing, Modeling MACHINE PERCEPTION Sensing, Modeling Figure 1.1: Robot classiication. A central goal o robotics research is the creation o robotics systes that can peror tasks autonoously in poorly known environents. To this end, enhancing the ability o a robot to perceive and odel its environent is essential. In anipulation tasks, this level o perception depends on the undaental ability o the syste to detect and control contacts between anipulated objects. At each step o task execution, otion planning and control involve oving ro one contact state to another. In these situations, the robot ust be able to recognize and distinguish aong all o the contact states involved in task execution. Furtherore, in order to ipleent contact-based otion planning and control laws, it is necessary to estiate during contact the paraeters (e.g., geoetric paraeters) describing the contact. These two probles o contact state estiation and paraeter estiation constitute the core o perceptual systes applicable to anipulation 2

16 tasks. In such systes, a undaental tool to solve this dual estiation proble is a contact state estiator. This estiator uses sensor data collected as objects are anipulated to deterine the sequence o actual contact states ro a network o possible contact states. In the process, it also estiates the paraeters o the individual contact state odels. The objective o this thesis is to provide design tools that assess the easibility o ipleenting a contact state estiator given a set o sensors, a list o paraeterized contact state odels, and a contact state network representation o the task. This research addresses our iportant properties required or the ipleentation o the estiator. These properties are: (1) the distinguishability o contact states, (2) the observability o each contact state inside the state network, (3) the identiiability o each contact state odel s paraeters, and (4) the excitability o the sensor inputs to perit estiation o all the paraeters associated with a contact state odel. This chapter provides an overview o the construction o a contact state estiator. In particular, it is shown that the design and ipleentation o an estiator requires the solution to nine sub-probles, seven o which are part o the design process. The next section introduces and deines these sub-probles. The ollowing section lists the assuptions on the contact odels utilized in this thesis and deines the concept o poorly known environents with respect to these assuptions. An outline o the work presented in this thesis is described at the end o the chapter. 3

17 1.1 Contact State Estiator Manipulation tasks can be represented as collections o contact states that describe how the anipulated object is in contact with objects in the environent. As an exaple, Fig. 1.2 illustrates a possible sequence o contact states associated with a planar peg-in-hole insertion task. The nuber o contact states corresponds to the nuber o possible contacts between the eatures o the anipulated peg and its environent (e.g., vertices, edges). In this exaple, the set o all possible states coprises all the one-point contacts, two-point contacts and line contacts between the peg and the hole. Note that Fig. 1.2 shows one possible contact state sequence; however, other sequences exist. robot gripper peg hole Figure 1.2: Peg-in-hole insertion decoposed into a sequence o contact states. The copletion o the peg-in-hole insertion task shown in Fig. 1.2 requires a control algorith that depends on the nature o the contact states as well as the paraeters characterizing these contacts (e.g., location o the hole, diension o the peg). For exaple, dierent control laws ust be applied whether the peg is inside or outside the 4

18 hole. Siilarly, the gains o the controller ust be set dierently i the hole is tilted or not. In the context o poorly known environents; however, this inoration is partially known and needs to be accurately estiated using a contact state estiator. As shown in Fig. 1.3, contact state estiation in a poorly known environent is a dual proble involving the estiation o contact paraeters as well as contact states, given a task description and a set o sensors. These two estiates can then be used in a control algorith and a otion planner to achieve robot autonoy. Task Description INPUTS Sensor Data CONTACT STATE ESTIMATOR Estiated Contact State PERCEPTION Estiated Model Paraeters MOTION PLANNER ACTION CONTROL ALGORITHM Figure 1.3: Perceptual syste applicable to anipulation tasks. 5

19 1.1.1 Design o a Contact State Estiator Given a anipulation task (e.g., pick-and-place, peg-in-hole insertion) and a list o contact states describing the possible geoetric interactions aong the objects in contact (i.e., contact topology), the goal o the design process is: 1) to encode the task into a network o states in which each node represents a possible contact state, and 2) to odel each contact state as an equation that relates the sensors o the anipulating robot and the paraeters o the objects in contact. As illustrated in Fig. 1.4, seven probles need to be solved when designing a contact state estiator: 1) task representation, 2) contact state odeling, 3) task encoding, 4) contact state identiiability, 5) data excitation, 6) contact state distinguishability and 7) contact state observability. Contact state odeling and task encoding constitute the outputs o the design process. The last our probles o Fig. 1.4 are used to veriy that these two outputs satisy the conditions o identiiability, excitability, distinguishability, and observability necessary to the ipleentation o a contact state estiator. As noted in the igure, detailed descriptions o these seven probles are presented in various chapters o the thesis. Brie deinitions o the probles are as ollows: 1) Task representation: The objective o this step is to resolve the task into a sequence o contact states between the anipulated object and the ixed objects in the environent. This procedure is divided into two parts. First a list o eleentary contact states (i.e., contact topology) representing the possible contact interaction between the objects in the task is chosen. Then, these eleents are used to decopose the task into a graph o possible contact state sequences. 6

20 2) Contact state odeling: Matheatical odels are needed or each contact state. This odeling proble is addressed by inding constraint equations that are used to relate the sensor data, st (), and the paraeters p characterizing the objects in contact. 3) Task encoding: The goal o the encoding phase is to ind a atheatical way o representing the inoration associated with the task. The proposed solution is to represent the contact state graph using a probability transition atrix in which each eleent corresponds to the likelihood o transition between states. 4) Contact state identiiability: The identiiability test veriies that the paraeters associated with each contact state odel can be estiated ro the available sensing odalities. 5) Data excitation: Sensor data need to be suiciently exciting to estiate all the paraeters associated with a contact state odel. The objective o the data excitation step is to ind sensor paths that can ensure the estiation o all the paraeters associated with a given contact odel. 6) Contact state distinguishability: Distinguishability tests i the proposed contact state odels and associated sensors are suicient to disabiguate each contact state ro the others in a given list o eleentary contact states. For a given task, the result is to asseble contact states into subsets which are distinguishable ro each other. 7) Contact state observability: Observability veriies that each state in a given contact state network is distinguishable using sensor and task inoration. This iplies that task inoration is suicient to disabiguate the contact states within each distinguishable subset o states. 7

21 robot gripper peg hole TASK 1. TASK REPRESENTATION Contact Topology (chapter 3) Task Decoposition (chapter 5) C1 C2 C3 C C 1 2 C 4 2. CONTACT STATE MODELING (chapter 3) ( ) ( ) C : s( t), p = C2: 2 s( t), p2 = 0 3. TASK ENCODING (chapter 5) 4. CONTACT STATE IDENTIFIABILITY (chapter 4) 6. CONTACT STATE DISTINGUISHABILITY (chapter 4) 5. DATA EXCITATION (chapter 6) 7. CONTACT STATE OBSERVABILITY (chapter 5) Figure 1.4: The seven design steps o a contact state estiator. 8

22 As illustrated in Fig. 1.4, the sub-probles associated with the design o a contact state estiator can be interdependent. For exaple, distinguishability, identiiability, and data excitation use analytical techniques that are based on the equations provided by the odeling step o the design process. Observability analysis requires distinguishability testing to investigate the distinguishability o every contact state inside a given contact state graph. Siilarly, the data excitation step utilizes contact state odels that have been shown to be identiiable since the paraeters associated with an unidentiiable contact odels cannot be estiated regardless o the chosen sensor path. Contact state odeling and task encoding result in atheatical representations o both the contact states and the contact state graph that are then utilized in the ipleentation o the estiator. As shown in Fig. 1.4, the design o the contact state odels and the design o the contact state network are iterative processes that depend on the results o the identiiability test, data excitation test, and observability test. For exaple, i contact odels are shown to be unidentiiable, then they need to be reodeled (e.g., alternate sensing odality) or reoved ro the list o candidate contact states. Siilarly, an unobservable contact state network ust be odiied until it satisies the observability property Ipleentation o a Contact State Estiator Given a set o sensors, a list o paraeterized contact state odels, and a contact state network representation o the task, the ipleentation phase ocuses on building a contact state estiator that generates the ost likely sequence o contact states 9

23 corresponding to the data as well as the states associated paraeters. As illustrated in Fig. 1.5, this ilter requires an estiation algorith and a detection algorith. These are deined as ollows: 8) Multiple Model Estiation: Given a sequence o sensor data points and a contact state network obtained ro the design phase, a ultiple odel estiation algorith is ipleented to estiates the paraeters and residual associated with each contact odel o the state network. The ipleentation o a ultiple odel estiation algorith using sliding nonlinear least squares is presented in Chapter 6. 9) Contact State Estiation: The decision algorith is built around a decision test that requires two inputs. First, estiation residuals are used to judge how well a portion o the data strea its the possible contact states. Second, conditional probability theory is used to adjust these results to account or knowledge o prior and anticipated contact states as ebodied in the task s contact state network created in the design phase. A decision test based on hidden Markov odel is discussed in Chapter 7. 10

24 Contact State Modeling Sensor Data Task Encoding MULTIPLE MODEL ESTIMATION Contact odel C : ( s( t), p ) = Contact odel C : ( s( t), p ) = Contact odel C : ( s( t), p ) = 0 n n n 8. ESTIMATION ALGORITHM (chapter 6) Paraeter estiate p 1 and residual ε p 1 Paraeter estiate p 2 and residual ε p 2 Paraeter estiate and residual ε p n p n CONTACT STATE ESTIMATION 9. DECISION ALGORITHM (chapter 7) PERCEPTION Estiated Model Paraeters Estiated Model Paraeters MOTION PLANNER ACTION CONTROL ALGORITHM Figure 1.5: Ipleentation scheatic o a contact state estiator. 11

25 1.2 Assuptions Without loss o generality, contact odels are considered or pairs o objects. One object, tered the anipulated object, is assued to be gripped and anipulated by a robot. The second object is called the environent object. The concept o a poorly known environent is deined by the ollowing assuptions: Objects are rigid polygons or polyhedrons o known shapes but unknown diensions. The anipulated object does not slip in the gripper. The environent object is ixed with respect to a world coordinate rae. In the ost general case, the coniguration (position and orientation) o the anipulated object with respect to the gripper is unknown and the coniguration o the environent object with respect a world rae is unknown. The paraeters associated with the objects conigurations (6 or polygonal odels and 12 or polyhedral odels) constitute unknown paraeters in the contact odels. Contact odels are coprised o nonlinear equalities involving coniguration paraeters and sensor variables. Inequality constraints (e.g., overlap constraint and non-penetration constraint) are not considered. Uncertainty in sensor variables (noise) is not considered in the design phase o the estiator; however, it is considered in its ipleentation phase. 12

26 These assuptions can be directly related to the our types o uncertainties typically considered in the literature: sensing uncertainty, anuacturing uncertainty (i.e., uncertainty in the shape and diension), uncertainty in the location o the anipulated object inside the gripper, and uncertainty in the location o the static objects inside the environent (Rosell et al. 2001). In this thesis, sensing uncertainties are iplicitly considered in the ipleentation o the contact estiator but not in its design. The eect o anuacturing on the shape o the objects is not considered (i.e., a lat surace is not considered as a curved surace due to anuacturing uncertainties). The last two types o uncertainties are considered since the locations o the anipulated and ixed objects are assued to be unknown. 1.3 Outline o the Thesis Chapter 2 presents a literature survey on contact estiation and discusses its relevance to contact state estiation in poorly known environents. First, prior work on contact state estiation is reviewed with respect to the nine sub-probles presented in Fig. 1.4 and Fig Next, the liitations o the prior work in the context o poorly known environents are discussed and related to the contributions o this thesis. Figures 1.4 and 1.5 provide a ap or the organization o this thesis and illustrate how the construction o a contact state estiator is analyzed throughout the chapters o this docuent. Chapter 3 starts by reviewing the contacting topology used to describe contact states geoetrically. The second part o this chapter describes the odeling o contact 13

27 states using pose, velocity and orce easureents. In particular, it is shown how contact states can be odeled as hoogeneous nonlinear algebraic equations paraeterized by tie-dependent sensor data and tie-independent paraeters representing the unknown locations and positions o the two objects in contact. An illustration o the three odeling techniques is provided at the end o the chapter. Chapter 4 deines the probles o distinguishability and identiiability in the context o contact state estiation. Both probles are based on inding sets o tie-independent paraeters that can satisy the contact odels siultaneously. As one o the ajor contributions o this thesis, a uniied solution to the two probles is proposed using Taylor series expansion. In this raework, the structure o the contact odels is decoposed as sets o algebraic equations given by the Taylor coeicients. Distinguishability and identiiability o contact state odels are then analyzed by solving these sets o equations or the tie-independent unknowns paraeterizing the contact states. This technique is ipleented or several contact states odeled using kineatic sensing. A coplexity analysis, based on the structural properties o the kineatic equations is presented at the end o the chapter. Chapter 5 addresses the proble o task easibility in poorly known environents. To this end, the observability o the contact state graph representing the task is analyzed. A solution to this proble is given by cobining distinguishability testing with the inoration contained in the history o the estiated paraeters associated with the 14

28 execution o the task. Once the contact state graph is shown to be observable, the encoding o the task is addressed using a probability transition atrix. Chapters 6 and 7 ocus on the ipleentation o a contact state estiator. The realization o an estiation algorith suited to paraeter estiation in poorly known environents is discussed in Chapter 6. First, the Levenberg-Marquardt, a nonlinear least squares technique shown to be robust to poorly known initial conditions is reviewed as well as the excitation condition necessary to its ipleentation. Thereater, an explicit estiation schee based on ultiple dierentiations and nullspace analysis is presented. This ethod proves to be ast, but too sensitive to noise to be practically ipleented. However, the analytical nature o the technique provides valuable insights on the type o exciting paths necessary or the estiation o the paraeters associated with the contact state odels. Chapter 7 ocuses on the ipleentation o a contact state detection algorith. The assessent is perored by a Hidden Markov Model (HMM), which cobines a easure o how well each set o contact equations it the sensor data with the probability o speciic contact state transitions. The approach is illustrated or a three diensional pegin-hole insertion using a tabletop anipulator robot. Contact states are odeled by pose equations paraeterized by tie-dependent sensor data and tie-independent object properties. At each sapling tie, ultiple odel estiation coupled with an HMM are 15

29 used to assess the ost likely contact state. Using only position sensing, the contact state sequence is successully estiated without knowledge o noinal paraeter values. Chapter 8 concludes the thesis by suarizing its contribution to the ield o contact state estiation. Suggestions or uture research are also presented at the end o the chapter. 16

30 Chapter 2 Prior Work Contact state estiation has been applied to a variety o applications, including odel-based copliant otion (De Schutter et al. 1999, Leebvre et al. 2003), ine anipulation (McCarragher and Asada 1996, Eberan 1997), task onitoring (McCarragher and Asada 1993, Hannaord and Lee 1991), an-achine cooperation (Dupont et al. 1999, Debus et al. 2000) and robot learning (Pook and Ballard 1993, Skubic and Volz 2000). The contact state estiators used in these applications are designed and ipleented using analytical and nuerical tools that can provide solutions to one or several o the sub-probles illustrated in Fig. 1.4 and Fig This chapter presents an overview o these tools and discusses their liitations in the context o poorly known environents. The irst section reviews prior work on contact state estiation and its associated subprobles. This section is divided into two parts. First, a review o contact state estia- 17

31 tors available in the literature is presented. Then the ost relevant techniques ound in the robotic literature with respect to each o the sub-probles deined in Chapter 1 are reviewed. The next section suarizes the liitations o the current techniques applicable to contact state estiation. Based on these liitations, the objectives o this thesis are presented at the end o the chapter. 2.1 Contact State Estiator Using contact states to peror anipulation tasks is not a new concept. The concept originated ro work in ine otion planning and orce control in which researchers quickly realized that a task is a contact-driven process that requires appropriate paraeter-dependent control laws at each o its contact phases. One o the irst papers to propose a solution to this proble is due to Asada and Hirai (1989). Their work oralized the concept o contact state estiation by reducing it to three coponents: sybolic representation o assebly processes (i.e., task representation in this text), contact state odeling, and contact state classiication (contact state detection in this text). This technique was ipleented successully to onitor ten contact states during a rictionless quasi-static part-ating assebly task (Hirai 1994). The approach was liited to tasks with a known geoetry (i.e., object shape and size were known, the location o the object inside the gripper was known, the locations o the objects in the environent were known). Recent works on paraeter-based contact state estiation are ocusing on relaxing these constraints. In this context, contact state estiation becoes a dual coupled 18

32 estiation proble in which both contact states and their paraeters are estiated. Progress in contact state odeling and paraeter estiation has been driven by research in odel-based copliant otion (e.g., Bruyninckx 1995, De Schutter et al. 1999, and Leebvre et al. 2003). In Bruyninckx or exaple, twist and wrench easureent equations are utilized to odel contact states as virtual echaniss paraeterized by the irst and second order geoetric properties o the contact suraces. These paraeterized odels are then used as the easureent equations o a Kalan ilter (De Schutter et al. 1999). Contact transitions are detected by onitoring the consistency o the ilter s innovations using a statistical threshold. In Eberan (1997), contact state estiation is perored using a statistical observer. The observer uses axiu likelihood to estiate the paraeters associated with linear contact odels, and then a sequential hypothesis tester based on the estiation s log-likelihood is utilized to detect contact state transitions. This approach presents the advantage o using a coon language (i.e., probability) to odel and detect the contact states. As a result, contact odels can be easily augented without changing the detection algorith (e.g., add probabilistic ipact odel, add probabilistic vibration odel). Both o these approaches assue relatively sall uncertainty and good noinal values or the paraeters. Thereore, these techniques cannot be used in a poorly known environent; nevertheless, they provide a good oundation upon which to build a contact state estiator. Many o the concepts presented in this thesis have been deined in the context o ine otion planning in sei-structured environents. The objective o the next sections is to 19

33 list the ost relevant techniques with respect to the sub-probles deined in Chapter 1 (i.e., task representation, contact odeling, task encoding, contact state identiiability, contact state distinguishability, contact state observability, data excitation, paraeter estiation, and contact state estiation) Task Representation Two ajor approaches are coonly used to represent tasks: the explicit approach and the odel-based approach (Park 1977). In the explicit approach, the task is represented by the actions that are needed to bring it to copletion (e.g., grasp, ove, insert). In the odel-based approach, the task is described in ters o the geoetric objects involved in its copletion (e.g., polygons, polyhedrons). The lack o detail in the explicit approach can leave soe lexibility in the representation that can lead to ultiple representations. On the other hand, the odel-based approach oers a level o detail that can be used to represent tasks uniquely and autoatically (e.g., Lozano-Perez 1981, Asada and Hirai 1989). This section only reviews prior work on odel-based task representation. Geoetric and orce representations are reviewed with an ephasis on geoetric representation. Geoetry-based Task Representation Two questions ust be addressed when representing a task using geoetry: what are the geoetric building blocks utilized in the representation? How can they be used to 20

34 discretize the task? These two issues are coonly deined in the literature as contact topology and task decoposition. Contact Topology In the context o otion planning, tasks are typically described using rigid concave or convex polygons or polyhedrons (e.g., Abler and Popplestone 1975, Lozano-Perez 1981). This assued geoetry o the task allows or its natural decoposition into sets o priitive geoetric constraints called contact states in Asada and Hirai (1989), eleental contacts in Desai and Volz (1989), or principal contacts in Xiao (1993). Several contacting topologies have been proposed in the literature to describe contact states in ters o contacting eleents such as vertex, edge, and ace (e.g., Lozano Perez 1983, Desai and Volz 1989, and Xiao 1993). The ost coplete topology is provided by Xiao (1993). In this approach, contact states priitives are called principal contacts and are described as pairs o contacting eleents which are not part o the boundaries o other contacting eleents. For exaple, a ace-ace contact is considered as a single ace-ace contact and not as a cobination o vertex-ace or edge-ace contacts. In this raework, Xiao showed that a task can be decoposed into contact orations that can be described by cobining our principal contacts in the polygonal case and ten principal contacts in the polyhedral case. The contact state decoposition o anipulation tasks involving curved objects reains an open proble. Recent progress has been ade by Luo et al. (2004). In this 21

35 work, the concept o principal contact has been extended to curved objects by segenting their curvature into curve segents and surace patches. Task Decoposition While task topology deals with the proble o deining a set o possible contact states that can be used to describe a task, task decoposition looks at the possible connections between contact states. This step is at the root o any otion planning approach since it provides a ap o the possible strategies that can lead to task copletion. A coon tool used in otion planning is the concept o coniguration-space obstacles introduced by Lozano-Perez (1981, 1983). In this representation, the conigurations (i.e., positions and orientations) o the anipulated block in the Cartesian space correspond to points in the coniguration space. The end result o a coniguration space obstacle is a aniold that corresponds to all the possible conigurations that result in contacts between a anipulated object and its environent. In a contact state point o view, this aniold can be partitioned into regions that correspond to the dierent contact states coposing the task (e.g., C-surace in Mason 1981). Coputation o coniguration spaces has been an iportant research topic in the otion planning counity, and as a result any algoriths are available (e.g., gross otion planning survey by Hwang and Ahuja 1992). Nevertheless, the coplexity o the coputation is such that coputing coniguration spaces or coplex 3D odels reains an open proble. As an alternative to coniguration spaces, contact state graphs can also be utilized to decopose tasks (e.g., Asada and Hirai 1989, Desai and Volz 1989, Hirukawa et al. 22

36 1994, and Xiao and Ji 2001). This concept, introduced by Asada and Hirai (1989), reduces a task to a discrete representation given by a graph in which each node represents a contact state and each link represents a possible transition ro a contact state to another. In contrast to coniguration space representations that result in a continuous representation o the task, contact state graphs result in a discrete representation o the task in the Cartesian space. This discretization allows or ast coputation even or coplex 3D odels. This representation perits otion planning by searching paths in the graph that lead to a speciied goal given a speciied starting contact state. Xiao and Ji (2001) have ade a signiicant contribution to contact state graph coputation. In their approach, contact state graphs are built by erging subgraphs that are obtained by repeatedly decreasing the level o constraint associated with the desired inal contact oration. This ethod allows or ast and autoatic generation o contact state graphs or coplex 3D polyhedra. Force-based Task Representation Task representation using orce analysis has been little studied in the literature. Brost and Mason (1989) present a graphical ethod in which a line o orce aps to point in the plane, a riction cone aps to a line segent and ultiple riction contacts ap to a convex polygon. This apping is deined as the orce-dual space. This space is built using three topological eleents: the cobination o the plane o positive oents, the plane o negative oents, and the line o zero oents. This technique has been ipleented as a contact distinguishability tool by Rosell et al. (2001) or analyzing 23

37 planar otion easibility under uncertainty. No extension to three diensions is known to the author. A cobined orce and geoetric representation o tasks is presented by Erdann (1994). In this approach, the concept o C-space is augented by incorporating the constraints that represent the eect o the riction cone. This approach is useul to deterine the possible otions o an object subjected to orce and torque Contact State Modeling A considerable literature on deriving closed-or expressions to characterize contact states appears in the contexts o otion planning (e.g., Abler and Popplestone 1975, Farahat et al. 1995a, Hirai and Asada 1993, McCarragher and Asada 1993, Xiao and Zhang 1997), grasping (i.e., Cai and Roth 1987, Montana 1988), and odel-based copliant otion (i.e., Bruyninckx et al. 1995, De Schutter et al. 1999). This section reviews how contact states have been odeled in the literature. The irst part o this section addresses prior work on physics-based representations o contact states, whereas the second part reviews the literature on probabilistic-based representations o contact states. Physics-Based Modeling o Contact States Three types o odeling techniques have been coonly used in the literature to represent contact states: kineatic odels, quasi-static orce odels, and dynaic orce odels. 24

38 Kineatic Representation In ters o kineatic representation o the contact states, Popplestone et al. (1980) is an early reerence in which coniguration constraints between objects are extracted ro the spatial relationships aong the objects eatures (e.g., ace, edge). In Farahat et al. (1995a), the contacts between a polygonal workpiece anipulated by two or three active polygons are represented as a set o hoogeneous kineatic equations paraeterized by the conigurations o the contacting objects. Closed-or expressions or the possible conigurations o the grasped object are then derived by solving the set o kineatic equations. Xiao and Zhang (1997) extended the analysis o polygonal contact states by providing an exhaustive description o all the possible kineatic equality constraints, overlap constraints, and non-penetration constraints characterizing the contacts. The above kineatic representations only deal with objects deined using irst order geoetric properties (i.e., orientation o contact norals and location o contact points). The kineatic description o objects exhibiting second order geoetric properties (i.e., curvatures) can be ound in the grasp literature. For exaple, a kineatic description o the relative otion o two bodies undergoing point contact is studied in the work o Cai and Roth (1987) and Montana (1988). Quasi-Static Force Representation Force representation o contact states has been extensively studied in a quasi-static point o view. Whitney (1982) presents one o the earliest and ost coplete works on 25

39 the kineatic and orce characterization o the three dierent phases associated with a chaered peg-in-hole assebly task (i.e., chaer crossing, one-point contact, two-point contact). Desai and Volz (1989) use static equilibriu conditions to describe the contact orces associates with planar contact states. The resulting equality equations are then transored into inequalities to odel the eects o sensing uncertainties. Hirai and Asada (1993) utilize the concept o polyhedral convex cones to characterize the range o orces that satisies the unidirectional constraints due to contacts between polyhedral objects. The sae reasoning is also applied to describe the range o adissible displaceent sets. Moreover, Hirai (1994) showed that both cones are dual to each other when the constraints are unidirectional. The eects o riction have been extensively investigated in the task assebly literature (e.g., peg-in-hole insertion). Two early and uch-cited reerences on the eect o riction orces during a peg-in-hole insertion are Siunovic (1979) and Whitney (1982). In the context o contact state estiation, however, riction is oten neglected. This sipliication is due to the estiation coplexity added by the nonlinear eect o riction. An exception is the work o Farahat et al. (1995b) in which riction orces obeying Coulobs law are linearized and represented as an extra constraint on the quasistatic equilibriu conditions that characterize the contact orations. One way to avoid the odeling coplexity o riction is to encode it as a probabilistic disturbance (e.g., Eberan 1995, De Schutter et al. 1999). Bruyninckx and co-workers present a systeatic and uniying approach to contact state odeling in a series o papers on odel-based copliant otion (Bruyninckx 1995, 26

40 Bruyninckx et al. 1995, De Schutter et al. 1999, Leebvre et al. 2003). In this approach, contacts between a anipulated object and the environent are odeled as virtual echaniss paraeterized by the irst and second order geoetric properties o the contact suraces. Motion reedos and their dual sets o possible reaction orces are odeled up to second order using twist and wrench bases. Finally, reciprocity and consistency equations or the easured twist and wrench are derived using kineatic chains and the properties o the objects in contact. In a practical point o view, this approach is copelling since it eliinates the need or contact point sensors (i.e., no tactile sensing). Dynaic Force Representation The dynaic odeling o contact states has largely been overlooked in the contact state literature due to the assued (and prograed) quasi-static nature o assebly tasks. An exception is the work o McCarragher and Asada (1993, 1995a, 1995b) in which the dynaics o contact states are represented by linearized equations o otion constrained by Lagrange ultipliers. The ultipliers are added to enorce the geoetric constraints corresponding to the contact states. Eberan (1995) proposed a slightly dierent approach in which the Hailtonian is used to derive the dynaics equations. This approach resulted in two sets o irst order dierential equations as opposed to one in McCarragher and Asada. In both works, the dynaics eect due to the ipact between rigid bodies is considered by looking at the change o oentu created by an ipulse representing the collision. 27

41 Probabilistic Modeling As an alternative to physic-based odels, probabilistic odels have also been used to describe contact states. These techniques, although lacking the physical insights o odel-based approaches, have proven to be very eicient in practice since the odels are ade ro experiental training data. For exaple, Hannaord and Lee (1991) odel the ap between orce signals and the our phases o a peg-in-hole insertion task (i.e., ove, tap, insert, and extract) using Gaussian probability density unctions. These odels are then used in an HMM to onitor task progression in a teleoperated assebly. In the sae context, Pook and Ballard (1993) use Learning Vector Quantization on inger tension signals to create our contact priitives or a tele-anipulation task (i.e., grasp, carry, press, slide). In Eberan and Salisbury (1994), our contact signals (i.e., ipacts, slip, no contact, and grasping contacts) are odeled using KL-expansion, a noral white noise process, and an auto-regressive odel Task Encoding In the contact state graph utilized to decopose a task, actions are not encoded, and as a result the graph is undirected. An early approach to build directed graphs was proposed by Lozano-Perez et al. (1984). In this approach, a directed graph, deined as a reachability graph, encodes the otions between the states using a coputed pre-iage o the desired goal. Adding the a priori knowledge o the otion (e.g., velocity cone) augents the graph by partitioning the ree space. This technique is used in Eberan (1997) to represent the possible otions inside a 2D aze. 28

42 In Donald and Jennings (1991) the concept o R-R graph (reachability graph o the recognizable sets) is proposed to encode the reachability relationship between the recognizability sets that partition the sensor space. This type o graph can then be included inside a lattice structure to represent the variability o the task knowledge. The botto o the lattice is associated with sall R-R graphs that represent the current liited knowledge o the task whereas the top o the lattice is associated with large R-R graphs that represent the expected ull knowledge o the task. Probabilistic approaches can also be used to encode the likelihood o transitions aong the nodes o a contact state graph. The ost coon representations are Markov chains (e.g., Hannaord and Lee 1991) and Petrie nets (e.g., McCarragher and Asada 1995). In a Markov chain, or exaple, the task is encoded inside a probability transition atrix in which each eleent represents the likelihood o transitioning ro one state to another (including sel transition). The atrix can be obtained ro training data (Rabiner 1989) Contact State Identiiability and Distinguishability Identiiability and distinguishability have been investigated in several ields under dierent naes. In this docuent, the notion o contact distinguishability is equivalent to the notions o contact recognizability (Erdann 1986, Xiao and Liu 1998) or contact identiiability (Rosell et al. 2001) presented in the otion planning literature. It is iportant to note that the concept o odel identiiability presented in this thesis reers to the identiiability o the paraeters used to describe the odel structure and not to the 29

43 identiication o the contact odel. These choices o notations are inspired by the well- established noenclature used in the state-space odel literature and presented at the end o this section. Distinguishability The concept o distinguishability is equivalent to the early notion o recognizability introduced by Erdann (1986) in which a state is said to be recognizable i it can be distinguished ro the other states using the robot s sensors. Donald and Jennings (1991) introduce the idea o perceptual equivalence classes as distinguishable sets that partition the world based on sensor easureents. They propose an algorith to copute the perceptual equivalence classes in the coniguration space and show that the coputational coplexity is exponential in the diensions o the space and polynoial in the nuber o points contained in the space. As discussed in Erdann (1986), control history and sensor history can iprove distinguishability by reining the partition o the sensor space. Sensor usion can also iprove distinguishability (Donald and Jennings 1991). In this context, Xiao and Liu (1998) investigate contact recognition (i.e., distinguishability) or position/orientation and orce/oent sensing in the presence o uncertainties (i.e., orce can disabiguate contact states when position is not able to and vice-versa). They propose a uzzy representation o contact states to take into account the indistinguishability nature o contact states. A recognition algorith is discussed, but the authors do not report any ipleentation. 30

44 A ew analytical approaches based on geoetry and/or orce have been ipleented to test distinguishability (Xiao and Zhang 1997, Rosell et al. 2001). The concept o contact equivalence presented by Xiao and Zhang (1997) is based on analytical derivations o the equations describing the relative otions between contacting polygonal objects. This concept is used to characterize contact states that have equivalent structural equations (i.e., they are indistinguishable). Another exaple is presented in Rosell et al. (2001) in which tools investigating the robustness o paths generated by gross-otion planners to odel uncertainty are discussed. In particular, the distinguishability o potential contact situations due to uncertainty is analyzed by coputing the intersection o the generalized orce doains corresponding to the possible contact states. I the doains intersect, then orce easureents cannot be used to disabiguate the two contacts. Identiiability While distinguishability is concerned with discriinating one contact state ro another based on sensor data, identiiability addresses the questions o what paraeters in a particular contact state odel can be estiated and, i so, how any solutions are possible. In the robot calibration literature, an identiiable odel corresponds to a inial paraeterization (Gautier and Khalil 1990). In this literature, identiiability has been ostly addressed with nuerical techniques (e.g., Atkeson et al. 1986, Sheu and Walker 1989). For exaple, analysis o the Jacobian atrix singular values (Sheu and Walker 1989) can be used tests or identiiability. Paraeters are said to be unidentiiable i they 31

45 are associated with sall singular values. An exception to nuerical techniques is the work o Gautier and Khalil (1990) in which sybolic coputations are used to extract the iniu set o inertial paraeters used to represent the dynaic odel o serial robots. In the context o contact states, the notion o C-space equivalence deined by Eberan (1995) can be regarded as an identiiability test. This technique is applicable only i the contact odel can be written as a linear unction o the sensor variables. To prove identiiability, Eberan s approach was to deonstrate the uniqueness o the apping between the coeicients o the sensor variables (typically nonlinear unctions o the paraeters) and the paraeter values. In Bruyninckx et al. (1995), the identiiability o the geoetric uncertainties paraeterizing the wrench and twist Jacobians o the contact states is discussed qualitatively. Distinguishability and Identiiability in the State-Space Model Literature The concept o a priori testing o odel distinguishability and identiiability is well established in the state-space odel literature with applications related to control (Ljung and Glad 1994), biology (Chapell et al. 1990), and cheistry (Walter et al. 1985). Distinguishability has been treated in Raksanyi and Walter (1985) and Walter and Pronzato (1996) while identiiability has been exained in Chapell et al. (1990), Lecourtier et al. (1982), and Ljung and Glad (1994). In a series o papers published in the id-eighties (i.e., Lecourtier et al. 1982, Walter et al. 1984, Walter et al. 1985) Walter and Lecourtier provided a unior approach to testing the distinguishability and identii- 32

46 ability o state-space odels. Using odels M ( p ) as given in (2.1) in which X is the state vector, p is a set o unknown tie-independent paraeters, U is the input vector, and Y is the output vector, Walter and Lecourtier deined distinguishability and identiiability as ollows: i () ( (), (),, ) (, ) = ( ( ),, ) Xt = Xt Ut pt M i ( p) =, X(0) = X, U(0) = U Yi t p g X t p t 0 0 (2.1) Two state space odels M ( p ) and M ( ) 1 2 p are distinguishable i (i) or alost any q there is no p such that Y ( t p) ( ) , = Y ( t, q) and (ii) or alost any p there is no q such that Y t, q = Y( t, p), or any input and tie in (2.1). Siilarly, a state-space odel M ( p ) is globally (locally) identiiable i or alost any q there is only one (a inite nuber o) p such that Y( t p), = Y( t, q) or any input and tie in (2.1). Given these deinitions, there exist a variety o techniques to solve or state-space odel distinguishability and identiiability. For linear odels, these ethods include equating transer unction coeicients (Walter et al. 1984) and siilarity transorations (Vajda et al. 1989). For nonlinear odels, techniques include linearization (Grewal and Glover 1976), Taylor series expansions (Pohjanpalo 1978) and generating series (Walter et al. 1984). 33

47 2.1.5 Data Excitation When addressing the proble o paraeter estiation, the question o data excitation needs to be addressed. Is the input path suiciently exciting to estiate all the paraeters associated with the odel? It is iportant to realize that this notion o suicient excitation is dierent ro the notion o identiiability presented in section Identiiability has to do with the uniqueness o a solution while excitation has to do with the continuity o the solution on the data. This distinction is iportant since both probles can result in a poor estiation. In the case o unidentiiability the poor estiation is a result o a structural proble that requires an appropriate re-paraeterization o the odel, whereas in bad excitation the poor estiation is a result o a nuerical proble that requires an appropriate re-selection o the sensor path. The proble o data excitation has been overlooked in the contact state estiation literature where it is traditionally assued (epirically) that the data are suiciently exciting (e.g., De Schutter et al. 1999, Leebvre et al. 2003, 2005). The proble, however, has been extensively investigated in the calibration literature. In this context, the excitation proble is reduced to inding easureents that result in a low condition nuber o the paraeter Jacobian (i.e., a condition nuber less than 100 (Schröer et al. 1992)). Hollerbach and Wapler (1996) show how regularization techniques, such as colun and row scaling o the paraeter Jacobian, can be applied to reduce the condition nuber. Miniization o the condition nuber has been studied or kineatic as well as dynaic calibration. For exaple, Khalil et al. (1991) select the easureent conigura- 34

48 tions or a siulated calibration o a TH8 robot by iniizing the condition nuber associated with the paraeter Jacobian atrix. In contrast to kineatic calibration where the easureents can correspond to discontinuous snapshots o the syste s conigurations, dynaic calibration requires the continuity and soothness o the input trajectories. This proble was irst studied by Arstrong (1989) who developed a constrained nonlinear optiization technique to generate optial robot excitation trajectories. In this approach, the condition nuber o an observation atrix derived ro the dynaic odel o the syste is iniized using a Lagrangian optiization technique. The outputs o the optiization are the points o a sequence o joint accelerations. Joint velocities and joint positions are then obtained using nuerical integration. In Gautier and Khalil (1992), the iniization o a cost unction based on the condition nuber o an observation atrix derived ro the energy odel o the syste is perored using a gradient conjugate optiization ethod. A discrete set o optiu position and velocity points is obtained and interpolated using a ith-order polynoial. Swevers et al. (1997) present a statistical approach in which the deterinant o the covariance atrix derived ro the dynaic odel o the syste is iniized using a sequential quadratic prograing ethod. The originality o the ethod coes ro the use o a inite Fourier series to paraeterize the robot trajectories. An interesting ethod presented by Ljung and Glad (1990, 1994) utilizes tools ro dierential algebra to extract a necessary excitability condition or arbitrary nonlinear state-space odels. This ethod, based on ultiple dierentiations, leads to the explicit 35

49 estiation o the tie-invariant paraeters o the odel. This closed-or solution allows or easy extraction o a necessary condition or data excitation Contact State Observability Observability as deined in this thesis can be directly related to the concepts o reachability and recognizability deined by Lozano-Perez et al. (1984) and Erdann (1986). A state is observable inside a task s graph i it can be reached and recognized. This is an iportant concept in task planning since reaching a goal without recognizing it does not result in task copletion. In Lozano-Perez et al. (1984), the notion o pre-iage o the goal is introduced as a solution to this proble. The central idea is that given the known geoetry o the task and sensing uncertainty bounds, pre-iages o local goals that can be reached and recognized using a single copliant otion can be coputed in the coniguration space. Backward chaining o the local goals starting ro the desired goal can then be used to develop a ulti-ove strategy that guarantees observability or otion planning with uncertainty. The sae proble is solved by Erdann (1986) using backward and orward projections. The backward projection solves the reachability proble while the orward projection ensures that the reachable states are also recognizable using the local history o the task. The history o the task helps to disabiguate states that are otherwise indistinguishable using only the current sensor inoration. As an exaple o orward projection, consider the our states described in Fig The states B and C are assued to be distinguishable based on the current sensing, whereas the states A and D are assued to be indistinguishable. Storing the state sequence can 36

50 help distinguished A and D. For exaple, i the current sensor easureent cannot disabiguate A ro D, but i it is known that B was the previous reached state, then it can be concluded that A and not D was attained. A B D C Figure 2.1: Exaple o orward projection associated with a our-state graph. In the raework o contact state graph, reachability is siply reduced to path connectivity while recognizability is equivalent to coplete distinguishability (i.e., all the contact states are pairwise distinguishable). In that regard, the reachable and recognizable graphs deined by Donald and Jennings (1991) ensure that the contact states are observable Paraeter Estiation An extensive body o research on paraeter estiation exists in the literature. Ljung (1987), a classic reerence on the subject, reviews the theory as well as any o the techniques used in paraeter estiation. Paraeter estiation has been extensively studied in the contexts o robot calibration (e.g., Hollerbach and Wapler 1996 or a good overview) and ine otion planning (e.g., 37

51 De Schutter et al. 1999, Leebvre et al. 2003, 2005 or a recent overview). Three popular estiation techniques are the Kalan ilter and its variations (e.g., extended Kalan Filter), least-squares estiation and its variations (e.g., nonlinear least squares, total least squares, weighted least squares), and axiu-likelihood estiation. Paraeter estiation has been largely utilized in ine otion planning to correct or sall uncertainty errors in task assebly. Siunovic (1979) is an early reerence in which a position-based Kalan ilter is utilized to estiate the relative position between two ating parts given a known contact state. In the context o copliant robot otion, De Schutter and Van Brussel (1988) derive a closed-or estiate o the orientation errors between the physical and coputational task raes using velocity and orce easureents. In De Schutter et al. (1999), the odeling approach introduced by Bruyninckx (1995) is used to estiate the irst order geoetric paraeters (i.e., location uncertainties on the ixed environent object and the anipulated object) associated with a cylinder-on-plane experient. To this end, the closure-update equation as well as the wrench and twist reciprocity equations are used as the easureent equations o an extended Kalan ilter. Leebvre et al. (2003) generalize this ethod to the estiation o geoetrical paraeters o rigid polyhedra objects. In this approach, the geoetry o the task allows or an autoatic generation o the ilter s easureent equations. This technique is then used to estiate all the grasping and environent uncertainties associated with the six contact states o a cube-in-corner placing task. Eberan (1997) provides a statistical odel-based approach to building a contact state estiator. Contacts between the end-eector o a PHANToM robot and a aze are odeled as linear 38

52 stiness relationships between the robot conigurations and the contact reaction orces. A axiu likelihood estiation technique is then used to estiate the stiness projection atrix characterizing the contact states. Most o the literature on contact state estiation assues good noinal paraeter values and ocuses only on the estiation o sall errors. This assuption allows or linearization schees and reduces the coputational diiculty o the estiation process. Solving or large initial paraeter uncertainties in real tie reains a diicult proble. Recent progress has been ade by Leebvre et al. (2005) in which a inite-diensional Bayesian ilter is proposed to estiate unknown paraeters. The ilter is based on inding a non-inial state vector in which the easureent odel is linear. A Bayesian ilter is ipleented to estiate the statistics o this higher diensional state space, and then an iterative extended Kalan ilter is utilized to retrieve the states associated with the original odel. This technique is applied with good result to the cube-in-corner task presented in their earlier paper Contact State Estiation Detection tests are oten based on inding a threshold that can decide whether a contact state is active based on soe raw or processed sensor easureents. Two ailies o tests are oten used in the literature: deterinistic and probabilistic decision tests. Prior work on both ethods is presented in this section. 39

53 Deterinistic Decision Test In Hirai and Asada (1993) the geoetry o the contact state odels is utilized to generate a inial set o orce-based discriinant unctions that are then used as boolean rules to segent orce easureents into contact states. In the context o dynaic robotic assebly, McCarragher and Asada (1993) utilize quantitative reasoning to detect contact state transitions. In this raework, the raw orce signals are transored into qualitative signals that are then used to recognize the contact states and their transitions. The approach is tested successully or a dynaic dual peg-in-hole insertion. Dupont et al. (1999) apply a siilar technique by using a Boolean cobination o thresholds orce and otion signals to segent a pick place task. One o the irst paraeter-based approaches to contact detection was proposed by Farahat et al. (1995b). In this research, a linear prograing technique is used to recognize contact states. A contact state is detected as active when it satisies the linear progra associated with its paraeterized odel and the collected orce easureents. In the context o robot learning, Skubic and Volz (2000) use a uzzy classiier to detect contact state patterns built ro orce and oent signals. The sae experient was also conducted successully using a neural network. Statistical Decision Test In Eberan and Salisbury (1994), sequential hypothesis testing is used to detect and label contact events. Six statistical hypotheses are derived to describe ive contact 40

54 events. The segentation and identiication o the events are perored by coputing a generalized likelihood ratio test over a oving data window. In the context o copliant otion, De Schutter et al. (1999) use the Gaussian innovations o a Kalan ilter to onitor contact transitions during the placing o a cube inside a corner. The su over the noralized innovation squares (SNIS) is coputed inside a window o ixed length 2 and a threshold based on 95% conidence boundary χ -test is used to decide whether the innovations are consistent with the odel. I the SNIS exceeds the threshold, then a transition is detected. In the above exaples, contact detection is solely based on the relationship (i.e., physics-based or probabilistic relationship) between the sensing and the contact states; however, task inoration can also be incorporated to iprove the likelihood o contact state detection. Task inoration is conveniently odeled using Petri nets, neural nets, and HMM. These three techniques oer the double advantages o incorporating task knowledge and contact state knowledge in a sae raework. For exaple, Petri nets have been used by McCarragher and Asada (1995a, 1995b, 1996) to onitor and control robotic assebly tasks using polygonal and polyhedral parts. Neural nets have been used in the context o robot learning by Asada (1990) and Skubic and Volz (2000). In Asada, or exaple, neural nets are used to represent and learn copliances ro orce and velocity teaching data. HMM is a very popular technique in data segentation. This stochastic technique, originally developed or speech recognition proble (Rabiner 1989), has been successully adapted to onitor contact states in assebly tasks. In this raework, the relation- 41

55 ship between the observations and the states is usually represented using ixture o Gaussian signals, and the task inoration is encoded inside a state network deined by the likelihood o transitions aong states. One o the advantages o this technique is that both the Gaussian probability density unction and the probabilities o transitions can be obtained ro unsupervised training, e.g., Bau-Welch algorith (Bau and Petrie 1966). One early exaple is ound in Hannaord and Lee (1991), in which an HMM is used to extract our contact events ro orce/torque easureents. In Hovland and McCarragher (1998), a ultiple HMM approach is used to onitor the 11 contact events describing the robotic insertion o a 2D L-shape part. The task is odeled as a discrete event dynaic syste in which each event is represented by a HMM. A recognition rate o 97% is achieved in s. Eberan (1997) presents an alternate approach to HMM by building a contact state observer using stochastic paraeter estiation and change detection theory. Each contact state is represented by a linear paraeterized odel. A prior distribution or the paraeters is obtained ro training data. A axiu likelihood estiator is then used to estiate the paraeters given orce and velocity easureents. The estiator outputs the log-likelihood between the odel and the easureents and also the residuals o the process. The residuals are ed to a change detector that coputed the log-likelihood o transitions aong states. These statistics are then sent to a single detection procedure that uses a bea search strategy over a given task network to deterine the ost likely path corresponding to the easureents. 42

56 2.2 Thesis Objectives The prior work presented in this chapter can be used to suarize the needs or contact state estiation in poorly known environents: There is no coplete raework in the literature that discusses the proble o contact state estiation in poorly known environents. The estiation o the contact paraeters is usually liited to sall uncertainties. Contact state estiation has been ainly addressed as an ipleentation proble in the literature. As a result, very ew tools have been developed to investigate the a priori easibility o contact estiation. In particular, no testing strategy has been developed to test systeatically the distinguishability, identiiability, observability, and excitability o contact states. Based on these liitations, the objective o this thesis is deined as ollows: given a geoetrical description o a task, a contact topology, and a set o sensors, the goal o this research is to provide design tools that can help generate contact state odels and contact state networks that will perit the easibility o contact state estiation in poorly known environents. Given this objective, a design raework is provided or the ipleentation o contact state estiators in poorly known environents. Its ain contributions are: The identiiability and distinguishability o paraeterized contact odels are analyzed using a uniying algebraic testing ethod based on Taylor series expansion. 43

57 The testing ethod is applicable to any contact state odel regardless o the chosen sensing odality. Task easibility in poorly known environents is reduced to a contact state observability proble. The concept o orward projection on the paraeter history is introduced as a solution to the observability proble. The eect o the sensor signals on the paraeters is analyzed analytically by deriving closed-or expressions or the contact paraeters. Task knowledge, encoded as a probability transition atrix, is derived ro a direct apping between the contact state graph representing the task in a structured environent and a distinguishable state graph representing the task in a poorly known environent. In addition to the progress ade in the design phase o the estiator, the ollowing contributions to the ipleentation phase o the estiator are presented: Large uncertainties (i.e., unknown paraeters) are considered in the ipleentation phase o the estiator by using a nonlinear least squares algorith. An HMM is ipleented as a contact state detector. This approach provides a uniying probabilistic raework to cobine the results ro the estiation step with the a priori knowledge o the task. 44

58 Chapter 3 Topology and Modeling o Contact States Manipulation tasks are readily described as a succession o contact states that describe how the anipulated object is in contact with objects in the environent. The nature o a contact state is characterized by the geoetric eatures that are used to odel the contacting objects (e.g., vertex, edge, ace). As an exaple, Fig. 3.1 shows a planar anipulating robot placing a block in a corner. A sequence o three contact states is used to coplete the task: a vertex-edge contact, a double vertex-edge contact, and a double edge-edge contact. Figure 3.1: Sequence o three contact states used to place a block in a corner. 45

59 Contact states are naturally described geoetrically; nevertheless, in order to be detected, they also need to be described with respect to the available sensing inoration (i.e., robot sensing). The objective o this chapter is to provide the background on contact topology and contact state odeling necessary to understand the next chapters o this thesis. The irst part o this chapter reviews the contact topology associated with polygonal and polyhedral objects. Additional aterial on this topic can be ound in a list o papers published by Xiao and her colleagues (Xiao 1993, Xiao and Zhang 1997, Xiao and Ji 2001). The Second part o the chapter discusses the odeling o contact states using pose, velocity and orce sensing. As a result o this odeling, contact states are represented as hoogeneous nonlinear algebraic equations paraeterized by tiedependent sensor data and tie-independent paraeters representing the unknown locations and positions o the objects in contact. The velocity and orce representations o contact states are discussed using wrench bases, twist bases, and screw transorations. Additional background on this aterial can be ound in Bruyninckx (1995) and Murray et al. (1994). 3.1 Contact State Topology As shown in Fig. 3.1, each contact state is characterized by a geoetrical description o the two objects in contact in ters o a inite nuber o contacting eleents. For polygons and polyhedra these eleents are vertex and edge and vertex, edge and ace, respectively. 46

60 3.1.1 Single Contact State Contact states describe the contact between a anipulated object and ixed objects in the environent. As a result, the nuber o single contacts between the two objects is 2 n e where n e represents the nuber o geoetric eleents (i.e., n = 2 in 3D), as suarized in Table 3.1 and 3.2. in 2D, and n = 3 Notations or Table 3.1 and Table 3.2 a b : Contact between the eleent a belonging to the anipulated object ( a ) and the eleent b belonging to the shaded ixed object ( b ). ( ab, ) { V, E, F } V: Vertex E: Edge F: Face Table 3.1: Planar single contacts between a anipulated object and a ixed object. V V V E E V E E 47

61 Table 3.2: Spatial single contacts between a anipulated objects and a ixed object. V V V E V F E V E E E F F V F E F F As discussed in Xiao (1993), several o these single contacts are liiting cases o other ones, and as such should be ignored. In 2D or exaple, { } V V can be consid- ered as a part o { V E }, { E V }, or { E E }, and as such should be discarded to avoid any abiguity. Table 3.3 suarizes all the possible degenerate contact states. 48

62 Table 3.3: Degenerate contact states. 2D 3D V V V V V E E V E E Note that these degenerate cases are also o liited interest due to ipleentation diiculty (e.g., a vertex-on-vertex contact is diicult to realize in practice). One exception to these degenerate cases is the{ E E } contact. This contact can be considered non-degenerate i the two edge eleents are crossing Multiple Contact States Situations involving ultiple contact states can occur when concave objects are in contact or when the anipulated object contacts ultiple ixed objects in the environent. The possible nuber o cobinations involving single contacts is given by the equation (3.1), where n represents the nuber o single contacts and k is the nuber o contact possibilities. n = C + n (3.1) k As an exaple, Table 3.4 enuerates all the possible pairs o contacts that constitute a double contact or 2D polygons ( n = 4, k = 2, = 10 ). Only the upper triangular part o the table is considered since the lower part describes the sae pairs o contacts (e.g., {, } E E V E is equivalent to{ V E, E E }). 49

63 Table 3.4: Double contacts or polygons. V V V E E V E E V V X X X X V E X X X E V X X E E X It can be shown (Xiao and Zhang 1997) that any o the cobinations given by (3.1) are in act topologically equivalent to lower cobinations o single contacts. As an exaple, Fig. 3.2 shows that the double contact { E E, E E } where the edges are parallel is equivalent to a single contact{ E E }. Moreover, Xiao and Zhang (1997) proved that ultiple contacts coposed o three or ore single contacts are always equivalent to either a single contact or a liited cobination o two or three single contacts. Figure 3.2: An edge-edge contact and its topologically equivalent single contact. 50

64 This notion o contact equivalence is essential when building a contact state estiator since it can directly reveal the distinguishability o two contact states (i.e., two topologically equivalent contact states are indistinguishable). This concept o distinguishability will be investigated in details in Chapter Contact State Modeling The ipleentation o a contact state estiator based on contact geoetry requires knowing the diensions and locations o the contact eleents. In the context o poorly known environents, this inoration needs to be estiated, and as a consequence, higher level representations o the contact states, based on available sensor easureents, are needed. Several sensing odalities are coonly available with anipulating robots; exaples include position, orientation, orce and torque sensing. Contact state odeling involves inding a ap between the geoetric constraints that characterize the contact states in the Cartesian space and the available sensors. As a irst step, coordinate raes and transoration atrices describing the unknown relative positions and orientations o the two objects in contact are developed. Then, contact states are odeled by algebraic constraint equations paraeterized by sensor data and tieindependent location uncertainties Contact State Paraeterization Contact states are characterized by the locations (i.e., positions and orientations) o the contacting eleents (e.g., vertex, edge, ace, etc). Thereore, local coordinate raes 51

65 ust be deined to represent the locations o each contact. Additionally, global raes ust be deined to represent the location o the anipulating robot and its gripper. These raes are independent o the contact geoetry. Four raes are deined as ollows: R w is the world coordinate rae associated with the anipulating robot. Fixed objects in the environent are located with respect to this rae. The position and orientation o the robot gripper are expressed with respect to this rae. R g is the coordinate rae associated with the robot gripper. The ixed location and orientation o the anipulated object is deined with respect to this rae. Force and torque are expressed with respect to this rae. R is the local coordinate rae associated with the contact eleent on the anipulated object. I the contact involves ore than one contact eleent, ultiple local raes ust be deined (i.e., one or each contact eleent). R is the local coordinate rae associated with the contact eleent on the ixed environent object. I the contact involves ore than one contact eleent, then ultiple local raes ust be deined. The positions and orientations o the local coordinate raes R and R depend on the nature o the contacting eleent. The ollowing rules are developed to assign the local raes uniorly: 52

66 Vertex: the coordinate rae associated with a vertex is positioned at the vertex origin and has the orientation o the global rae associated with the object to which the vertex belongs. Edge: the coordinate rae associated with an edge is positioned at one the two vertices bounding the edge such that the edge is oriented along the positive x -axis. The y -axis is oriented in the direction o the outward noral o the edge and it is perpendicular to the noral o the ace ro which the edge belongs to. The reaining z -axis ollows the right hand rule. Face: the coordinate rae associated with a ace is positioned at one o the vertices bounding the ace such that its z -axis points in the direction o the outward noral o the ace. The y -axis is oriented perpendicularly to the z -axis, toward the inside o the ace. The reaining x -axis ollows the right hand rule. Figure 3.3 illustrates how global and local raes are assigned or three dierent contact states. In each case, the global raes, w R and g R are assigned arbitrarily whereas the local raes are assigned ollowing the ethodology presented above. Note that in practice, the global raes depend on the geoetry o the anipulating robot and can be assigned using the Denavit-Hartenberg convention (Denavit and Hartenberg 1955). 53

67 x -axis y -axis z -axis R 1 R w R R g R R w R 1 R g R2 R2 a) { V E } R g b) { V 1 E 1, V 2 E2 } R w R R c) { V F } Figure 3.3: Exaple o raes assignents or our dierent types o contacts. Transoration atrices To express the location o a rae R a with respect to a rae R b, the concept o a hoogeneous transoration atrix is utilized. A change o coordinate raes is given 54

68 by the transoration atrix b T a in equation (3.2); where b R a k k represents a rotation atrix and b k Pa represents a position vector. T b b R P k = 2in2D =, 0 3in3D 1 1 k = k b a a a (3.2) The yaw-pitch-roll convention is used to express the orientation atrix b R a in equation (3.3). The yaw, pitch, and roll correspond to a θ y rotation around the xa -axis, a θ p rotation around the ya -axis, and a θ r rotation around the za -axis, respectively. cosθr sinθr 0 cosθp 0 sinθp b R a = sinθr cosθr cosθy sinθy (3.3) sinθ 0 cosθ 0 sinθ cosθ p p y y g Using this noenclature, three hoogeneous transorations, T () t, T () t, and g T () t are deined. T ( t) is a hoogeneous transor atrix which relates the gripper w w rae to the world rae based on the geoetry o the reote anipulator. Siilarly, T () t relates the anipulated object to the gripper rae, and T ( t ) relates the g environent object to the world rae. These three atrices have the generic or given by equation (3.4) in 2D and equation (3.5) in 3D, where { ab, } { wg,,, } w w g 55

69 a a cos θ () t sin θ () t x () b t b a a Ta () t = sin θ () t cos θ () t yb() t (3.4) a a a a cos θ () sin θ ()0 cos θ ()0sin θ () () r t r t p t p t xb t a a a a sin θ ( ) cos θ ( ) cos θ ( ) sin θ ( ) ( ) () r t r t = y t y t yb t b T a t a a sin θ ()0cos θ () a a 0sin θ () cos θ () () (3.5) p t p t y t y t zb t It is assued that the location o the anipulated object and the location o the environent object are unknown with respect to the gripper rae and world rae respectively. Moreover, it is also assued that the anipulated object does not slip in the gripper, and that the environent object is static. As a result, Tg and T w are unknown g tie-independent atrices. In contrast, the atrix T ( t ) is a known tie-dependent atrix since the location o the gripper with respect to the world rae is assued to be perectly known. w Pose Representation Contact states represent geoetric constraints between the anipulated object and the ixed objects in the environent. These constraints proscribe soe o the poses (i.e., position, orientation) between the anipulated object and the environent object. As a result, soe coponents o the hoogeneous transoration atrix between the rae 56

70 associated with the anipulated object and the rae associated with the ixed environent object ust be zero (i.e., soe coponents o exaple, Fig. 3.4 shows a planar { V E } T or T ust be zero). As an contact. Due to the contact geoetry, no translation noral to the environent object s edge is allowable, and thereore T (2,3) ust be zero in (3.4). g R w R R R Figure 3.4: Constraint coniguration o a robot due to a planar vertex-edge contact. The nuber o coponents o the atrix T and T that can be zero depends on the nature o the contact state. This nuber is liited to six in the 2D case, and twelve in the 3D case, as shown in (3.6) and (3.7). 2D case: No translation along x T (1, 3) = 0 No translation along y T (2,3) = 0 No rotation around z T (1, 2) = 0 No translation along x T (1, 3) = 0 No translation along y T (2,3) = 0 No rotation around z T (1, 2) = 0 (3.6) 57

71 No translation along x T (1, 4) = 0 No translation along y T (2,4) = 0 No translation along z T (3, 4) = 0 No rotation around x T (3, 2) = 0 No rotation around y T (3,1) = 0 No rotation around z T (2,1) = 0 3D case: No translation along x T (1, 4) = 0 No translation along y T (2,4) = 0 No translation along z T (3, 4) = 0 No rotation around x T (3, 2) = 0 No rotation around y T (3,1) = 0 No rotation around z T (2,1) = 0 (3.7) In both the 2D and 3D cases in (3.6) and (3.7), the constraints on the rotations can be ound by analyzing the or o the transoration atrices or exaple, the hoogeneous transoration atrix T or T. In the 3D case, T has the or given by (3.5). I the constraint prevents any rotation around the x -axis (i.e., the yaw angle is zero), then the rotation atrix o T can be written as in (3.8). As a result, the absence o rotation around the x -axis iplies that R (3, 2) is zero resulting in T (3, 2) being zero. R cosϕr sinϕr 0 cosϕp 0 sinϕp = sinϕ cosϕ sinϕp 0 cosϕ p r r cosϕr cosϕp sinϕr cosϕrsinϕ p = sinϕr cosϕp cosϕr sinϕrsinϕp sinϕp 0 cosϕ p (3.8) 58

72 Since the hoogeneous transoration between the anipulated object and the environent object is not directly known, T or T ust be coputed in an indirect way using the available inoration o the proble (i.e., sensor data, tie-dependent location unknowns). In that eect, the kineatic closure equation (3.9) can be used to close the loop between the anipulated object and the environent object, as illustrated in Fig As a result, the transoration atrices T and T can be expressed using the g kineatic easureents (i.e., T ( t ) ) and the paraeterized location unknowns associated w with the anipulated object and the environent object (i.e., equation (3.10). T g and w T ) as shown in T g () t T T T w = I (3.9) w g T = T T () t T T = T T () t T w g w g g w g w (3.10) g R g R w R T R w R T R R R Figure 3.5: Kineatic closure equation. 59

73 As a inal step, pose equations are created by expressing the constraints (3.6), (3.7), using the kineatic closure equations (3.10). To ake these equations algebraic, the unknown trigonoetric nonlinearities inside the transoration atrices T g and T w are eliinated using the change o variables = cosθ, 1 sinθ ( i = 1 in 2D, and i i = i+ i i = 1, 3, 5 in 3D). This change o variables reduces (3.4) to (3.11), and (3.5) to (3.12). Siilar equations are obtained or (3.11) and (3.12). T w by replacing the variable by the variable in Tg = where (3.11) = 1 T g = where = = = 1 (3.12) The inal expressions o T and T are obtained in 2D by substituting (3.11) and (3.4) in (3.10), and in 3D by substituting (3.12) and (3.5) in (3.10). The detailed expressions o the atrices used to copute (3.6) and (3.7) are presented in Appendix A. An exaple detailing the derivation o a pose equation or a vertex-to-edge contact is pre- 60

74 sented in section 3.3. The general or o the pose equation is given by the constraint equation (3.13) where p represents the unknown geoetric paraeters characterizing the contact (e.g.,, ) and st () represents the known kineatic conigurations o the anipulating robot. i i h( p, s( t )) = 0 (3.13) Wrench and Twist Representations Contact states can also be described using orce and oent constraints resulting ro the physics o the contact. For exaple, in order to aintain a { V E } contact, the vertex o the anipulated object ust exert a orce noral to the edge o the ixed object at the contact point (assuing a rictionless contact). Siilarly, the sae orce and a oent preventing any rotation o the anipulated object ust be exerted when aintaining a { E E } contact. Thereore, enuerating all the possible orces and oents necessary to aintain a contact is another way o characterizing contact states. Alternately, linear and angular velocities can also be used to represent contact states. In this context, contacts can be characterized by enuerating all the linear and angular velocities allowable by the contact. For exaple, in order to aintain a 2D { V E } contact, the vertex o the anipulated object is allowed to slide along the ixed edge and rotate around the contact point. Contrasting to the orce representation and the kineatic representation o section , velocity representations use otion reedos as opposed to otion constraints to describe a contact. 61

75 A large body o literature has been devoted to orce and velocity representations o contacts as described in section As a general approach, wrench bases, twist bases, and screw transoration atrices are coonly used to express the eect o orces and velocities in a given coordinate rae (Bruyninckx 1995). The wrench and twist deinitions and properties presented below can be ound in several robotics textbooks, e.g., Murray et al. (1994). Wrench: A wrench i n i T F is a vector consisting o a orce/oent pair F = [, ] expressed in the rae i R where k, n k,{ n 3, k 2} = = in 2D, and { n 6, k 3} = = in 3D. i n i Twist: A twist V is a vector consisting o a linear/angular velocity pair V = [ v, ω] expressed in the rae i R where k v, ω n k. T Wrench basis: A wrench basis G i n p is a atrix whose coluns represent the independent directions o the orce/oent which produce no otion o the anipulated rigid body in the rae i R. For ultiple contacts, the total wrench basis is the union o the wrench basis o its individual contacts expressed in the sae coordinate rae. Twist basis: A twist basis J i n q is a atrix whose coluns represent the independent directions o the linear/angular velocity allowed by the contact in the rae i R. For ultiple contacts, the total twist basis is the intersection o the twist bases o its individual contacts expressed in the sae coordinate rae. 62

76 Wrenches and twists are oten reerred to as wrench o constraint and twist o reedo in the contact literature (Duy 1990). Wrench and twist bases can be obtained by inspection when expressed with respect to a coordinate rae c R located at the contact. This rae is positioned at the contact patch centroid and has the sae orientation as the highest order contact eleent involved in the contact (e.g., an edge or a vertex-edge contact, a ace or an edge-ace contact). For two contacting eleents o the sae type, the orientation o the ixed object. c R is chosen arbitrarily to be the sae as the eleent associated with Wrench consistency-based equation: F i i = Gφ (3.14) p where φ represents the intensity o the wrench. Twist consistency-based equation: V i i J λ = (3.15) q where λ represents the intensity o the twist. Wrench screw transoration atrix: A screw transoration atrix F S is used to transor a wrench or a wrench basis ro the rae b R to the rae a R. b a In 2D: F S b R 0 21 a = Pa (2) Pa (1) Ra 1 b a b b b In 3D: F S R = b 0 b a 33 a b b b Pa Ra Ra (3.16) (3.17) 63

77 where P is the skew-syetric atrix represented by b a b b 0 P (3) (2) a Pa b b b Pa = Pa (3) 0 Pa (1) b b P (2) (1) 0 a Pa The rotation atrix b Ra and the position vector b Pa were deined in Twist screw transoration atrix: A screw transoration atrix V S is used to transor a twist or a twist basis ro the rae b R to the rae a R. b a In 2D: S b b P (2) a Ra = Pa (1) V b b a (3.18) In 3D: V S b a b b b R a Pa R a = b 0 3*3 Ra (3.19) As entioned in Murray et al. (1994, p.63), wrench transoration atrices can be used to represent a wrench with respect to a coordinate rae which is not inside the rigid body. The obtained wrench represents the equivalent orce/oent pair applied as i a lever ar were physically present between the coordinate rae and the rigid body. Thereore, care ust be taken when applying a wrench transoration atrix to avoid creating artiicial orce/oent pairs. A siilar coent applies to twist coputation. Assuing a rictionless contact between rigid objects iplies that no instantaneous power is dissipated when oving a rigid body through an applied orce. In this situation, 64

78 the wrench is said to be reciprocal to the twist and the inner product between the wrench and the twist is zero (Ball 1900): i i F V = 0 (3.20) The reciprocity condition (3.20) can then be used with the wrench and twist consistency equations (3.14) and (3.15) to show reciprocity between the wrench and twist bases: i T i ( ) 0 G J = (3.21) In addition, equation (3.20) can be cobined with the consistency equations (3.14) and (3.15) to produce reciprocity equations or the wrench (3.22) and twist (3.23) respectively. i T i ( ) 0 J F = (3.22) i T i ( ) 0 G V = (3.23) It is assued that orces and oents are easured by a orce sensor located at the gripper coordinate rae, and that velocity is coputed with respect to the world rae. As a result, a wrench transoration atrix ust be used to transor a wrench basis ro the contact rae to the world rae and a twist transoration atrix ust be deined to transor a twist basis ro the contact rae to the gripper rae. The 65

79 reciprocity equations or the easured wrench and easured twist are expressed as ollows: ( V c c T ) g g 0 SJ F = (3.24) ( F c c T ) w w 0 SG V = (3.25) The screw transoration atrices used in (3.24) and (3.25) are not directly known; however, they can be coputed indirectly by applying the closure chain described in Fig. 3.4 to screw transorations, as shown in (3.26). S g () t S S S w = I (3.26) w g As illustrated in Fig. 3.5, there exist two possible paths to go ro one rae to another in a closed kineatic chain. As a result, there are two ways o expressing the reciprocity equation or the easured wrench and easured twist, as shown in (3.27) and (3.28), respectively. In practice, the path that ost sipliies V S c g or F S c w is chosen. V V c c T g ( Sg SJ ) F ( g w ) = 0 V w V V c c T g S () t S S J F = 0 F F c c T w ( Sw SG ) V ( w g ) = 0 F g F F c c T w S () t S S G V = 0 (3.27) (3.28) 66

80 The inal expressions o the reciprocity equations or the easured wrench and easured twist are obtained in 2D by substituting (3.18) in (3.27) and (3.16) in (3.28) respectively, and in 3D, by substituting (3.19) in (3.27) and (3.17) in (3.28). As or the pose equation, these equations are ade algebraic by replacing the trigonoetric nonlinearities by polynoial expressions. An exaple illustrating the concepts o the reciprocity equation or the easured wrench and easured twist is given in section 3.3. Equations (3.27) and (3.28) only hold when no power is dissipated during the otion. As a consequence, these equations generally do not apply during transitions between contact states since transitions alost always create ipact orces (Eberan 1995) Relationship Between the Pose and Twist Equations The pose equations derived in section represent constraints on the conigurations o the anipulator robot (e.g., { x(), t y(), t θ () t } in 2D) iposed by the geoetric nature o the contact states. Given a geoetric description o the anipulated object and the ixed object, the conigurations that satisy the pose equations associated with a contact state can be coputed using (3.6) or (3.7). The result is a set o coniguration points that describe a surace in the coniguration space (C-surace in Mason 1981). This concept is illustrated in Fig The pose equation derived in (3.33) is utilized to extract the { x(), t y(), t θ () t } conigurations associated with the given geoetry o a vertex-edge contact. Figure 3.6(a) shows one possible coniguration in the Cartesian space; Fig. 3.6(b) shows ultiple conigurations in the Cartesian space, and Fig. 3.6(c) shows the C- surace associated with the contact. 67

81 a) 0 coniguration point b) y () x () y () x () c) y () x θ (rad) () Figure 3.6: Representation o a vertex-edge contact. a) Representation o the contact in the Cartesian space or one coniguration, b) Representation o the contact in the Cartesian space or ultiple conigurations, and c) Representation o the contact in the coniguration space. The geoetric properties o the coniguration space have been extensively studied in the literature, e.g., Arnold (1989). The relationship between the kineatic equation and the twist reciprocity equation can be derived ro the cotangent space o the C-surace. The cotangent space N is the space o all the wrenches (Arnold 1989) as shown in (3.29). 68

82 Fro a geoetrical point o view, the cotangent vector at a coniguration s is given by the noral o the C-surace at that point, as expressed in (3.30). N G w = (3.29) h h h N = h( p, s( t) ) = (3.30) s1 s2 sn T Using the chain rule, the tie derivative o the pose equation is expressed in (3.31) as the product o the easured twist and the gradient o the pose equation. As a result, the reciprocity equation or the easured twist is equivalent to the tie derivative o the pose equation as shown in (3.32). ( ) n dh p, s( t) h T w = s i = [ h ] V (3.31) dt s i= 1 ( ) i dh p, s( t) w T w = ( G ) V (3.32) dt 3.3 A Contact State Modeling Exaple To illustrate the odeling techniques presented in this chapter, a vertex-to-edge contact between two polygons is considered in Fig The pose equation, the reciprocity equation or the twist and the reciprocity equation or the wrench corresponding to this contact are derived in this section. 69

83 g R w R R R c R Figure 3.7: Single vertex-edge contact between two polygons Pose Equation No translation noral to the environent object s edge is allowable. Thereore, T (2,3) ust be zero. Following Appendix A, the corresponding equation can be written as: ( ) h : x+ y+ + cos θ + ( + )sinθ = 0 (3.33) p Reciprocity Equation or the Measured Twist As shown in Fig. 3.7, the rae c R associated with the contact is located at the contacting vertex and has the orientation o the rae associated with the ixed object (i.e., R ). As a result, the transoration atrix between the contact rae and the anipulated object rae reduces to a rotation atrix. T c R 0 21 = (3.34) 70

84 In this rae, the basis or the wrench can be written as ollows: c G 0 = 1 0 (3.35) The wrench basis can then be expressed in the world rae using (3.28): w g F F c c G = R () t S S G (3.36) w g where R () t g w R () t 0 g w 21 = Note that or the sake o siplicity, the joints and links o the anipulated robot are not odeled in this exaple. As a consequence, there is no physical link between the gripper rae and the world rae. Thus, the last screw transoration atrix in (3.28) is replaced by a rotation atrix ollows: g R w. Using (3.16), the wrench basis can be expanded as G 2 w = 1 ( sinθ + cosθ ) + ( cosθ sinθ ) (3.37) 71

85 As a inal step, the reciprocity equation or the easured twist is expressed in (3.38) using (3.28). One can easily veriy that this equation corresponds to the derivative with respect to tie o the pose equation presented in (3.33). ( ( ) θ ( ) ) h : v + v + + cos + sinθ ω = 0 (3.38) t 2 x 1 y Reciprocity Equation or the Measured Wrench The basis or the twist can be written in the contact rae in (3.39). This basis corresponds to the otions allowable by the contact. c R by inspection, as shown c J 1 0 = (3.39) The twist basis can then be expressed in the gripper rae using (3.27): J = S S J g V V c c g (3.40) Using (3.18), the twist basis can be expanded as ollows: 1cosθ + 2sinθ 4 g J = 2cosθ 1sinθ (3.41) As a inal step, the reciprocity equation or the easured wrench is expressed in (3.42) using (3.27). 72

86 h w ( θ θ) ( θ θ) Fy 2cos 1sin + Fx 1cos + 2sin = 0 : τ F y 3 + F x 4 = 0 (3.42) 3.4 Suary This chapter discussed the representation o contact states. In the irst part o the chapter, the representation o contact states in Cartesian space was reviewed. It was shown that rigid polygonal and polyhedral objects could be described using a liited nuber o geoetric eleents (i.e., vertex, edge, and ace) which could then be cobined to or contact states. This approach was utilized to derive lists o possible single and double contact states or tasks involving polygonal and polyhedral objects. In the second part o the chapter, the representation o contact states in the sensor space was presented. Three sensor odalities were considered (i.e., pose, twist, and wrench) and techniques based on kineatic constraints and twist and wrench reciprocity were described to obtain the contact odels. As a result, contact states were odeled as hoogeneous nonlinear algebraic equations paraeterized by tie-dependent sensor data and tie-independent paraeters representing the unknown locations and positions o the objects in contact. As shown in Fig. 1.4, the two sub-probles o contact topology and contact state odeling treated in this chapter aect all o the reaining design probles associated with the construction o a contact state estiator. The next chapter discusses the distinguishability and identiiability o the contact odels developed in this chapter. These two properties are needed to ensure the easibility o contact state estiation as well as 73

87 the easibility o paraeter estiation. As discussed, the contact topology can be chosen to ensure contact state distinguishability in the Cartesian space (i.e., no degenerate contact states in Table 3.3). Nevertheless, tools need to be created to investigate the distinguishability o contact states in the sensor space. The next chapter presents a solution to this proble. In addition, the identiiability o the paraeters associated with each contact odel is also investigated. 74

88 Chapter 4 Identiiability and Distinguishability Testing This chapter addresses two undaental questions that ust be answered when orulating atheatical descriptions o contact states: are the contact states distinguishable ro each other? Can the unknown or iprecisely known paraeters in these descriptions be identiied? An analytical ethod is presented or evaluating the distinguishability and identiiability o a set o contact state odels described by nonlinear algebraic equations. In contrast to the analytical techniques proposed in the literature that ocus either on distinguishability or identiiability, the proposed approach, based on Taylor series expansion o the contact equations, presents a uniied technique or the testing o both properties. Moreover, it can be applied to any contact odel that 75

89 can be written as hoogeneous equations, regardless o the sensing odality (e.g., pose, wrench, twist) and diensionality (e.g., planar, spatial) chosen to represent the odel. This chapter starts by deining the concepts o identiiability and distinguishability in the context o contact state estiation. In particular, it is shown that identiiability reduces to a uniqueness proble whereas distinguishability reduces to an existence proble. Both probles require contact odels to be copared and analyzed. A ethod based on Taylor series expansion o contact odels is utilized to test the capability o candidate odels to estiate both the paraeters and the states. The technique is illustrated or several planar and spatial exaples. A coplexity analysis o the ethod applied to pose equations is discussed at the end o the chapter Contact State Distinguishability and Identiiability Deinitions Given nonlinear algebraic odels o contact states, paraeterized by sensor variables, st (), and tie-independent coniguration paraeters, p (e.g., any o the contact state odels derived in 3.3), distinguishability and identiiability can be deined in the anner o Walter and Pronzato (1996) as ollows: Deinition 4.1. Two contact state odels A and B, paraeterized by a sensor path s() t and by coniguration paraeters ( p or A and q or B) are distinguishable i, or alost any sensor path s() t o suicient length, there is no solution or the paraeter set { p, q } such that the contact odels are satisied siultaneously. 76

90 Deinition 4.2. A contact state odel A is globally identiiable i, given alost any sensor path s() t o suicient length, there exists a unique solution or p that satisies odel A. I there are a inite nuber o solutions then contact odel A is locally identiiable. In these deinitions, the sensor path ust be at least o the iniu length corresponding to the nuber o unknown paraeters associated with the odels. The phrase alost any sensor path is eant to rule out unexciting paths. In deinition 4.2, local identiiability is equivalent to the odel being inial (i.e., a odel in which unidentiiable paraeters are eliinated or grouped into ters which are identiiable, Gautier and Khalil 1990). Note that a odel being inial guarantees the existence o a solution but not its uniqueness. The ollowing section develops a systeatic ethod or evaluating these deinitions or contact states odeled as hoogeneous nonlinear algebraic equations. 4.2 Taylor Series Testing o Distinguishability and Identiiability Distinguishability and identiiability testing is based on inding practical ways to analyze and copare equations. In that regard, Taylor series expansion can be eective since it allows a nonlinear odel to be written as a unique set o algebraic equations in which each equation corresponds to a coeicient o the series. Thus, Taylor series reduce the distinguishability and identiiability testing to a coparison o the dierent coeicients o the series. While there are an ininite nuber o coeicients, a decision 77

91 on distinguishability and identiiability can oten be ade based on coparing a odest nuber o coeicients. As a result, no approxiation is being ade when using the series. This technique was successully applied to testing the identiiability and distinguishability o zero-input state-space odels (e.g., Pohjanpalo 1978, Walter and Pronzato 1996). In this approach, the Taylor series o the output vector is written as a + succession o tie derivatives evaluated at tie t = 0. These coeicients or a set o algebraic equations that ust be solved or all the possible sets o paraeters. Although the sets o equations can be diicult to solve by hand, tools ro coutative algebra can be used to sipliy the equations (Raksanyi et al. 1985). In developing a Taylor series approach or contact odels, the odel M is peritted to be nonlinear in the paraeters p as well as the sensor variables st ( ), ( ) F p, s( t) = 0 M : H ( p ) = 0 (4.1) F() includes all the sensor-dependent equality constraints while H ( ) odels any additional equality constraint on the paraeters. Assuing that the unction F ( ps, ) is analytic, a Taylor series expansion o order k with respect to the sensor variables can be written as: 78

92 k F( p, s) = ai ( p, s0 ) i= 0 i df( ps, ) ai ( p, s0 ) = i ds i ( s s ) s= s0 0 k! (4.2) Note that each coeicient o the Taylor series (i.e., ai ( p, s 0) ) is a unction o the unknown constant paraeters and the known sensor values. I ore than one sensor variable appears in the odel, then partial derivatives with respect to all the sensor variables can be coputed. Equation (4.3) shows a second order expansion o a unction o three variables {,, } s s s. This expansion could be applied, or exaple, to planar contact states described using pose equations by substituting { s, s, s } by {,, } x y θ. 3 2 F( p, s 3 3 ) F p, s si si0 sj sj0 ( i i0 ) ( ) ( )( ) F( p, { x, y, θ} ) = F( p, s) + s s + + s= s0 i= 1 si = 1 = 1 2! = i j si s s s j 0 s= s0 s= { s1, s2, s3} s0 = { s10, s20, s30} (4.3) Since the unction F ( ps, ) is assued to be ininitely dierentiable with respect to its sensor variables, its ixed derivatives are equal. With as the nuber o sensor variables and k as the order o the expansion, the nuber o coeicients n c o the series is given by: ( k + )! nc = (4.4) k!! 79

93 4.2.1 Distinguishability Testing Based on the uniqueness o the Taylor series expansion, two contact state odels are equivalent i and only i all the coeicients o their expansions are equal, as given below: a0( p, s0) = b0( q, s0) a1( p, s0) = b1( q, s0) = (4.5) an( p, s0) = bn( q, s0) H1( p) = H2( q) (, ) B( q, s) A p s This equality leads to the ollowing test or distinguishability. Deinition 4.3. Two contact state odels A and B in the or o (4.1) are distinguishable i and only i or any s 0, (i) given any choice o p, there is no solution to (4.5) or q, and (ii) given any choice o q, there is no solution to (4.5) or p. To deonstrate that two odels A and B are distinguishable, their Taylor coeicients (i.e., a( p, s 0) and bqs (, 0) ) ust dier in at least one equation o (4.5) or all choices o paraeters Identiiability Testing The identiiability o a contact odel can be tested by considering how any sets o paraeters yield the sae Taylor series coeicients in (4.2). This test can be perored by counting the nuber o solutions or p, given ˆp, in the equation below: 80

94 a0( p, s0) = a ˆ 0( p, s0) ˆ a1( p, s0) = a1( p, s0) = (4.6) a (, ˆ n p s0) = an( p, s0) H ˆ 1( p) = H1( p) (, ) A( pˆ, s) A p s This test can be stated orally as ollows. Deinition 4.4. A contact state ode A is identiiable i and only i, or all s 0, given any ˆp, there is a unique solution to (4.6), which is p = pˆ. I a inite nuber o solutions or p exist then A is locally identiiable. A is unidentiiable i an ininite nuber o solutions exist. Since the Taylor series is developed around noinal sensor values, the approach appears to be local in the space o sensor variables. It is iportant to note, however, that the solutions or the paraeters are obtained without substituting nuerical values or the sensor values s 0 and so the results are truly global in the sensor space. Testing identiiability involves solving or the nuber o solutions or p in (4.6). Note that the solution p = pˆ always exists. 81

95 4.3 Exaples Four exaples are presented to illustrate the Taylor series technique o testing the distinguishability and identiiability o contact odels or polygonal and polyhedral objects. The irst two test the distinguishability and identiiability o a pair o polygonal contact odels while the last two ocus on the identiiability and distinguishability testing o a pair o polyhedral contact odels Exaple 1 Distinguishability o Polygonal Vertex Edge Contacts As a irst exaple, the distinguishability o the odels developed or the contact states o Fig. 4.1 is tested. The pose equations or the contacts { V E } { E V } and (i.e., contact A and contact B respectively) are obtained using the technique described in section The corresponding equations are shown in (4.7) and (4.8). g R g R w R R R R R Figure 4.1: Contact between two polygons. a) Contact between a vertex o the anipulated object and an edge o the ixed object, and b) Contact between a vertex o the ixed object and an edge o the anipulated object. 82

96 a F ( p, s) p5 ( p2p3 p1p4) cos ( t) ( p1p3 p2p4) sin ( t) p2x( t) p1y( t) 0 A: = + θ + θ + = H a 2 2 ( p ) = p 1 + p 2 1= 0 (4.7) F ( q, s) = q + qq q q + q x() t q y()cos t () t qq + q q q x() t q y()sin t () t = 0 ( ) ( ) b B : θ θ H b 2 2 ( q ) = q 1 + q 2 1= 0 (4.8) Series coeicients are given in (4.9) or contact A and in (4.10) or contact B. Due to the cyclic nature o the derivatives o sine and cosine, derivative ters beyond second order do not generate independent equations. a ( ) ( ) θ ( ) = a θ (, ) = ( + ) cosθ ( ) sin = a x (, ) = = a y (, ) = = a θθ (, ) = ( ) cosθ + ( + ) sin = a θ x (, ) = 0 = a θ y (, ) = 0 = a xx (, 0 ) 0 a xy (, ) 0 a (, ) 0 a0 = F p, s0 = p5 + p2p3 p1p4 cos 0 p1p3 + p2p4 sinθ0 + p2x0 p1y0 = 0 a1 F ps0 pp 1 3 pp pp 2 3 pp 1 4 θ0 a2 F p s0 p2 a3 F p s0 p1 a4 F ps0 pp 2 3 pp pp 1 3 pp 2 4 θ0 a5 F p s0 a6 F p s0 a7 F p s = a8 = F p s0 = a9 = Fyy p s0 = (4.9) 83

97 ( ) θ ( ) ( ) θ ( ) b b0 = F ( q, s0) = q5 + qq 1 4 q2q3 + q2x0 q1y0 cos 0 qq 1 3+ q2q4 q1x0 q2y0 sinθ0 = 0 b b1 = Fθ ( q, s0) = qq qq 2 4 qx 1 0 q2y0 cos 0 qq 1 4 qq q2x0 qy 1 0 sinθ0 b b2 = Fx ( q, s0) = q2cosθ0 + q1sinθ0 b b3 = Fy ( q, s0) = q1cosθ0 + q2sinθ 0 b b4 = Fθθ ( q, s0) = ( qq 1 4 qq q2x 0 qy 1 0) cosθ + 0 ( qq qq 2 4 qx 1 0 q2y0) sinθ0 b b5 = Fθ x( q, s0) = q2sinθ0 + q1cosθ0 b b6 = Fθ y ( q, s0) = q1sinθ0 + q2cosθ0 b b7 = Fxx ( q, s0) = 0 b b8 = Fxy ( q, s0) = 0 b b9 = Fyy ( q, s0) = 0 (4.10) Applying Deinition 3 to test the distinguishability o odels (4.7) and (4.8) involves cobining (4.9)-(4.10) in the or o (4.5). It can be directly observed that the pair o equations below cannot be satisied since they contradict q + = in (4.8) q2 1 a5 = b5 0= q2sinθ0 + q1cosθ0 2 2 q1 + q2 = 0 a6 = b6 0= q1sinθ0 + q2cosθ0 (4.11) Since the equations have no solution regardless o whether p or q is given, the two contact state odels are distinguishable Exaple 2 Identiiability o a Polygonal Vertex Edge Contact The identiiability o the contact odel representing the contact state o Fig. 4.1(a) is tested here. To apply Deinition 4, at least ive independent equations are needed to 84

98 solve or the ive paraeters p 1 - p 5. The Taylor series coeicients expressed in (4.9) provide ive equations which can be cobined with the last equation o (4.7) to obtain a set o six equations in the or o (4.6). p5 + ( p2p3 p1p4) cosθ0 ( p1p3 + p2p4) sinθ0 + p2x0 p1y0 pˆ5 ( pˆ2pˆ ˆ ˆ 3 p1p4) θ0 ( pˆ ˆ ˆ ˆ 1p3 p2p4) θ ˆ ˆ 0 p2x0 p1y0 ( pp pp) ( pp pp) ( pp ˆ ˆ pp ˆ ˆ ) ( pp ˆ ˆ pp ˆ ˆ ) = + cos + sin + = cosθ sinθ0 = cosθ sinθ0 p ˆ 2= p2 (4.12) p ˆ 1 = p1 ( pp 2 3 pp 1 4) cosθ0 +( pp pp 2 4) sinθ0 = ( pp ˆ ˆ 2 3 pp ˆ1 4) cosθ0 + ( pp ˆ ˆ ˆ ˆ pp 2 4) sinθ p ˆ ˆ 1 + p2 = p1 + p2 = 1 To obtain all possible solutions or p given ˆp, it can be seen that the third and ourth equations deine uniquely p 1 and p 2 and that this solution also satisies the sixth equation. Given p 1 and p 2, the reaining paraeters appear linearly in the three reaining equations. Consequently, equation (4.12) adits only one solution, given below. p = pˆ p 1 1 = pˆ 2 2 p = pˆ p p 3 3 = pˆ 4 4 = pˆ 5 5 (4.13) By Deinition 4, contact state A is globally identiiable. 85

99 4.3.3 Exaple 3 Identiiability o a Polyhedral Vertex Face Contact To deonstrate the applicability o the approach to polyhedral odels, this exaple considers the identiiability o the contact { V F } shown in Fig g R Robot gripper o R e R R Figure 4.2: Vertex Face contact state. Using the techniques described in section 3.2.2, the contact odel can be written as ollows: 86

100 c F ( p, s) = p10 + K1x( t) + K2 y( t) + K3z( t) + p7[ K1cos β( t)cos α( t) + K2cos β( t)sin α( t) K3sin β( t) ] K1 ( cos α( t)sin γ( t)sin β( t) cos γ( t)sin α( t) ) + p 8 + K2 ( cos γ( t)cos α( t) + sin γ( t)sin β( t)sin α( t) ) + K3 cos β( t)sin γ( t) K1 ( cos γ ( t)cosα()sin t β() t + sin γ()sin t α() t ) + p9 = 0 + K2 ( cos α()sin t γ() t + cos γ()sin t β()sin t α() t ) + K3 cos γ()cos t β() t C : K1 = p1p4p5 + p2p6 K2 = p2p4p5 p1p6 K3 = p3p5 2 2 p1 + p2 = p3 + p4 = p5 + p6 = 1 (4.14) Here, [,,,,, ] e o p p p p p p are used to paraeterize the three unknown rotations used in T and [,, ] p p p represent the unknown translations used in T g. The paraeter p 10 represents the agnitude o the position vector between the world and environent object raes projected on the noral o the contact s ace. The sensor variables { xyzαβγ,,,,, } represent the position and orientation o the robot gripper. At least ten equations are needed to solve or the ten paraeters. Since six sensor variables are available, seven Taylor coeicients are provided through irst order expansion. These equations can be cobined with the last three equations o (4.14) to obtain the desired nuber o equations. Since p 1 - p 6 are the only coeicients ultiplying { x(), t y(), t z() t }, the irst order coeicients with respect to these sensing 87

101 variables can be cobined with the last three constraint equations o (4.14) to produce the algebraic syste o six equations: (, ) (, ) (, ) a= F ps = K= ppp+ pp a = F p s = K = p p p p p c a3 = Fz p s0 = K3 = p3p5 2 2 p1 + p2 = p3 + p4 = p5 + p6 = 1 c 1 x c 2 y (4.15) This syste adits an ininite nuber o solutions given in (4.16) with p 5 and p 6 as ree paraeters. Note that the choice o the ree paraeters is arbitrary; any pair { p, p }, { p, p } or {, } p p can be chosen to solve or the reaining paraeters. By 5 6 Deinition 4, the contact state is unidentiiable ( ) 2 pppp ˆ ˆ pˆ1 p6 + pp ˆ4 5 p1 = p6 + pˆ 4p5 2pppp ˆ ˆ ˆ ˆ p2 p4p5 + p5 1 p2 = p ˆ 6 + p4p5 p ˆ 3 = p3 p ˆ 4 = p ( ) (4.16) Equation (4.16) shows that two out o the three angles speciying the orientation o the environent object can be uniquely identiied. This can be explained geoetrically by noticing that the contact state is invariant under rotations o the environent object about the ace s noral. This suggests that the contact should be re-paraeterized using 88

102 only two angles to odel the orientation uncertainty o the environent object. In this case, p 5 and p 6 can be selected arbitrarily giving a unique solution to (4.16). Note that over-paraeterization is oten a result o applying general odeling techniques, such as the techniques presented in section 3.2. To obtain a inial representation, the unidentiiable paraeters ust be grouped to or identiiable paraeters or eliinated (Gautier and Khalil 1990). This reduction task can be diicult to ipleent analytically or arbitrary odels. The proposed identiiability test is a general tool or detecting the presence o unidentiiable paraeters. Given that p1 - p 4 are identiiable, the identiiability o the reaining paraeters can be investigated by looking at the our reaining irst order Taylor coeicients o (4.14), as shown in (4.17). c a0= F ( p, s0) = p10+ K1x0+ K2y0+ K3z0+ p7u + pv 8 + pw 9 c a4 = Fα ( p, s0) = p7uα + pv 8 α + pw 9 α c a5 = Fβ ( p, s0) = p7uβ + pv 8 β + pw 9 β c a6 = Fγ ( p, s0) = p7uγ + pv 8 γ + pw 9 γ (4.17) In these equations, the variables UVW,, and their derivatives are nonlinear unctions o the known variables α, βγ,, K1, K2, and K 3. Moreover, it can be shown that these our equations are linearly independent. As a consequence, p 7 p 10 can be uniquely identiied given that K1, K2and K3 are known. Thereore, the vertex-ace contact odel is said to be globally identiiable, as long as the orientation uncertainty o the environent object is paraeterized by two angles. 89

103 4.3.4 Exaple 4 Distinguishability o Polyhedral Vertex Face Contacts This exaple exaines the distinguishability o two contacts having the sae topological or. The two contact odels consist o a single vertex o the anipulated object in contact with either o two orthogonal aces o the environent object, as shown in Fig 4.3. The equations characterizing the contacts { V F1 } and { V F2 } are presented in (4.18) and (4.19) respectively. Note that (4.18) and (4.19) are inial paraeterizations obtained by selecting p5 = q5 = 1 and p6 = q6 = 0. Figure 4.3: Two vertex ace contact states in which the location o the contact point is the sae on the anipulated object but on two orthogonal aces o the environent object. 90

104 c' F ( p, s) = p10 + p1 p4x( t) + p2 p4 y( t) + p3z( t) + p7[ p1p4cos β( t)cos α( t) + p2p4cos β( t)sin α( t) p3sin β( t) ] pp 1 4( cos α()sin t γ()sin t β() t cos γ()sin t α() t) + p 8 + pp 2 4( cos γ()cos t α() t+ sin γ()sin t β()sin t α() t) + p3 cos β()sin t γ() t ( ) ' pp 1 4 cos γ( t)cos α( t)sin β( t) + sin γ( t)sin α( t) C : + p9 = 0 + pp 2 4( cos α( t)sin γ( t) + cos γ( t)sin β( t)sin α( t) ) + p3 cos γ( t)cos β( t) 2 2 p1 + p2 = p3 + p4 = 1 (4.18) d F ( q, s) = q10 + qq 1 3x( t) + q2q3 y( t) q4z( t) + q7[ qq 1 3cos β( t)cos α( t) + q2q3cos β( t)sin α( t) + q4sin β( t) ] qq 1 3( cos α()sin t γ()sin t β() t cos γ()sin t α() t ) + + q 8 qq 2 3( cos γ( t )cos α( t ) + sin γ( t )sin β( t )sin α( t )) q 4 cos β( t )sin γ( t ) qq 1 3( cos γ()cos t α()sin t β() t + sin γ()sin t α() t ) D: + q9 = 0 + qq 2 3( cos α()sin t γ() t + cos γ()sin t β()sin t α() t) q4 cos γ()cos t β() t 2 2 q1 + q2 = q3 + q4 = 1 (4.19) Since the orientation o the contacting ace is the only dierence between the two contacts, distinguishability is analyzed by looking at the paraeters deining the unknown orientation o the two aces, i.e., p1 p4 or contact C and q1 q4 or contact D. As a consequence, the distinguishability proble reduces to the coparison o the irst order Taylor coeicients with respect to the position sensing variables. Deinition 3 is applied to the equations ored by cobining the three irst order coeicients o 91

105 (4.18) and (4.19) together with the inal equations o (4.18) and (4.19) in the or o (4.5). (, d 0) x (, 0) (, d 0) = y (, 0) (, ) d (, ) c' a1 = b Fx p s = F q s 1 pp 1 4 = qq 1 3 c' F 2 2 y p s F q s a = b pp 2 4 = qq 2 3 c' a3 = b3 Fz p s0 = Fz q s0 p3 = q p1 + p2 = q1 + q2 = 1 p1 + p2 = q1 + q2 = 1 p1 + p2 = q1 + q2 = p3 p4 q3 q = + = p3 + p4 = q3 + q4 = 1 p3 + p4 = q3 + q4 = 1 (4.20) The two solutions o (4.20) or p given q appear in (4.21). The sae solutions arise when the syste is solved or q given p. These two solutions deine the sae orientation and thus constitute a single solution. p1 = q1 p2 = q2 p = q p4 = q3 3 4, p p p p = q 1 1 = q 2 2 = q 3 4 = q 4 3 (4.21) The inal solution or (4.5) is given by (4.22). p1 = q1 p2 = q2 p = q p4 = q3 p7 = q7 p8 = q8 p9 = q9 p = q (4.22) 92

106 This solution yields the anticipated result indicating the orthogonality o the two contact aces. By Deinition 3, since a solution exists, the two contact states are indistinguishable. I, however, the orientation angle θ o the environent object with respect to the noral o its ront ace is known then: p = q = cosθ 3 3 p = q = sinθ 4 4 (4.23) Equation (4.23) contradicts (4.22); consequently the contact states are distinguishable. This result can be explained geoetrically by noticing that a 90 degree rotation around the environent object s ront ace o the contact depicted in Fig 4.3(a) leads to the contact pictured in Fig 4.3(b), aking the two contacts indistinguishable. On the other hand, i the rotation angle is known, then the two contacts are distinguishable since no transoration than can change Fig 4.3(a) to Fig. 4.3(b) exists. Recall that in Exaple 3, the vertex-ace contact odel used here was ade identiiable by eliinating the paraeter corresponding to rotation about the ace s noral. Exaple 4 reveals that distinguishability can urther restrict the choice o paraeterization (i.e., ree paraeters) beyond what is needed or identiiability. 4.4 Coputational Coplexity Analysis The distinguishability and identiiability tests presented in this thesis reduce to solving the sets o algebraic equations in (4.5) and (4.6). Deterining the actual nuber o solutions to sets o nonlinear algebraic equations constitutes a challenge whose 93

107 diiculty increases with the nuber o equations, ν, in the set. In this section, upper and lower bounds, ν in and ν ax, are derived on the nuber o equations in the sets described by (4.5) and (4.6) or pose equations Identiiability Testing The coplexity o the proposed approach depends on the size o the syste o equations in (4.6) used to test identiiability. The goal o identiiability is to show that there is a unique set o paraeters satisying the syste o nonlinear equations in (4.6). By construction, the syste adits at least one solution. Since exactly deterined systes o nonlinear equations usually adit ultiple solutions, additional equations ay be needed to show uniqueness. As a consequence, a lower bound on the nuber o independent algebraic equations needed to solve or the unknowns is given by n, the nuber o unknown paraeters associated with the given contact equation. ν = n (4.24) in I H () in (4.6) provides β equations, then the nuber o Taylor series coeicient equations, n c, needed to achieve this lower bound is given by: n ν β c in (4.25) 94

108 This is a lower bound on n c since there is no guarantee that the algebraic equations ro the Taylor series are independent. This bound on the nuber o coeicients can be related to the iniu order o the series, k in, needed to produce the by substituting the iniu value o n c ro (4.25) into (4.4). ( k + )! kin = in νin β 0 k = 1,2,3, + > k!! (4.26) An upper bound on the series order can be derived by noting that pose equations involve only cyclic and polynoial unctions o the sensor variables. As a consequence, the ters in the Taylor coeicients start repeating or go to zero ater a inite nuber o dierentiations. The nuber o dierentiations required or a variable to repeat or go to zero is deined here as the degree o cyclicity o the variable and is denoted C (). The degree o cyclicity associated with the angular sensor variables s a (e.g., { (), (), ()} s = α t β t γ t in 3D) equals our since, in every ter (see e.g., (4.14)), the angular a variables appear as linear trigonoetric unctions, e.g., sine or cosine. The degree o cyclicity or the positional sensor variables p s (e.g., = { (), (), ()} s x t y t z t in 3D) equals one since they appear linearly in the kineatic pose equations. See (4.14) as an exaple. Since identiiability testing involves coparing sets o equations having identical structures, the signs o the repeating unctions cancel, which reduces the cyclicity o the angular variables or identiiability testing ro our to two: p ( ) = 2 C s ( ) = 1 a C s (4.27) p 95

109 For exaple, when testing (4.10) or identiiability, it can be seen that b 3 and b 5 yield the sae equations when written in the or o (4.6). As a consequence, the upper bound kax on the series order is given by: k s C s (4.28) ax = di( a) ( a) in which di( s ) = 1 in 2D and ( ) a equations available to test or identiiability is di s = 3 in 3D. Thereore, the axiu nuber o a ν ( k + )! ax ax = + β (4.29) kax!! While ν ax can be large, any o these equations are either zero or redundant. The subset ν * ax ν o nonzero independent equations can be obtained using a coputer algebra ax package. The nuber o equations used to solve or identiiability is bounded as * νin ν νax νax. This bound can related to n c, the nuber o Taylor coeicients, as: ν β + n c ν (4.30) in * ax 96

110 Table 4.1 presents the lower and upper bounds as well as the actual nuber o equations, v act, needed to test or identiiability in Exaples 2 and 3, corresponding to equations (4.7) and (4.14). In both cases, the actual nuber o coeicients needed corresponds to the lower bound. Table 4.1: Bounds on the nuber o equations needed or identiiability testing. ν = n c + β β ν in * ν ν ax ax v act Exaple 2: { V E } Exaple 3: { V F } Distinguishability Testing The goal o distinguishability testing is to show that there is no set o paraeters satisying the syste o nonlinear equations in (4.5). To do so, at least one contradiction, valid or all choices o paraeters, ust be ound in these equations. Assuing that a contradiction is not present in H1( p) = H2( q), at least one Taylor coeicient is needed to establish distinguishability, resulting in the ollowing lower bound: n 1 ν β + 1 (4.31) c in 97

111 In contrast to identiiability testing, the lack o structural siilarities between the let and right sides o (4.5) prevent the sipliications presented or identiiability (i.e., eliination o the redundant and nonzero paraeters). Thereore, an upper bound on the nuber o coeicients or distinguishability testing is not easily established. In practice, the Taylor coeicients are coputed one by one until a contradiction is ound. I no contradiction is ound ater a large nuber o coeicients (i.e., k = 10 in (4.4)), then one gives up without drawing a conclusion on the distinguishability o the contact states. Note that when a contradiction can be ound, the actual nuber o equations coputed or distinguishability testing depends on the order in which the Taylor coeicients are coputed. In Exaple 1, the second order ixed derivatives F θ x and F θ y are needed to prove distinguishability. These derivatives correspond to the sixth and seventh coeicients, respectively, o (4.9) and (4.10). This choice in the order o the coeicients is arbitrary (e.g., F θ x could be coputed beore F θθ ); however, it ipacts the nuber o equations coputed or distinguishability. The worst possible ordering results in the coputation o all the Taylor coeicients associated with the order o the expansion that yields a contradiction in (4.5). This expansion s order is labeled the nuber o equations corresponding to the worst ordering is given as ollows: * k and * ( k + ) ν! ax = + * k!! β (4.32) 98

112 Table 4.2 gives the bounds as well as the actual nuber o equations, v act, needed to test or the distinguishability o Exaples 1 and 4. A second order expansion and irst order expansion were needed or Exaples 1 and 4, respectively. Table 4.2: Bounds on the nuber o equations needed or distinguishability testing. ν = n c + β β ν in ν ax v act Exaple 1 E E { V } vs. { V } Exaple 4 F V { V 1 } vs. { F2 } Suary This chapter presented a ethod or deterining the distinguishability and identiiability o sooth nonlinear algebraic odels describing contact states. The Taylor series ethod provides a uniied approach to testing the capability o candidate odels to estiate both the paraeters and the states. The analytical nature o the approach is well suited or poorly known environents since no nuerical value or the contact paraeters is necessary or the testing. The coplexity o the ethod depends on the nuber o Taylor coeicients that needs to be coputed. For pose equations, it can be shown that this nuber is bounded above and below or identiiability testing and lower bounded or distinguishability testing. For the exaples considered, a odest nuber o 99

113 ters were needed or testing. As a result, no approxiation is being ade when using the series or testing contact distinguishability and identiiability. In the context o contact state estiation, the proposed identiiability and distinguishability tools oer the ollowing advantages. First, the identiiability tool provides a useul ethod to redesign contact odels by detecting and reoving the unidentiiable paraeters, as discussed in exaple 4. Second, the distinguishability tool provides a practical way o analyzing the partitioning o the sensor space corresponding to the contact states. As illustrated in exaples 1 and 4, pose inoration is suicient to distinguish between the two polygonal contacts { V E } and { V E }, whereas it is not suicient to distinguish between the two polyhedral contacts { V F1 } { V F2 }. As a result, the contacts { V E } and { V E } regions in the sensor space, whereas contacts { V F1 } and { V F2 } and or two separate are luped into the sae region in the sensor space. While the exaples presented here involved only eleental contacts based on pose equations, the ethodology is equally applicable to ore coplex contacts using additional sensor odalities. Additional work is needed to test the identiiability and distinguishability o all o the contacts associated with the polygonal and polyhedral objects presented in Table Moreover, the relationship aong distinguishability in dierent sensor spaces reains to be investigated. 100

114 In this chapter, the distinguishability o contacts is based on the structure o the contact odels and sensor inoration. The next chapter discusses how additional inoration, such as paraeter history, can be used to augent the distinguishability o contact states. This augented distinguishability is then used to investigate the observability o contact states in poorly known environents. 101

115 Chapter 5 Contact State Observability and Task Encoding Given a geoetric description o the objects coposing a task and a contact state topology, a contact state graph representing all possible contact sequences that can lead to task copletion can be generated. This contact state graph can then be used or task planning, assuing that each state o the graph can be deterined using sensor data. The objective o this chapter is to investigate whether it is possible to distinguish each contact state ro the others inside a contact state graph, given that the environent is poorly known. This property, known as observability, is essential when testing the easibility o a task. The irst section o the chapter discusses task representation, and an exaple illustrating the construction o a contact state graph or a block placing task is provided. 102

116 The second section o the chapter analyzes the observability o the exepliied contact state graph in the context o poorly known environents. To this end, the contact state graph obtained ro task decoposition is reduced to account or the distinguishably constraints associated with poorly know environents. Then, a orward projection ro the observable reduced graph to the contact state graph using a inite history o the contact state paraeters is presented as a solution to the observability proble in poorly known environents. Once a contact state graph is shown to be observable, then it can be encoded to be used inside a contact state estiator. The inal section o this chapter addresses the proble o encoding a state graph in a or that is well suited or ipleentation purposes. The approach eployed here uses a probability transition atrix to atheatically represent the task and the likelihood o transitions between contact states. 5.1 Task Representation Manipulation tasks involving geoetrical objects are naturally decoposed into contact states. Given a list o contact states (i.e., contact topology in Chapter3), several algoriths have been developed to autoatically decopose a task into sequences o contact states (Xiao and Ji 2001). These sequences are then assebled into a contact state graph in which each node represents a contact state and each link represents a possible transition ro a contact state to another. In this thesis, autoatic decoposition o the task is not addressed; however, an exaple o anual decoposition o an assebly task requiring the placeent o a block is considered. 103

117 5.1.1 Contact Topology As illustrated in Fig. 5.1, the task is deined by two polygonal objects. The block is assued to be rectangular, and the edges deining the corner are assued to be ininite. As a consequence, the task involves two edges ro the ixed corner (i.e., E1, E 2 ), and our edges and our vertices ro the anipulated object (i.e., E1 4, V1 4). The block is assued to be grasped (i.e., crossed circle) by a planar anipulating robot. V 2 V 3 E1 V1 2 E 2 E4 E3 V4 Manipulated block E1 E? START Fixed corner GOAL Figure 5.1: Block-in-corner assebly task. By enuerating all the possible contacts, it can be seen that 25 contacts can occur between the edges and vertices coposing the task. As shown in Fig. 5.2, the task coprises one no-contact, eight vertex-to-edge contacts, eight edge-to-edge contacts, our double vertex-to-edge contacts, and our double edge-to-edge contacts. The black wedge inside the crossed circle o the anipulated object indicates its orientation. 104

118 Figure 5.2: Contact state topology o a block-in-corner assebly task Task Decoposition Using the contact topology presented in Fig. 5.2, the contact state graph associated with the block-in-corner task can be generated anually, as shown in Fig This graph coprises 128 possible transitions between states. The transitions between the nocontact and the double contacts are not odeled since they are very unlikely. The nuber o possible contact state sequences required to reach the corner position ro the ree otion position depends on the nuber o connections involved in the sequence. It should be clear that this nuber becoes large very quickly. 105

119 C 5 V E 2 2 V3 E1 E3 V E 1 4 V4 E1 E2 START C 0 C 13 C 1 C 21 C 6 C 14 GOAL C 2 C 7 C 15 C 22 C 8 C 16 C 9 C 17 C 3 C 23 C 10 C 18 C 4 C 11 C 12 C 19 C 20 C 24 Figure 5.3: Contact state graph representing a block-in-corner assebly task. 106

120 5.2 Contact State Observability For a task to be easible, all the contact states coposing a given contact state graph ust be distinguishable. For exaple, the goal o the task illustrated in Fig. 5.3 is to place the anipulated block in contact with the environent corner. The task is copleted when contact state C 1 is reached. Thereore, contact C 1 needs to be distinguished ro all the other contacts coposing the graph since reaching a goal without recognizing it does not result in task copletion. The concept o distinguishability presented in section 4.1 tests the distinguishability o contact states based on current sensing inoration; however, this inoration ay not be suicient to recognize all the possible contacts coposing the task. The distinguishability o every contact state inside a contact state graph is discussed in this section by introducing the concept o observability. By analogy with control theory, the concept o observability is deined as ollows: Deinition 5.1. A contact state graph is said to be observable i, given a inite history o contact state transitions, the distinguishability o the contact odels (as deined in Chapter 4) together with the task inoration are suicient to deterine the initial contact state. This proble o observability can be related to the proble o recognizability introduced by Erdann (1986) and Donald and Jennings (1991) (i.e., see section 2.1.6). In this context, the notion o task inoration represents the task based knowledge that 107

121 can be utilized to augent the distinguishability o the contact states obtained ro the current sensing. The objective o this section is to investigate contact state observability in poorly known environents. To this end, the observability o the contact state graph illustrated in Fig. 5.3 is discussed. First, it is shown that only groups o contact states can be distinguished inside the contact graph when assuing that the environent is poorly known. As a result, a reduced graph containing only groups o distinguishable contact states is extracted using distinguishability testing. Second, task inoration, represented as a inite history o the contact state paraeters, is presented as a way o augenting distinguishability. Finally, distinguishability testing and a orward projection o the paraeter history are cobined to show observability Distinguishable State Graph As discussed in Chapter 4, every pair o contacts having the sae topological or is indistinguishable without urther inoration regarding the orientation o the edges or the location o the vertices. Analyzing the contact state graph o Fig. 5.3, it can be observed that only ive dierent groups o topologically dierent contact states are utilized to construct the graph: no contact, vertex-to-edge, edge-to-edge, double vertex-to-edge, and double edge-to-edge. Using the distinguishability test presented in section 4.2.1, these groups o contacts can be shown to be distinguishable; however the individual contacts inside the groups are indistinguishable. (e.g., C 13 and C 14, C 10 and C 12 ). 108

122 Figure 5.4 shows how the 25 contact states o Fig. 5.3 can be reduced to a ive-state graph in which each state coprises an indistinguishable subset o the original states. This reduced state graph is deined as a distinguishable graph; it corresponds to a iniu representation o the task iposed by the current sensing o the syste. Each node in this reduced graph is distinguishable ro the others. S 0 S 1 S2 S3 S4 Figure 5.4: Distinguishable contact state graph Observability Conjecture Planning in the distinguishable state graph shown in Fig. 5.4 does not have substantial value since each node represents a group o several contact states. To be practical, planning has to be done in the contact state graph o Fig This section investigates 109

123 how task inoration can be used to augent distinguishability and achieve contact state graph observability. In the context o poorly known environents, the observability proble can be stated as ollows: how any transitions are needed in a distinguishable graph to deterine a contact state in its associated contact state graph? Based on current sensing inoration, the type but not the exact state can be deterined; however, distinguishability can be augented by using the inoration contained in the paraeter estiates. In act, it can be noticed that paraeter values can be used to label contact states. As an exaple, the topologically equivalent contact states C 13 and C 16 in Fig. 5.3 are considered. The vertex-edge contact equation derived in (3.33) is satisied when either contact C 13 orc 16 is active. The position o the top right vertex V 1 is estiated when C 13 is active, whereas the position o the botto let vertex V 3 is estiated when C 16 is active. As a result, storing the paraeter history o the task can help to distinguish the contact states associated with the task. This reasoning leads to the ollowing conjecture: Observability Conjecture (2D): Given the label, but not the location o the closest anipulated object vertex to the gripper rae, the contact state path to deterine the contact state history is one that perits estiation o all anipulated and environent object odel paraeters. 110

124 The idea o the conjecture is to extract all the object eatures (i.e., vertices and edges in 2D), with respect to a ground truth (i.e., the label o the closest anipulated object vertex to the gripper rae is known), that were used to build the contact state graph. Once this paraeter inoration is known, then it can be cobined with distinguishability testing to deterine the contact states inside the state graph. For exaple, i a vertex-edge contact is detected and the estiated paraeters correspond to the location o the vertex V 1 and the orientation o the edge E 2, then it can be concluded that contact C 13 has been reached. To illustrate this conjecture, the paraeter history associated with the sequence o states shown in Fig. 5.5 is analyzed. The unknown grasp location o the anipulated object is represented by a crossed circle. The vertex V 2 is assued to be the closest to the gripper. Starting ro the vertex-edge contact C 13, the anipulated object is rotated clockwise our ties then slid until it reaches the botto edge o the ixed corner, resulting in the state sequence C13 C11 C20 C9 C17 C

125 V E 2 2 V3 E1 E3 V E 1 4 V4 E1 E2 C 13 C 9 C 17 C 23 C 11 C 20 Figure 5.5: Exaple o a contact sequence that perits estiation o all anipulated and environent object odel paraeters. 112

126 Figure 5.6 shows the state sequence and the associated paraeter history cobined with the known geoetry o the task. First, the location o the vertex V 1 and the orientation o the edge E 2 are estiated. Second, the orientation and part o the length o the edge E 1 are estiated. Third, the vertex V 2 is estiated resulting in the coplete identiication o the length o E 1. Fourth, the orientation and part o the length o the edge E 2 are estiated. Fith, the location o the vertex V 3 is identiied and cobined with the paraeter history and the a priori knowledge o the objects shape to estiate the reaining parts o the rectangular anipulating object. Finally, the orientation o the edge E 1 is estiated. At this point, the inoration obtained ro the paraeter values is suicient to perit estiation o all anipulated and environent object paraeters. As a result, contact state C 23 is deterined in the contact state graph shown in Fig. 5.3, and recursively, all the contact states leading to C 23 are also deterined. By deinition 5.1, the contact state graph is said to be observable. 113

127 114 Figure 5.6. Forward projection on the paraeter history. a) Contact state sequence in which the block is rotated clockwise and then slid toward the botto edge o the ixed corner, and b) The paraeter history is cobined with the knowledge o the objects shape resulting in a 2D odel o the anipulated object and its environent. a) b) C 13 C 11 C 20 C 9 C 17 C 23 1 E 2 E 1 V 1 V 1 V 2 V 1 E 1 V 2 V 1 E 2 E 2 E 2 E 2 E 1 E 2 E 2 V 1 V 4 V 3 V 1 E 2 E 3 E 4 E 2 V 1 V 4 V 1 E 2 E 3 E 4 E 3 V 2 E

128 Ultiately, the concept o observability is goal dependent. I the goal is to put the cube inside the corner regardless o its orientation, then the distinguishable state graph provides suicient inoration. However, the ore speciic the goal is, the ore critical the proble o observability becoes. Paraeters were assued to be perectly estiated in this discussion. Nevertheless, noise or poor excitation can result in an erroneous estiation o the paraeters that can degrade the observability o the graph. The sequence o states presented in Fig. 5.6 results in a coplete geoetric construction o the objects used in the task; however, not every sequence o contact states results in a ull odel. The question o the necessary nuber o steps needed to construct arbitrary polygons or polyhedrons is not addressed in this thesis. A suicient condition can be provided using orward projection on the paraeter history. The sequence proposed in Fig. 5.5 is one possible projection; however other sequences also exist. 5.3 Task Encoding I a contact state graph is shown to be observable, then it eans that enough inoration (i.e., sensing inoration, paraeter history, and task structure) is available to disabiguate every contact state inside the graph. As a next step, the observable contact state graph needs to be encoded in a atheatical or suitable or ipleentation in a contact state estiator. The approach eployed here uses a probability transition atrix to represent the probability o speciic contact state transitions ebodied in a task contact state network. 115

129 5.3.1 Contact State Network Probabilistic approaches can be used to encode the likelihood o transitions aong the states o a contact state graph. In this context, the contact state graph is transored into a network structure in which each node corresponds to a contact state, and each transition aong states (including sel-transition) is weighted by its probability o occurrence. As an exaple, the state network associated with the distinguishable state graph o Fig. 5.4 is presented in Fig Grey connectors represent transitions that were not odeled in the state graph (i.e., ineasible transitions or highly unlikely transitions), and the a ij s are used to label the possible transitions. S 0 : { } V V E E 0 S 1 : { V E } S 2 : S 3 : { E E } 4 a 00 a 11 a22 a33 a44 S : E E E E a01 a12 a 23 a 34 S 0 a S 1 a S2 S a 32 a 43 a 02 a a a a a a 31 a a 41 a 30 a 04 S 4 a 40 Figure 5.7: Distinguishable state network. 116

130 The state network can be represented in a copact way using an n n probability transition atrix A, as described in (5.1). Each eleent o the atrix, a ij in (5.2), corresponds to the probability o being in a state i given the previous state j. As an exaple, the probability transition atrix associated with the network described in Fig. 5.7 is shown in (5.3). In this exaple, the probabilities associated with ineasible or highly unlikely transitions are set to zero. The selection o the reaining probabilities is discussed in the next section. a00 a01 a0( n 1) n 1 a10 a11 a1 ( 1) aij 1 n = A= with j= 0 aij 0 a( n 10 ) a ( n 1)( n 1) (5.1) ( + 1 / ) a = P c = C c = C (5.2) ij t i t j a00 a01 0 a03 0 a a a a 0 A a a = a30 a31 0 a33 a a a (5.3) Selection o the Probability Transition Matrix This section presents a two-step ethod to assign values to the transition probability atrix. As a irst step, siple rules corresponding to the liited knowledge o the task 117

131 are used to assign the transition probabilities. As a second step, these probabilities are weighted by counting how oten each transition is used in the state graph. Step 1: Epirical Knowledge As a irst step, a set o siple rules based on epirical knowledge o anipulation tasks is used to assign the transition probabilities. These rules are as ollows: State transitions are short: a > a, 0 i, j n 1 ii ij The no-contact to contact transition is ore likely to occur with single contacts than with ultiple contacts (e.g., a01 >> a02 in (5.3)) Once the anipulated block is in contact, it stays in contact: a 0 = 0,1 i n 1 Unconstrained transitions are set uniorly ollowing the row constraint o (5.1) i The third rule is controller dependent and can be relaxed depending on the application (e.g., teleoperated anipulation task). These rules are applied to the state network o Fig. 5.7 resulting in the sybolic transition atrix shown in (5.4). Nuerical values can be assigned by setting the sel-transition probabilities. The sel-transitions are set to 0.8 in this exaple (i.e., aii > a ) and the reaining probabilities are set uniorly ollowing the ij row constraint o (5.1), as shown in (5.5). 118

132 a00 a01 0 a a11 a12 a13 0 A 0 a a 0 0 = a31 0 a33 a a a (5.4) A = (5.5) Step 2: Adjacency Matrix Analysis As a second step, the probabilities o the non-zero o-diagonal ters are weighted by counting how oten each transition is used in the state graph. This coputation can be done using the adjacency atrix associated with the state graph. The adjacency atrix is siply a transition atrix in which each easible transition is set to one and others (including sel transitions) are set to zero (Wilson 1985). One interesting property o this atrix is that its th k power gives the nuber o paths o length k between two states. For exaple, the eleent (1, 2) o the third power adjacency atrix associated with the contact state graph o Fig. 5.3 corresponds to the nuber o paths having three connections between the contacts C 0 and C 1. In this exaple, there exists our paths with three transitions as shown in (5.6). 119

133 C C C C C C C C C C C C C C C C (5.6) Thereore, the adjacency atrix can be used to count all the transition occurrences in the contact state graph by coputing all the possible sequences o lengths k (i.e., k = 1, 2, 3 ) between the initial and inal contact states (i.e., the start state and goal state in Fig. 5.3). For each k, the transition occurrences are sorted into a n n state transition atrix D k with zero diagonal eleents. Suing the atrix D k over a large range o k (e.g., k ax = 10) indicates how oten each transition occurs in the state graph. This nuber is then noralized to obtain the likelihood o transitions (excluding seltransitions) associated with the state graph as shown in the atrix * A in (5.7). kax Dk (, i j) * k = 1 A (, i j) = where i j n kax D (, i l) l= 1 k= 1 k (5.7) As a inal step, the transition probability atrix is obtained by setting the seltransitions probabilities and using equation (5.8) or the o-diagonal ters. ( ) * Ai (, j) A(, i j)1 Aii (,) wherei j = (5.8) 120

134 This technique can also be used to assign the transition probabilities associated with the distinguishable state graph o Fig To this end, the transitions o the contact state graph needs to be sorted according to the possible transitions o the distinguishable state graph. For exaple, the twelve transitions (i.e., our sequences with k = 3 transitions) o (5.6) correspond to our a 01 transitions, our a 13 transitions, and our a 34 transitions in (5.4). This result is written in a copact or in the atrix H k in (5.9). The coputational coplexity associated with the H k atrix depends on the length o the sequence k and the size o the adjacency atrix. H = (5.9) For the block-in-corner exaple presented in Fig. 5.3, sequences staring at C 0 and ending at C 1 were coputed or up to 10 transitions (i.e., k ax = 10) resulting in ore than 70,000 transitions in the contact state graph. These transitions were then sorted with respect to the distinguishable state graph and noralized according to (5.7), as shown in (5.10). 121

135 A * = (5.10) As a inal step, sel-transitions are set to 0.8 and equation (5.8) is utilized to obtain the probability transition atrix associated with the distinguishable state network o Fig A = (5.11) 5.4 Suary The concept o observability is iportant in the design o contact state estiators since a task ight not be easible i the contact state graph is unobservable. In the context o poorly known environents, observability was solved using a ap between the distinguishable graph representing the task in the poorly known environent and the contact state graph representing the task in the structured environent. This apping was deined as a orward projection on the paraeter history associated with the execution o the task. 122

136 The next two chapters ocus on the ipleentation o a contact state estiator. In particular, it is shown how the probability transition atrix presented in this chapter can be used inside a decision test to represent the task knowledge necessary to the segentation o the contact states. 123

137 Chapter 6 Paraeter Estiation and Data Excitation Contact state estiation is a dual estiation proble involving the identiication o contact state paraeters as well as the estiation o the contact states corresponding to the sensor inoration. As shown in Fig. 6.1, the irst part in the ipleentation o a contact state estiator is the realization o a ultiple odel estiation algorith. This algorith estiates the paraeters and residuals associated with each contact odel, given sensor data and a list o contact state odels. The second part o the contact estiator uses these results as inputs to a decision test that deterines which one o the contact odels is the ost likely to correspond to the sensor data. This chapter addresses the ipleentation o the estiation algorith utilized during ultiple odel estiation. The next chapter will discuss the ipleentation o the decision test. 124

138 Sensor Data Multiple Model Estiation Contact odel Contact odel Contact odel C1: 1( p1, s t ) = C : ( p, s ) = 0 C : ( p, s ) = 0 t n n n t ESTIMATION ALGORITHM Paraeter estiates: Estiation residuals: p1, p2,, pn ε (), t ε (), t, ε () t p1 p2 p n Decision DECISION ALGORITHM Task encoding Estiated Contact State Estiated Model Paraeters Figure 6.1: The two parts o the ipleentation o a contact state estiator: ultiple odel estiation and contact state estiation. The estiation algorith o Fig. 6.1 needs to satisy two iportant criteria when used or contact state estiation in poorly known environents: 1) the algorith ust be ast, and 2) the algorith ust be robust to a poor choice o initial estiates. These two criteria are highly dependent on the excitation o the sensor path since a poor choice o excitation can result in slow convergence or erroneous estiates. As an exaple, 125

139 consider a probe o unknown length L, oving on an edge o unknown slope β, as depicted in Fig The position and orientation o the probe {,, } x y θ are suicient i i i inoration to identiy L and β. A bad choice o sensor excitation, however, can result in erroneous paraeter estiates as illustrated in Fig. 6.2(a) and Fig. 6.2(b). In the irst case, the probe is pivoting around the contact point resulting only in the identiication o the probe s length L. In the second case, the probe is sliding with a constant orientation resulting only in the identiication o the edge s slope. Intuitively, a good excitation requires all but one sensor signal to be independently excited as depicted in Fig. 6.2(c). Finding an exciting path can be challenging since the contact constraint liits the possible conigurations and reduces the possible sensor paths. As a result, excitation conditions that can guarantee the estiation o all the paraeters associated with the contact odels are needed when ipleenting the estiation algorith o a contact state estiator. sensor path probe x, y, θ L x, y, θ + n n n contact point β a) b) c) Figure 6.2: Probe oving on an inclined line. a) The probe is pivoting around the contact point resulting in a poor exciting path. b) The probe is sliding with a constant orientation resulting in a poor exciting path. c) The probe is sliding and rotating resulting in an exciting path. 126

140 This chapter addresses the two probles o paraeter estiation and sensor excitation. Two estiations algoriths are presented: an iplicit ethod and an explicit ethod. The iplicit ethod is used in the ipleentation o the contact state estiator, whereas the explicit ethod is utilized in the design o sensor paths. The iplicit ethod uses the Levenberg-Marquardt algorith, a nonlinear least squares technique, to estiate the paraeters. This ethod is well suited or poorly known environents since no accurate noinal paraeter values are needed to initialize the algorith. The explicit ethod uses ultiple dierentiations o the sensor signals to provide closed-or solutions or the paraeters associated with the contact odels. Even though this ethod is ar too sensitive to sensor noise to be used in practice, it provides an interesting basis to analyze the eect o sensor excitation on paraeter estiation. 6.1 Estiation Algorith: Iplicit Solution Paraeter estiates and sensor excitation conditions can be explicitly characterized when a closed-or solution to the estiation proble is available. Obtaining such a solution is relatively easy or linear algebraic systes (i.e., Gaussian eliination); however, it still reains a diicult challenge or nonlinear systes. As a consequence, iplicit solutions based on nuerical optiization techniques are coonly used (e.g., Gill et al. 1981, Ljung 1987). The objective o these algoriths is to ind a solution that can iniize a given cost unction. For contact state estiation in a poorly known environent, the estiator needs to be robust to poor initial conditions and needs good convergence properties. The Levenberg-Marquardt ethod, a least squares iniization 127

141 technique corresponding to these criteria, is reviewed in the next section (Gill et al. 1981) The Levenberg-Marquardt Algorith As shown in Chatper 3, contact states can be odeled as nonlinear hoegeneous equations paraetrized by sensor variables st () and unknown constant paraeters p : ( p, s( t )) = 0 (6.1) The su o squares unction deined in (6.2) is used as a cost unction to iniize (6.1) with respect to p. This unction is coonly chosen or its siplicity and algebraic properties (Gill et al. 1981) i i i= T T F( p) = ( p, s( t )) ( p, s( t )) ( p) ( p) ( p) (6.2) where ( p) ( p, s( t )) ( p, s( t )) ( p, s( t )) = 1 2 n T The goal o the iniization process is to ind a search direction d, in the paraeter space, that can result in a large reduction o F( p ). The direction is then used to update the estiate as shown in (6.3). 128

142 p p d (6.3) = + k+ 1 k k I F( p ) is linearly approxiated, then a siple way o iniizing this cost unction is to ollow the direction o the negative gradient. This direction is known as the steepest descent and is deined as ollows: d = g (6.4) k k Steepest descent ethods are easy to ipleent since they only require the coputation o the gradient; however, they are not very eicient (i.e., slow convergence). A ore eicient solution is to approxiate the cost unction by a quadratic: T 1 T F( pk + d) Fk + gkd + d Gkd (6.5) 2 where g is the gradient and G is the Hessian. The iniu is achieved when the gradient vanishes, yielding the search direction that solves equation (6.6). This direction is known as the Newton direction (Gill et al. 1981). Gd k k = g k (6.6) The nature o the least squares cost unction allows or easy coputations o the gradient and the Hessian atrix: 129

143 T g( p) = J( p) ( p ) (6.7) T G( p) = J( p) J( p) + Q( p) (6.8) Least squares ethods usually assue that the irst order ter o the Hessian doinates the second-order ters (i.e., Q( p ) ). This assuption is valid only i the proble has a sall residual at the solution. Since hoogeneous equations are used to odel contact equations, the assuption is satisied or the contact state corresponding to the observed sensor data. As a result, both the Hessian and the gradient can be coputed using only J( p ), the Jacobian atrix o ( p ). Assuing that Q( p ) can be neglected, the Newton direction (6.6) becoes: T ( ) 1 T d = J J J (6.9) k k k k k I the quadratic or is a bad approxiation, the best thing to do is to utilize the steepest descent direction deined in (6.4) with the gradient equation (6.7). d = T k Jk k (6.10) The Levenberg-Marquardt (L.M.) provides a ethod to switch between Newton s ethod and the steepest descent ethod. The idea is to use the steepest descent when ar ro the iniu and then switch to the Newton s ethod when close to the iniu. The L.M. direction is given by (6.11), where λk is non-negative scalar. I λk is zero then 130

144 the L.M. direction reduces to the Newton direction, and i λ k tends to ininity then the L.M. direction reduces to the steepest descent direction. T ( λ ) 1 d = J J + I J (6.11) T k k k k k k Several approaches can be ipleented to copute λ k (Nuerical Recipes, p.684, 1988). The idea is to increase or decrease λ k when the cost unction at the current iteration is greater or saller than the cost unction at the previous iteration. The L.M. ethod is an unconstrained least squares technique. As a consequence, the contact equations ust be taken in their unconstrained ors. As an exaple, the algebraic equation corresponding to a { } V E contact is considered in (6.12). This equation can be utilized with a least squares estiator i p 1 and p 2 are replaced by their trigonoetric counterparts, as shown in (6.13). ( ) θ ( ) p5 + p2p3 p1p4 cos () t p1p3 + p2p4 sin θ() t + p2x() t p1y() t = p1 + p2 = 1 p = sin p 1 0 p = cos p 2 0 (6.12) (6.13) 131

145 6.1.2 Estiator ipleentation As shown in (6.3), nonlinear optiization ethods are iterative. Thereore, the rate o convergence is an iportant actor when deciding on an optiization algorith. The Levenberg-Marquardt algorith has been shown to have linear convergence during its steepest descent ode and quadratic convergence during its Newton ode (Gill et al. 1981). As a consequence, the rate o convergence o the algorith depends on the choice o initial conditions. A poor choice o initial conditions will ostly yield linear convergence while a good one will ostly result in quadratic convergence. The estiator can be ipleented in a batch ode or in a recursive ode. Traditionally, batch techniques are used or estiating tie-invariant paraeters while recursive techniques are used or estiating tie-varying paraeters. In the context o ultiple odel estiation, the paraeters associated with all o the contact states ust be estiated siultaneously, as shown in Fig In this situation, paraeters can be seen as quasi tie-invariant or quasi tie-varying since they are constant or the odel associated with the active contact and varying or the other contact odels. As a result, both recursive and batch techniques can be used with soe odiications to estiate the paraeters. A recursive technique can be used with an added orgetting actor to liit the corruption o the good estiates when transitioning ro one contact state to another. Siilarly, a batch technique could be used in a sliding window o ixed length as shown in Fig In this research, the latter estiation technique is used since batch nonlinear least squares shows aster convergence and better robustness to noise than its recursive counterpart. The selection o the oving data window length δ is 132

146 deterined by two considerations. Its lower bound is provided by n, the nuber o paraeters to be estiated in (6.1). This is the iniu nuber o tie steps needed to solve the over-deterined set o equations representing the contact states. Its upper bound corresponds to the iniu tie interval a contact state is expected to be active. For a iniu tie interval T and sapling requency F s, the window length is bounded by: n< δ < F T (6.14) s Figure 6.3: Estiation using a sliding window estiation schee. 133

147 In practice, sliding nonlinear least squares can be coputationally intensive, especially when poor initial conditions are chosen. One easy solution is to set the speed o the anipulator robot such that it allows enough coputational tie. A second and ore elegant solution is to use the estiates associated with a detected active contact state to initialize all the contact odels that share the sae paraeters. This type o reinitialization can be done ater a contact transition has been detected. Ipleentation o this technique will be discussed in section Excitation Condition Ideally, contact states have the or presented in (6.1). Realistically, however, sall errors due to sensor noise and unodeled disturbances result in a nonzero residual ε in (6.15). We ake the assuption that these errors can be odeled as norally distributed rando variables with zero ean and covariance σ 2. As a result, the residual s probability distribution (pd), deined as p ε or a given contact state C i, is as ollows: ( p, s) = ε ( ε ) p = P C ε / i (6.15) Due to the nonlinear structure o ( ), the pd o (6.15) is hard to characterize analytically; however, it can be estiated o-line using Monte-Carlo siulations. As an an exaple, Fig 6.4 shows the result o a Monte-Carlo siulation corresponding to a vertex-to-edge contact. First, given the diensions o the polygons and the location o 134

148 the static object (i.e., illed rectangle), the coniguration space corresponding to the contact state is coputed (C-surace in Mason 1981). The space is discretized, and 2,500 conigurations (i.e., x, y, θ ) are coputed. Fig. 6.4(a) shows one possible coniguration. The conigurations are then corrupted by adding a norally distributed noise o zero ean and variance σ ( σ = σ = σ = 0.5 or this siulation). The residual is x x θ coputed or each coniguration and the residuals pd is built by noralizing the residuals histogra. The process is repeated or rectangles o variable sizes and locations. The diensions o both rectangles and the coniguration o the ixed rectangle are chosen randoly inside a unior distribution. Fig. 6.4(c) shows the pd resulting ro one hundred dierent rectangles (i.e., residuals were coputed to build the pd). It is clear that the distribution can be well approxiated using a uniodal Gaussian. To be coplete, this siulation should be run or every possible contact state. 135

149 a) b) y () p ε x () ε () c) 0.06 p ε ε () Figure 6.4: Contact s residual pd by Monte Carlo siulation. a) A siulated coniguration o a vertex-to-edge contact, b) Contact s residual pd corresponding to objects o ixed sizes and locations, and c) Contact s residual pd corresponding to objects o dierent sizes and locations. Using a linear approxiation, the covariance atrix o the paraeters K can be expressed using the Hessian and the covariance o the error (Bates and Watts 1988): ˆ ( ˆ) 2 σ = G p K (6.16) 136

150 Least squares techniques assue that the second order ter o the Hessian can be neglected (i.e., (6.8)), and as a result (6.16) can be sipliied as ollows: ( ) 1 2 ( ˆ) T ( ˆ) ˆ K J p J p σ (6.17) The aount o uncertainty in the paraeters is easured by the paraeter covariance atrix K. This uncertainty can be used to derive a conidence interval on the estiates: Kii Kii pˆ ˆ i t pi pi + t α α (6.18) where t α is the upper (1 α) 2 critical value or a student distribution with 1 degrees o reedo. For a 95% conidence interval, α = Tables o t-values can be ound in statistics books, e.g., Dougherty (1990). Excitable paths should result in sall intervals in (6.18). Since the paths (i.e., sensor data) only aect the covariance atrix, excitation is directly related to the agnitude o diagonal ters o the K atrix. It is clear ro (6.17) that good excitation should ensure T that the Hessian atrix J( p) J( p ) is invertible. The Hessian atrix is well-conditioned i the Jacobian is ar ro singularity and badly conditioned otherwise. The condition nuber κ is coonly used to test or atrix conditioning (Lawson and Hanson 1974). In the literature, atrices with a low condition nuber are generally considered well 137

151 conditioned (Schröer et al. 1992). Paths yielding a condition nuber ( J p ) κ ( ) 100 are typically considered as exciting paths. Note that the Levenberg-Marquardt ethod already iproves the conditioning o the Hessian inversion by adding a constant to the ain diagonal o the atrix to be inverted. 6.2 Estiation Algorith: Explicit Solution Explicit ethods provide closed-or solutions or the paraeters associated with the contact odels that depend only on the sensor variables. Closed-or solutions are interesting since they are coputationally eicient and provide insights on the type o sensor paths that need to be used to estiate the paraeters (i.e., excitation conditions). This section presents an explicit estiation technique or the paraeters associated with the pose equation o (6.12). Exploiting the structure o the kineatic equations, ultiple dierentiations o the contact equations are used to decouple the nonlinear estiation proble into two sub-probles. First, the nonlinear apping between the Taylor coeicients characterizing the contact odel and the paraeters is deined. Then, the nonlinear relationship between the Taylor coeicients and the sensor variables is expressed using ultiple dierentiations o the contact odel. These two nonlinear aps are then cobined to provide a closed-or solution or the paraeters that depends only on the sensor easureents and their derivatives. 138

152 6.2.1 Estiation Through Dierentiation As discussed in section 4.4, pose equations can be characterized exactly by a inite nuber o Taylor coeicients. In that context, the goal o identiiability testing is to show that there exists a unique inverse apping between the Taylor coeicients representing the contact equations and the paraeters, as shown in (6.19). In this equation, s is the inite vector o Taylor coeicients associated with the algebraic contact state odel shown in (6.1). 1 = s (, ( )) p g s t (6.19) I the inverse o g is unique, then the odel is said to be globally identiiable. The odel is locally identiiable i a inite nuber o inverse appings exist. The easibility o the inversion as well as its coputation was the ocus o the identiiability test presented in section 4.2. This inversion can be diicult to solve by hand; however, tools ro coutative algebra can oten be used to obtain a solution (e.g., Gröbner bases). As an exaple, the inversion o the equations used to characterize the pose equations o the globally identiiable contact { } V E described in (6.12) is presented in (6.20). Note that or this exaple the inversion is easily done by hand. 139

153 ( ) θ ( ) ( ) = ( ) θ ( ) (, ) = 2 (, ) = 1 ( ) ( ) θ ( ) ( p, s) = p5 p1p3+ p2p4 cos ( t) p2p3 p1p4 sin θ( t) p1x( t) p2y( t) = 0 θ p, s p p p p cos ( t) p p p p sin θ( t) x p s p y p s p θθ p, s = p2p3 p1p4 cos ( t) + p1p3+ p2p4 sin θ( t) p1 = x p2 = y p3 = ( θ y xθθ ) cos θ ( t) ( θ x + y θθ ) sin θ ( t) p4 = ( θ x + y θθ ) cos θ( t) ( θ y x θθ ) sin θ( t) p5 = θθ xx() t yy() t (6.20) To obtain an explicit solution or the paraeters, the Taylor coeicients o (6.19) are expressed using the known sensor variables st ( ). The chain rule is used to express the exact dierentials o the analytic unction ( pst, ( )) as a unction o Taylor coeicients and exact dierentials o the sensor variables. Equation (6.21) shows the dierentiation or three sensor variables {, x, y} θ. ( x y θ ) 0 =,, 0 = d = θ dθ + xdx + ydy 0 = d = d + d x + d y + d + dx 2 + yy ( dy) + 2θxdθdx + 2θydθdy + 2 xydxdy n 0 = d = ( θ) ( ) θ θ x y θθ xx (6.21) 140

154 The hoogeneous syste o equations represented by (6.21) can be written in a copact or as the product between the Taylor coeicient vector s and a atrix M which is unction o the dierentials o the sensor variables, as expressed in (6.22). Moreover, the special structure o the kineatic equations, expressed through the inite nuber o Taylor coeicients, can be used to reduce the atrix M to a n c n c square atrix, where n c represents the nuber o nonzero and linearly independent Taylor coeicients. Equation (6.23) provides an iterative schee to obtain the excitability atrix M. n ( ) n 0 = d = M d s s (6.22) ds j 1 j di( s) M1, j = 0 di( s) < j nc M dm M T i n j n Tn : c n d c s s n, = 1, + c i j i j i 1, k k, j 2 c,1 c k = 1 (6.23) As an exaple, (6.24)-(6.25) show the dierent steps that lead to the M atrix or the exaple presented in (6.20). As shown in Table 4.1, the pose equation associated with this contact possesses our independent Taylor coeicients, excluding the zero-order 141

155 coeicient. These our coeicients are used to obtain the T atrix o (6.23) by coputing the total dierentials o each Taylor coeicient: dθ dθ θ dx = x d y y dθθ dθ θθ T (6.24) As a inal step, the iterative schee o (6.23) is used to obtain the M atrix shown in (6.25). 0 dθ dx dy 0 θ d θ d x d y dθ x = d θ dθ d x d y 3d θdθ y d θ 6d θdθ d x d y 4d θdθ + 3d θ dθ θθ M (6.25) The eects o the sensor signals on the Taylor coeicients can be analyzed by investigating the nullspace o M. It is iportant to notice that when a sensor path corresponds to an active contact state then the deterinant o M goes to zero since the Taylor coeicients cannot be all equal to zero. Thereore, ψ, the diension o the nullspace associated with M, ust be greater than or equal to one. As a result, n c ψ o the Taylor coeicients can be estiated as a unction o the reaining coeicients (i.e., ree paraeters o the nullspace). A unique solution or the Taylor coeicients can be coputed i there exists ψ reaining independent constraints on the Taylor coeicients 142

156 (i.e., equations that were not used to construct g in (6.19) or M in (6.22)). Note that this nuber o reaining equations, deined as v r, is ixed and depends only on the structure o the pose equation, whereas the diensionality o the nullspace, ψ, depends on the value o the sensor easureents. The relationship between v r and ψ will be investigated in the next section when addressing the proble o data excitation. Assuing that there exist ψ reaining independent constraints on the Taylor coeicients (i.e., vr s can be ound: = ψ ), then a nonlinear apping Φ between the nullspace o M and ( ker( )) =Φ M (6.26) s The nullspace can be coputed analytically using the echelon or o the atrix M. As a result, each coponent o the nullspace can be written as a nonlinear unction o the sensor variables and their dierentials: ker( M ) =Γ ( ds n ) (6.27) As an exaple, the one-diensional nullspace o (6.25) is cobined with the reaining equation o (6.12) (i.e., p p2 1 + = ) to obtain all the Taylor coeicients o (6.20) in (6.28). The choice o the ree variable θθ is arbitrary but does not change the end result. 143

157 ker( M ) = N θ N 1 x = N2 θθ y 3 N p + p = 1 + = 1 =± N + N ( ) x y θθ 1 3 (6.28) As a inal step, the contact state paraeters o (6.20) are written in ters o the nullspace coponents o M as shown in (6.29). The nullspace coponents are coputed analytically using the row echelon or o M, as shown in (6.30). Each represents the 3 3 inors that corresponds to the ( i, j ) th eleent o M, as shown in (6.31). Cobining equations (6.29)-(6.31) result in the closed-or solution o the contact state paraeters. ij p p p p p ( N1 + N3 ) 2 2 ( N1 + N3 ) ( NN N ) cosθ ( NN + N ) ( N1 + N3 ) ( NN + N ) cosθ + ( NN N ) ( 1 Nx Ny) ( N1 + N3 ) = = = = = N N ( N1 + N3 ) sinθ sinθ (6.29) 144

158 M echelon N = N2 = N det( M ) 44 0 (6.30) ( ) θ ( ) = 3d θdθ dxd y d xdy d dxd y d xdy = 3d yd θdθ dθ d y 3d θ dydθ + dydθ d θ dydθ = 3d xd θdθ dθd x 3d θdxdθ + dxdθ d θ dxdθ 44 = dθ d xd y d xd y d θ dxd y d xdy + d θ dθ dxd y d xdy ( ) ( ) ( )( ) (6.31) Equation (6.32) suarizes the our steps that are needed to characterize explicitly the paraeters associated with a contact equation. First, the odel ust be identiiable, and the inverse ap 1 g between the paraeters and the Taylor coeicients needs to be characterized. Second, ultiple dierentiations are used to obtain a linear ap M between the Taylor coeicients and the derivatives o the contact equation. As a third step, the Taylor coeicients are expressed using the nullspace o M. Finally, the nullspace is coputed using the inors o M which are nonlinear unctions o the dierentials o the sensor variables. 145

159 (, ( )) 1 p= g s s t n n M ( d s) s = d = 0 s =Φ( ker( M) ) n ker( M) =Γ( d s) (6.32) Estiator Ipleentation As a result o (6.32), the paraeters are an explicit unction o the sensor signals and their dierentials. The nuber o sensor saples needed or the estiation depends on the axiu nuber o sensor signal dierentiations and the nuerical schee used to copute these dierentials. For exaple, the coputation o the third order sensor dierentials in (6.31) (e.g., d 3 θ, d 3 x, d 3 y) require ive saples when using a secondorder central dierence schee. Note that ore saples are required i higher order central dierence schees are used. Theoretically, this ethod is interesting since it allows or ast coputations without the need or initial conditions. Unortunately, this technique cannot be used in practice since it involves dierentiation o potentially noisy sensor signals. To illustrate this point, an iplicit estiation and an explicit estiation are conducted or a siulated path noise-ree path corresponding to a vertex-to-edge contact state odeled in (6.12), as shown in Fig The iplicit estiation is perored over a sliding window o length 10 using the initial conditions described in (6.34). The explicit solution is coputed using (6.29)-(6.31). Both estiations converge to the true values (6.33), as shown in Fig The iplicit estiation was coputed in 24 seconds whereas the explicit estiation 146

160 was coputed in less than 0.3 seconds on a Pentiu III running at 800 MHz. Figure 6.7 shows the estiations result when the siulated path o Fig.6.4 is corrupted by a norally distributed noise o ean and covariance described in (6.35). It can be seen that the explicit estiation is extreely sensitive to the noise, whereas the iplicit estiation still converges to the true values. T [ ] [ ] p1 p2 p3 p4 p 5 = (6.33) T [ ] ini ini ini ini ini p p p p p = (6.34) X ~ N( x,0.5) Y ~ N( y,0.5) ~ N ( θ,0.5) T Θ (6.35) T Figure 6.5: Sensor path associated with a vertex-edge contact. 147

161 P 1 (rad) P 2 (rad) P 3 () Saples Saples Saples P 4 () P 5 () True value Explicit estiate Iplicit estiate Saples Saples Figure 6.6: Noise-ree explicit and iplicit paraeter estiations. P 1 (rad) P 2 (rad) P 3 () Saples Saples Saples P 4 () P 5 () True value Explicit estiate Iplicit estiate Saples Saples Figure 6.7: Noisy explicit and iplicit paraeter estiations. 148

162 Even though the explicit estiation schee is not practical, the closed-or solution can give useul insights on the type o exciting paths that can be used or estiating the contact state paraeters Excitation Condition Assuing that a odel is identiiable, a closed-or solution or the paraeters can be ound i the Taylor coeicients can be written as unctions o the nullspace o M, as expressed in (6.26). As ention is section 6.2.1, this is possible i the nuber constraints on the Taylor coeicients satisies the ollowing inequality: * β o ( ( )) di Ker M * β (6.36) * The quantity β represents the nuber o constraints on the paraeters (e.g., p p2 = 1 in (6.12)) that were not already utilized to test or identiiability (i.e., equations not used to construct g in (6.19)). The diension o the nullspace depends only on the sensor path, whereas * β is a ixed quantity which depends only on the structure o the contact odel. In addition, the diension o the nullspace ust be greater that one when the contact is satisied since the Taylor coeicients cannot all be equal to zero. As a result, an excitation condition requires sensor paths that can satisy: * ( ( )) 1 di Ker M β (6.37) 149

163 Many sensor paths can solve this proble; a practical solution is to choose a path that is siple to ipleent (e.g., low order derivative path). As an exaple, the vertex-edge contact illustrated in Fig. 6.4(a) and odeled in (6.12) is considered. The inequality * (6.37) is satisied i the nullspace o M is less or equal to one since β is equal to one or this contact (i.e., * β = β ro Table 4.1). As a consequence, the sensor path ust ensure that the nullspace is o diension one. Analyzing the atrix M in (6.25), it can be seen that several paths can satisy this criteria (e.g., paths such that d 2 θ, dx, and dy are nonzero in (6.25)). It is also iportant to notice that even when (6.37) is satisied, zero nullspace coponents can result in non-estiable paraeters in the inal closed-or solution. For exaple, i a path is chosen such that N 2 is zero but N 1 and N 3 are nonzero in (6.29), then the paraeter p 1 cannot be estiated even though (6.37) is satisied. As a consequence, an additional condition to obtain the ull estiation o the paraeters is to choose a path that produces non-zero nullspace coponents. The two excitations conditions associated with a ull explicit estiation can be suarized as ollows: Sensor paths ust be chosen such that (6.37) is satisied. Sensor paths ust be chosen such that all the coponents o the nullspace o M are nonzero. 150

164 These conditions can then be used as a design tool to create an input sensor path that will produce an explicit solution or all the paraeters. A partial solution can be ound, however, i either one o these two conditions is not satisied. For exaple, it can be shown that a pure rotation (e.g., Fig. 6.1(b)) or the vertex-to-edge contact state odeled in (6.12) results in a nullspace o diension two that does not satisy (6.37); nevertheless, the paraeters p 3 and p 4 can still be explicitly estiated in (6.12). The proo is provided in Appendix B. The explicit ethod provides an interesting basis to analyze the eect o sensor excitation on paraeter estiations; however, additional work is needed 1) to show that the sensor paths designed or the explicit ethod are also valid or the iplicit ethod, 2) to show the suicient and/or necessary nature o the two excitation conditions presented in this section and 3) to extend the technique to ore coplex contact states (e.g., spatial contact states). 6.3 Suary This chapter investigated the ipleentation o an estiation algorith or the ultiple odel estiation step o the contact state estiator. As a result, a sliding nonlinear least squares ethod using the Levenberg-Marquardt algorith was chosen due to its robustness to poorly known noinal paraeters. An explicit ethod was also proposed or pose equations. This ethod, based on ultiple dierentiations o the contact odel, was shown to be ast but too sensitive to noise to be practically ipleented. The analytical nature o the solution, however, provided a good oundation 151

165 to investigate the eect o sensor excitation on paraeter estiation. In particular, two excitation conditions were derived and used as design tools to create sensor inputs that could ensure the estiation o all the paraeters associated with a given contact odel. The next chapter discusses the entire ipleentation o a contact state estiator. First, t the ipleentation o a decision test that cobines the residuals o the ultiple odel estiation algorith developed in this chapter with the task encoding discussed in Chapter 5 (i.e., probability transition atrix) is presented. Then, the estiator is ipleented and tested or a 3D peg-in-hole insertion task. 152

166 Chapter 7 Contact State Estiation Using a Hidden Markov Model A inal and essential aspect o contact state estiation is the ipleentation o a decision algorith to resolve which contact state is active and which set o property estiates is valid. The goal o this chapter is to provide a decision test based on the estiation inoration coing ro the ultiple estiation algorith described in Chapter 6 and the prior inoration coing ro the task knowledge described in Chapter 5, as illustrated in Fig The approach eployed here uses a Hidden Markov Model (HMM) to cobine the residuals, representing how well the contact odels it the sensor data, with the probability o speciic contact state transitions ebodied in a task contact state network. The HMM presents a convenient statistical raework in which all the inoration regarding the task is converted into probabilities. Using this raework, a 153

167 Viterbi search algorith is used to estiate the ost likely path inside the task network corresponding to the given easureents. The irst part o this chapter reviews the HMM raework, and the second part presents an application in which two HMMs are successully utilized to segent the our contact states used to describe a peg-in-hole insertion task. 7.1 The Hidden Markov Model Fraework An HMM can be described as a probabilistic observer by which a stochastic hidden process can be observed using the probabilistic structure o the task state network and a probabilistic relationship between the states and one or several observable stochastic signals. This section reviews the principal coponents that constitute an HMM and describes how they can be used to estiate contact states HMM Structure The contact state network o an HMM is described by n, the nuber o states, ρ, the n -vector o initial state probabilities in (7.1), and A, the n n state transition probability atrix deined in (5.1) and (5.2). [ ] ρ = ρ ρ ρ with ρ = ( ) 1 i= 1 ρ = P c = C 1 i n i i n T n i (7.1) The probabilistic relationship between an observable signal sequence O, o length T, 154

168 and the dierent states that coprise the task network is given by the probability density unction ( ) B O deined as ollows: i t ( ) ( / ) B O = P O c = C (7.2) i t t t i As a result, all the inoration relevant to the task is contained in the three probability easures: A, B, and ρ. Using Rabiner s notation (Rabiner 1989), HMM s are expressed in a copact or as: ( AB,, ρ ) λ = (7.3) In order or an HMM to be atheatically practical, the ollowing assuptions are ade: Markov assuption: the current state only depends on the previous state. Stationarity assuption: the state transition probabilities are independent o the tie at which the transitions occur. Statistical independence o the observations: the current observation is statistically independent o the previous observation HMM Based Segentation The HMM representation is appropriate to solve segentation probles (e.g., speech recognition). In the HMM context, the segentation proble can be posed as a decoding proble (i.e., uncovering the hidden states o the odel). The objective is to ind the 155

169 best sequence o states that axiizes the probability PQOλ ( /, ) (i.e., the probability o having the state sequence Q given the observations O and the odel λ ). The probability o axiizing PQOλ ( /, ) with respect to Q is equivalent to axiizing PQOλ (, / ) with respect to Q as shown in (7.4). ( /, λ ) P Q O (, / λ ) ( / λ ) P Q O = (7.4) P O The joint conditional probability PQOλ (, / ) can be expressed as in Rabiner (1989): ( ) ( ) ( ) P Q, O/ λ = P O/ Q, λ P Q/ λ = ρ B ( O ) a B ( O ) a a B ( O ) (7.5) c c c c c c c c c c T where the c s sybolize the states at tie t i (i.e., Q= cc 1 2c3 ct ) T T T Maxiizing (7.5) with respect to Q requires the coputation o (7.5) or all the possible state sequences. This nuber is exponential in the nuber o observations. For exaple, a proble with only three observation saples and two states has a axiu o eight state sequences (i.e., ). I the nuber o observations increases to 100, then possible state sequences ust be coputed. To solve this optiization proble eiciently, a dynaic prograing technique known as the Viterbi algorith is eployed (Viterbi 1967). 156

170 The Viterbi Algorith The Viterbi algorith is a technique that reduces the optiization o a global proble to a sequential optiization o sipler sub-probles. This technique can be applied to the optiization o (7.5) since the proble can be broken into a sequence o optial subprobles. In eect, it can be noted that or each observation, the optial path ending in each state can also be part o the inal optial path obtained or the ull observation sequence. As an exaple, a three-state Viterbi optiization is illustrated in Fig There exists nine possible state sequences between tie t 1 and t 2. The Viterbi algorith coputes the probability (7.5) or each sequence and then keeps the ost likely path ending in each state. This result is then used by induction to copute the ost likely sequences between ties t 2 and t 3. As a result, the overall optiization proble is broken into T optial sub-probles. As a inal step, the state corresponding to the last observation is uncovered and then used to back estiate the coplete state sequence axiizing (7.5). The Viterbi algorith reduces to 2 nt the nuber o coputations o the joint probability (7.5), aking the algorith a practical solution or probles with a liited nuber o states. 157

171 t= T... Tie δ δ δ = ρ B ( O ) 1, = ρ B ( O ) 2, = ρ B ( O ) 3, State t= T... Tie δ δ1,1a1 i = Max δ a B ( O ) δ3,1a3i i,2 2,1 2i i 2 State t= T... Tie δ δ1,2a1 i = Max δ a B ( O ) δ3,2a3i i,3 2,2 2i i 3... t=t State State T-1 T... Tie End δ Max δ δ 1, T = 2, T 3, T Final step: Backtracking T-1 T... Tie Optial state sequence State Figure 7.1: Ipleentation scheatic o the Viterbi algorith. 158

172 7.1.3 HMM Ipleentation The ipleentation o an HMM requires the coputation o the probabilistic quantities ρ, A and B introduced in (7.3). Two ethods are coonly used in the literature: unsupervised training and supervised training. In unsupervised training, a learning algorith is used to extract the three probability easures ro unlabeled data sets. Several learning algoriths, such as the Bau-Welch ethod (Bau and Petrie 1966), are requently used with HMMs. Unsupervised training ethods are well-suited or applications possessing a large nuber o states (e.g., speech recognition). When the nuber o states is liited, supervised training techniques using labeled data can be ore practical. These ethods oer ore control to the user and allows the decoupling o the learning proble: the task knowledge (i.e., ρ and A ) can be set using one ethod, and the observation knowledge (i.e., B ) can be set using another one, as described below. Task knowledge: The initial state probability vector ρ is generally known or a anipulation task since it is oten assued that the task starts in a non-contact state (i.e., ree otion). As or the probability transition atrix, it can be obtained using the techniques described in 5.3. The eleents o the atrix are coputed by counting the nuber o occurrences o each transition given a large nuber o contact state sequences (i.e., large k in (5.7)). 159

173 Observation knowledge: The observation probability distribution, B, represents the probabilistic ap between the observations and the states. Several choices o observations can be used to characterize the states (e.g., raw signals, processed signals). The observations that provide the ost direct ap to the contact states should be chosen. In the context o paraeter-based contact state estiation, raw sensor signals (e.g., position, orientation, orce and torque easureents) can be used to characterize the contact states; however, estiation residuals are better suited since a direct correspondence between a contact being active and the size o the residual exists. In eect, or each contact state, the residual o the estiation process is a scalar which depends only on sensor inputs st () and the estiated tie-independent paraeters ˆp. As shown in equation (7.6), the residual corresponds to the su o squares o the estiated nonlinear contact unction ( pst ˆ, ( )) coputed inside an estiation window o length δ. ε p δ t= 1 ( pst ˆ, ( )) 2 = (7.6) The objective o the estiation phase is to ind the paraeter estiates that iniize (7.6). Thus, the agnitude o the residual is an indication o the goodness o the it, assuing a well-conditioned data window (i.e., rich excitation). As a result, contact state odels associated with sall residuals are ore likely to correspond to the actual state than odels associated with large residuals. In the HMM literature, the observation probability distribution is coonly ap- 160

174 proxiated with ixtures o ultivariate Gaussians (Rabiner 1989) as shown in (7.7). In this context, the distribution is characterized by its ean and covariance atrix. Since the residuals are nonlinear unctions, these statistics can be diicult to obtain. However, they can be approxiated analytically using irst-order Taylor series, or they can be extracted using Monte Carlo siulations (Breipohl 1970). s 1 B O = O U O i n t T J ij T 1 i( t) exp ( t i) i ( t i), 1 ;1 k j= 1 (2 π ) U 2 i J j= 1 s ij = 1 (7.7) where, k represents the axiu nuber o coponents o the signal O. Also, U i are a k 1 vector o eans and a k k covariance atrix, respectively. i and 7.2 Experiental Contact State Estiation In this section, the ipleentation o the contact state estiator illustrated in Fig. 6.1 is utilized during a anipulation task. As an exaple o a coon assebly task, a pegin-hole insertion is considered, as shown in Fig The expected sequence during the task can be reduced to our steps. First, the peg is slid towards the hole on the planar surace (Contact 2). As the peg enters the hole, it pivots on the ri o the hole (Contact 3) and typically aintains this contact until the other side o the hole is reached. It then stays in double contact with the ri and the inside o the hole (Contacts 3 and 4) until the peg is inserted ar enough that the task can be easily copleted. 161

175 No Contact (C 1 ) Contact 2 (C 2 ) Contact 3 (C 3 ) Contact 3,4 (C 4 ) Figure 7.2: Possible sequence o contact states during peg-in-hole insertion The goal o the experient is to ipleent an HMM to estiate the sequence o contact states coposing the task. The HMM presented in this experient is based on an epirical selection o the HMM variables. A possible iproveent o the approach is discussed at the end o the section Syste Coniguration A PHANToM haptic device, as shown in Fig. 7.3, is used as the anipulating robot. The positions o the syste s six joints are easured using high-resolution optical encoders. The kineatics o the robot are known, and a closed loop calibration technique (Hollerbach and Wapler 1996) is used to iprove the absolute accuracy o the syste. A cylindrical peg is attached directly to the tip o the anipulator robot. The hole is drilled perpendicular to the surace o a rectangular aluinu block that is ounted on a three degree o reedo vise (roll-tilt-pan). The insertion is perored anually, using the anipulating robot to record the kineatic data. 162

176 Figure 7.3: Experiental apparatus - PHANToM 1.5, cylindrical peg and orientable hole Contact State Equations Pose equations are utilized to describe the contact states associated with the task. The technique developed in section is used to represent the kineatic constraints associated with the contact state. Additional nonlinear constraints are needed to represent the position o the contact point on the peg s ri and hole s ri. Equation (7.8) corresponds to the pose equation associated with Contact 2. Equations or Contact 3 and Contact 4 takes siilar or and can be ound in Debus et al. (2004). ε p2 = K 2+q 14sin β2+q24cos β2sin β1 -q 34cos β1cos β2+ + L -q 33 cos cos +q 23 sin cos +q13 sin p β1 β2 β1 β2 β2 + R cos - q 31 cos cos + q21 sin cos - q11 sin p α2 β1 β2 β1 β2 β2 + R sin - q 32 cos cos + q22 sin cos - q12 sin p α2 β1 β2 β1 β2 β2 where K 2= H zcos β1cos β2- Hxsin β2 - Hycos β2sin β1 (7.8) The bolded variables in this equation are the identiiable unknown paraeters which 163