USING SIMULATION FOR PLANNING AND DESIGN OF ROBOTIC SYSTEMS WITH INTERMITTENT CONTACT By Stephen George Berard A Thesis Submitted to the Graduate Faculty of Rensselaer Polytechnic Institute in Partial Fulfillment of the Requirements for the Degree of DOCTOR OF PHILOSOPHY Major Subject: COMPUTER SCIENCE Approved by the Examining Committee: Jeffrey Trinkle, Thesis Adviser Kurt Anderson, Member John Mitchell, Member Barbara Cutler, Member Rensselaer Polytechnic Institute Troy, New York April 2009 (For Graduation May 2009) c Copyright 2009 by Stephen George Berard All Rights Reserved ii CONTENTS LIST OF TABLES . vii LIST OF FIGURES . viii ACKNOWLEDGMENT . xiii ABSTRACT . xiv 1. Introduction . 1 1.1 Contributions . 2 2. Background . 6 2.1 Rigid Body Kinematics . 6 2.1.1 Position and Orientation . 6 2.1.2 Rotation Group . 8 2.1.3 Euclidean Group . 9 2.1.4 Velocity of a Rigid Body . 11 2.1.4.1 Kinematic Update . 12 2.1.4.2 Velocity of a Point on a Rigid Body . 13 2.1.4.3 Twists . 14 2.2 Kinetics . 15 2.2.1 Wrenches . 16 2.2.2 Transforming Twists and Wrenches . 17 2.2.3 Equations of Motion . 17 2.3 Complementarity Problem . 18 2.3.1 Finding Solutions of Complementarity Problems . 19 2.4 Complementarity formulation of Dynamics . 21 2.4.1 Equality Constraints . 23 2.4.1.1 Bilaterally Constrained Dynamics Formulation . 24 2.4.2 Contact . 25 2.4.2.1 Rigid Contact . 25 2.4.3 Instantaneous Formulation of Constrained Dynamics . 32 2.4.3.1 Nonlinear DCP Formulation . 32 2.4.3.2 Linear DCP Formulation . 33 2.4.4 Visco-Elastic Contacts . 33 iii 2.5 Solving Constrained Equations of Motion . 35 2.5.1 Stabilization Methods . 36 2.5.1.1 Projection Methods . 38 2.5.1.2 State-Space Methods . 39 2.5.2 Unilateral Constraints . 39 2.6 Time-Stepping Formulation of Rigid Body Dynamics . 40 2.6.1 Nonlinear Complementarity Problem Formulation . 41 2.6.2 Linear Complementarity Problem Formulation . 42 2.7 Examples . 42 2.7.1 Planar 2 Bar Pendulum . 43 2.7.2 Planar 2 Bar Pendulum Attached to Block . 46 2.7.2.1 System Dynamics . 48 2.7.3 Planar 2 Bar Pendulum Attached to Block with Friction . 49 2.7.3.1 System Dynamics . 49 2.7.4 Planar 2 Bar Pendulum Attached to Block with Friction and Position Controlled Body . 51 2.7.4.1 System Dynamics . 52 2.7.5 Planar 2 Bar Pendulum Attached to Block with Friction, Po- sition Controlled Body, and Moving Floor . 54 2.7.6 Full Planar Model . 54 3. Alternative Time-Stepping Formulations . 60 3.1 Existing Alternative Complementarity Formulations of Dynamics . 60 3.2 Modeling Nonconvex NonPenetration Constraints . 62 3.2.1 Non-Penetration Constraint . 62 3.2.2 Future Work . 66 3.3 2.5D Model . 66 3.3.1 Surface Friction Normal Force . 67 3.3.2 Nonlinear DCP Formulation . 68 3.3.3 Linear DCP Formulation . 69 3.4 Quasistatic Formulation . 70 3.4.1 Instantaneous-time models . 70 3.4.2 Discrete-time models . 71 3.4.3 A mildly nonlinear model: Model-DQC . 71 3.4.4 A linear model: Model-DLC . 72 iv 3.4.5 Uniqueness . 73 3.4.6 Example: Fence-Particle Problem . 75 3.4.6.1 Results . 78 3.5 Geometrically Implicit Formulation . 80 3.5.1 Contact Constraint for Rigid Bodies . 82 3.5.1.1 Objects described by a single convex function . 82 3.5.1.2 New Discrete Time Model . 85 3.5.1.3 Objects described by intersections of convex functions 86 3.5.2 Contact Constraints for Compliant Bodies . 88 3.5.2.1 Objects described by a single convex function . 89 3.5.2.2 Objects described by intersections of convex functions 92 3.5.3 Illustrative Examples . 93 3.5.3.1 Example 1: Disc on a Plane . 93 3.5.3.2 Example 2: Sphere on Two Spheres . 95 3.5.3.3 Example 3: Disc falling on a compliant half-plane . 96 3.5.3.4 Example 4: Probabilistic Grasp Planning . 100 3.6 Summary . 103 4. daVinci Code . 104 4.1 Simulation Overview . 105 4.1.1 Bodies . 106 4.2 Plugin Overview . 107 4.2.1 Collision Detection . 107 4.2.2 Time-steppers . 107 4.2.3 Complementarity Problem Solver . 108 4.3 Simulated and Experimental Results . 108 4.3.1 Design of a Part Reorienting Device . 108 4.3.2 Needle pushing a planar slider . 111 4.3.3 Probabilistic Grasp Planning . 113 4.4 Summary . 116 5. Sources of Error in a Simulation of Rigid Bodies . 117 5.1 Dynamic Model . 118 5.1.1 Instantaneous Dynamics . 119 5.1.2 Discrete Time Dynamics . 121 5.2 Results . 122 v 5.2.1 Simulation Verification . 122 5.2.1.1 Analytical Results . 122 5.2.1.2 Qualitative Results . 125 5.2.2 Trajectory Error as a function of step size . 125 5.2.3 Trajectory Error as a function of Friction Cone linearization . 127 5.2.4 Solution Time of Problem . 130 5.3 Computation of Plate's Orientation . 131 5.4 Summary . 133 6. Planning New Motions . 135 6.1 Results . 136 6.1.1 Circle Motion . 137 6.1.2 Saddle Motion . 140 6.2 Summary . 141 7. An MPEC Formulation of Nonrecursive Filtering . 143 7.1 Model . 143 7.2 Filtering approaches . 144 7.3 Non-Recursive Filters . 144 7.4 MPEC Formulation . 145 7.5 Problems with Filtering Approaches . 146 7.6 Examples . 147 7.6.1 Simulation Example . 147 7.6.1.1 Single Particle . 147 7.6.1.2 Multiple Particles . 150 7.6.2 Sliding Block Example . 150 7.7 Summary . 156 8. Discussion and Conclusions . 157 REFERENCES . 159 vi LIST OF TABLES 4.1 Hierarchical approach to design . 110 7.1 Results of the estimation problem for a single particle. The correct value of µ is 0.2. 150 7.2 Results of the estimation problem with multiple particles. \µ Error" is the root mean squared error. 150 vii LIST OF FIGURES 1.1 The four new time-stepping formulations are displayed along with the existing Stewart-Trinkle time-stepper. The y axis is the model space, and the x axis is the time-stepper accuracy. The new methods are not mutually exclusive, for example it is possible to combine the 2.5D, non-convex, and quasi-static steppers together. 4 2.1 Position and orientation of a rigid body specified by a body fixed ref- erence frame fAg with origin at the center of mass of body A, denoted A∗. ...................................... 7 2.2 Displacement of body A to body B..................... 10 2.3 Velocity of a point p on a rigid body. 13 2.4 Forces and moments acting on a rigid body. 16 2.5 Coordinate system for contact constraints . ..