Solving Inverse Kinematics Constraint Problems for HighlyArticulated ModelsbyKangTeresaGeA thesispresented to the University of Waterlooin fullment of thethesis requirement for the degree ofMaster of MathematicsinComputer ScienceTechnical Report CS-2000-19Waterloo, Ontario, Canada, 2000c Kang Teresa Ge 2000I hereby declare that I am the sole author of this thesis.I authorize the University of Waterloo to lend this thesis to other institutions or individualsfor the purpose of scholarly research.I further authorize the University of Waterloo to reproduce this thesis by photocopying or byother means, in total or in part, at the request of other institutions or individuals for the purposeof scholarly research.iiThe University of Waterloo requires the signatures of all persons using or photocopying thisthesis. Please sign below, and give address and date.iiiAbstractGivenagoalpositionfortheendeectorofahighlyarticulatedmodel,thetaskofndingtheangles for each joint in the model to achieve the goal is an inverse kinematics problem. Redundancyof the degrees of freedom (DOF) can be used to meet secondary tasks such as obstacle avoidance.Joint limit constraints and collision detection can also be considered, as can loops.Solutionstoredundantinversekinematicproblemsarewell developed. ThemostcommontechniqueistodierentiatethenonlinearequationsandsolvealinearJacobianmatrixsystem.The pseudoinverse of the Jacobian can be calculated via a singular value decomposition (SVD).ThegeneralSVDalgorithmreducesagivenmatrixrsttoabidiagonalformthendiagonalizesit. The iterative Givens rotations method can also be used in our case, since the new Jacobian isa perturbation of previous one. Secondary tasks can be easily added to this matrix system, butit is nontrivial to generalize this problem to a highly redundant model in a complex environmentin the computer graphics context.Forthisthesis, Iimplementedandcomparedseveral SVDtechniques; andfoundthatbothgeneral and iterative algorithms are of linear time-complexity when solving a three-row matrix. Ialso programmed and compared dierent methods to avoid one or multiple obstacles. A completesystem was built to test the speed and robustness of the implementation.ivAcknowledgementsIwouldliketothankmysupervisors, StephenMannandMichael McCool, fortheirguidancesthroughout mycourses andresearch, their brilliant suggestionthat helpedmeout whenmyresearchisstuck,andtheirpatienceinreadingandcorrectingthedraftsofthisthesis. SpecialthankstoJohnMcPheeforhisinterestonmyresearch, hisexcellentteachingskill, aswell ashis help and concern all through my research. I would also like to thank both my readers, JohnMcPhee and Peter Forsyth, for taking time to read my thesis.I am grateful to my family for their never ending love and support. I would also like to thankmy friends in both Canada and China for their friendship.Finally, I would like to thank NSERC to make this research possible.vTrademarksSGIandOpenGLareregisteredtrademarksofsgi. Allotherproductsmentionedinthisthesisare trademarks of their respective companies.viContents1 Introduction 11.1 Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.2 Forward Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.3 Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51.3.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51.3.2 Redundancy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61.4 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.5 Discussion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72 InverseKinematicsSolvers 92.1 Analytic Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.2 Numerical Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.2.1 Matrix Pseudoinverse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.2.2 Matrix Transpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.3 Lagrangian Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152.4 Reach Hierarchy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162.5 Constrained Inverse Kinematic Problems . . . . . . . . . . . . . . . . . . . . . . . . 182.5.1 The Lagrange Approach. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.5.2 Introducing New Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.5.3 Penalty Function Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20vii3 SingularValueDecomposition 223.1 Pseudoinverse and Singular Value Decomposition. . . . . . . . . . . . . . . . . . . 223.2 Damped Least-Squares Method. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.3 Golub-Reinsch (LAPACK) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313.4 Column-wise Givens Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 313.5 Row-wise Givens Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 353.6 Performance Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354 ObstacleAvoidance 374.1 Avoiding One Obstacle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384.1.1 Task Priority Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 384.1.2 Cost Function Approach. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434.1.3 Objective Function Approach. . . . . . . . . . . . . . . . . . . . . . . . . . 434.2 Avoiding Multiple Obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 475 Results 485.1 Joint Limits Methods Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . . 505.2 SVD Techniques Comparison . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 515.3 Avoiding Multiple Obstacles Approaches . . . . . . . . . . . . . . . . . . . . . . . . 535.3.1 Task Priority Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 535.3.2 Cost Function Approach. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 545.3.3 Objective Function Approach. . . . . . . . . . . . . . . . . . . . . . . . . . 555.3.4 Other Approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 555.4 Obstacle Avoidance Techniques Comparison. . . . . . . . . . . . . . . . . . . . . . 585.5 Discussion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 605.5.1 Objective Function Method . . . . . . . . . . . . . . . . . . . . . . . . . . . 616 Conclusion 626.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 626.2 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63viii6.3 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64AApplication 65A.1 Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65A.2 Editable Tree Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67A.3 Collision Detection. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67A.4 Obstacle Geometry. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69A.4.1 Spheres . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69A.4.2 Planes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70A.4.3 Cylinders . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70A.5 Direct Manipulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72A.5.1 Identifying the Chain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72A.5.2 Determining an End Eector Goal . . . . . . . . . . . . . . . . . . . . . . . 73A.5.3 Menu Items . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74Bibliography 76ixListofTables5.1 The comparison of the three one-obstacle avoidance techniques. . . . . . . . . . . . 585.2 The comparison of multiple obstacles avoidance techniques. . . . . . . . . . . . . . 59xListofFigures2.1 A three-segment planar chain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.2 A physical interpretation of the columns of the Jacobian matrix . . . . . . . . . . . 132.3 Workspace of a three-segment planar chain . . . . . . . . . . . . . . . . . . . . . . 172.4 Basic(a) and dual(b) joint adjustment problems. . . . . . . . . . . . . . . . . . . . 183.1 A geometric interpretation of the singular value decomposition . . . . . . . . . . . 243.2 The transformation of uncertainty . . . . . . . . . . . . . . . . . . . . . . . . . . . 263.3 An example of an ill-conditioned Jacobian. . . . . . . . . . . . . . . . . . . . . . . 273.4 A comparison of the damped least-squares solution to least-squares solution. . . . 304.1 Primary and secondary goal of a redundant manipulator. . . . . . . . . . . . . . . 394.2 The form of the homogeneous term gain and the obstacle avoidance gain . . . . . . 414.3 Using objective function method to avoid an obstacle . . . . . . . . . . . . . . . . . 445.1 A snapshot of the application . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 495.2 Two joint limits methods: projection and clipping. . . . . . . . . . . . . . . . . . . 495.3 The time performance comparison of all the SVD techniques discussed. . . . . . . . 525.4 The time performance comparisonof LAPACKandrow-wise Givens rotationsmethod. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 525.5 The elephant trunk may be in a kinked form . . . . . . . . . . . . . . . . . . . . 54A.1 DAG of the elephant skeleton structure . . . . . . . . . . . . . . . . . . . . . . . . 66xiA.2 Finding distance between a point to a plane. . . . . . . . . . . . . . . . . . . . . . 70A.3 Finding distance between a point to a cylinder. . . . . . . . . . . . . . . . . . . . . 71A.4 Finding distance between a line segment to a sphere. . . . . . . . . . . . . . . . . . 71A.5 Determining an end eector goal . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74xiiChapter1IntroductionElephant trunks, tentacles, snakes, and even some desk lamps can be considered as highly artic-ulatedmultilinkstructures. Tomodelthosehighlyarticulatedstructuresandmanipulatetheminteractively using computer techniques is challenging.Researchonarticulatedmodels is well developedintheelds of robotics andmechanicalengineering, althoughtheirconcernsaresomewhatdierentthanthoseof computergraphics.Researchers in both robotics and mechanics have their own concerns and criteria, such as dexterity,forces, etc. Most of thetime, thesephysical concerns canbeignoredincomputer graphics.Computer graphics researchers tend to want to simulate arbitrary highly linked models and controlthem in an arbitrary complex environment, while robotic scientists and mechanical engineers careonlyaboutthespecicrobotormachinethattheyareusing. Athirdmajordierenceisthatcomputer scientists prefer to present (or render) animated frames on line, while robotic scientistsandengineerswouldrathertodocomplexolinecomputationtoensurecorrectness. All ofthe above motivated my work on solving inverse kinematics (IK) constraint problems for highlyarticulated models in a computer graphics framework.Foranyarticulatedmodel, theendeectorspositionandorientationinCartesianspaceisdenedbyitsjointconguration. Givenanal position(andorientation)of theendeector,nding a suitable joint conguration is an inverse kinematic problem. In general, this is a nonlinear1CHAPTER1. INTRODUCTION 2problemandtherearenoclosed-formsolutionsforitincrementally. Butbydierentiatingthenonlinear equations with respect to time, we can get a linear equation and use the pseudoinverseof the Jacobian to solve it. The joint conguration of an articulated model is specied by a treestructure. Each node in the tree represents a rigid body or a joint. Each joint has its joint limitconstraints and they are handled by two dierent methods in my work. Collision detection amongbody parts or between the model and its environment, and joint loops, are also considered in mywork.The pseudoinverse can be calculated via a singular value decomposition (SVD). This methodcandetectsingularitiesofamatrix. Toavoidthediscontinuitiescausedbysingularitiesandtorobustly calculate the pseudoinverse, I used the damped least square method [22].LAPACK implements an optimized version of the general SVD method developed by Goluband Reinsch [10]. This method can be used to compute the SVD for any matrix. Maciejewski [24]incrementallyusesGivensrotationstocalculatetheSVDforroboticjointcongurations. Heuses previous joint conguration information to speed up the algorithm. To suit the needs of myapplication, wherethree-rowmatricesaremainlyconsidered, IdevelopedasimilarincrementalmethodbasedonGivensrotationsbysimplycomputingtheSVDof thetransposedJacobianmatrix. The time spent on inverse kinematic computation for both my method and the algorithmimplemented by LAPACK are linear functions of the number of degrees of freedom, and they runat almost the same speed.To specify position (and orientation) of an end eector only takes 3 (or 6) degrees of freedom.When a linked model has more than 3 (or 6) joints (degrees of freedom) we call it a redundantsystem. The redundancy can be used to accomplish secondary tasks such as obstacle avoidance.The task priority technique,the cost function method,and the objective function approach areimplemented in this work to avoid obstacles. Multiple obstacles can be considered either simul-taneously using task space augmentation or separately as weighted multiple tasks.I ran several experiments to test the accuracy of the SVD algorithms and the obstacle avoidancealgorithms. Empirical performance analysis was also done.CHAPTER1. INTRODUCTION 3Placing this work in context requires some background knowledge about articulated kinematicmodeling. The following sections of this chapter presents data structures for representing jointedlinkedmodelsandthebasicmotioncontroltechniquesforthesemodels. Chapter2toChapter4 talks about previous research by others. Dierent inverse kinematic techniques are reviewed inChapter 2. Chapter 3 describes the pseudoinverse used in the inverse kinematic solvers, and thesingular value decomposition that nds it. Chapter 4 explains how to avoid obstacles using theredundancy of the kinematic system. Chapter 5 discusses the results I have obtained by imple-menting the previous algorithms andbyextending them. Chapter 6concludes and summarizesmy work, and gives some idea for future work.1.1 ModelingAnarticulatedobjectisoftenmodeledasasetof rigidsegmentsconnectedwithjoints. Thejoints are usually rotational (also known as revolute), or translational (also known as prismatic),withonesingledegreeof freedom(DOF). Theymayalsobeacombinationof jointsof thesetwotypes, forinstance, aspherical jointisajointwith3rotational DOFsinthex, y, andzdirections. MultipleDOFjointscanbedecomposedintokinematicallyequivalentsequencesofone DOF joints separated by zero length links. Most joints in this work are rotational, and eachof them has one rotational degree of freedom in one of three (x, y, z) directions subject to jointlimits.A minimal kinematic model is dened by its individual rigid segment lengths, the joint degreesof freedom, theirmaximumandminimumjointlimits, andatreestructuredhierarchyof thesegments and joints.Insuchahierarchy, eachnodeinthetreerepresentseitherarigidbodyorajoint. Eachsegment is associated with a coordinate system to which it is rigidly axed. Each joint maintainsthe rotations currently in eect at the corresponding joint. Joint rotations are coordinate systemtransformations relating the orientation of the parent and child segments in the tree. This hier-archical denition ensures that all segments inherit the rotations applied to joints higher in theCHAPTER1. INTRODUCTION 4tree. For instance, in a human model, a rotation applied at the shoulder joint causes the entirearmtorotate, insteadofjusttheupperarmsegment. Onexedjointorrigidsegmentinthemodel shouldbespeciedastherootofthetree. Whenatransformationisappliedtoit, theentire model is transformed.Mathematically, each joint i has a transformation matrix Mi. The matrix Mi is either a trans-lation matrixT(xi, yi, zi) or a rotation matrixR(i), both of which are relative to the coordinateframeofjointisparent. ThematrixT(xi, yi, zi)isthematrixthattranslatesbytheosetofjoint i from its parent joint i 1, and R(i) is the matrix that rotates by i about joint is rotationaxis.The position and orientation of any segment in the model is found by concatenating the trans-formations at each joint from the root (joint 1) to the last joint node (jointi) above this segmentinthetreestructure. Sincecolumnvectorsareusedinthisproject, theoveralltransformationfrom the childs coordinate system to the parents is given by the product of the matrices alongthe path from parent to child,M= MiMi1...M2M1, (1.1)whereeachMontherighthandsideisthetransformationmatrixofeachjointrelativetoitsparent coordinate system.There are two fundamental approaches to control the movement of the model: kinematics anddynamics.1.2 ForwardKinematicsForwardkinematics explicitlysets thepositionandorientationof eachsegment at aspecicframe timebyspecifying the jointangles foreachjoint. The positionofthe endeectorin theparents coordinate system is found by multiplying the position vector of the end eector in itsown coordinate system with the transformation matrixMin Equation 1.1.Normally, not every frame in an animation is dened explicitly. Only a series of keyframes aregiven such information. The rest are calculated by interpolating the joint parameters between theCHAPTER1. INTRODUCTION 5keyframes.Linearinterpolationisthesimplestmethod, buttheresultisunsatisfactory. Consideranydegreeoffreedominpositionororientationofasegment, expressedasafunctionoftimeanddenoted by x(t). The velocity and acceleration are the rst and second time derivatives. The valuex(t) should be twice dierentiable,since physically acceleration can never be innite. However,linear interpolationhas adiscontinuous rst derivativeandsointroduces jerky, non-physicalmovement.Higher order interpolation methods, such as quadratic and cubic spline methods, can providecontinuousvelocityandacceleration. Thisproducessmoothertransitionbetweenandthroughthe keyframes.1.3 InverseKinematicsForward kinematics can only indirectly control the position of each segment by specifying rotationanglesatthejointsbetweentherootandtheendeector. Thismayresultinunpredictablebehaviorduringinterpolation. Incontrast, inversekinematics providesdirectcontrol overtheplacement of theendeector bysolvingforthejoint anglesthat canplaceit at thedesiredlocation.1.3.1 ProblemStatementA kinematic chain is a linear sequence of segments connected pairwise by joints. It can also bereferredtoasamanipulatorinrobotics. Thesegmentthatneedstobemovediscalledtheend eector and it is the free (distal) end of the chain. The other end of chain is called the xed(proximal) end or base.We specify the conguration of a chain with n one-DOF joints as a vector (q1, ..., qn), or simplyq. Theposition(andorientation)oftheendeector, x, isdescribedbyitsforwardkinematicfunction:x = f(q).CHAPTER1. INTRODUCTION 6This is a non-linear function with the joint space as its domain and the task space as its range.Thejointspaceisformedbyvectorswithnelements, thejointparameters. Thetaskspaceisformedbyvectorswith3elementsif onlythepositionof theendeectorisconsidered, or6elements if the orientation is also considered. In my work, only position is considered.The goal of inverse kinematics is then to place the end eector at a specied position x, anddetermine the corresponding joint variable vector q:q = f1(x). (1.2)Solving this equation is not trivial and the result may not be unique, since f has no unique inverse.The next chapter is dedicated to dierent solutions to the inverse kinematic problem.1.3.2 RedundancyIf the number of joints in the kinematic chain is the same as the dimensionality of the task spacein which they lie (which in our case is 3), we call this system perfectly constrained. If the jointspace has a lower dimensionality, we call it an overconstrained system.Themostinterestingcaseiswhenasystemhasmoredegreesof freedomthanthenumberof constraints imposedbythegoal parameters. Wecall this underconstrainedor redundant.Thedierencebetweenthedegrees of freedomandgoal-imposedconstraints is thedegreeofredundancy. In this case, there may be innitely many qs for one particular x in Equation 1.2.Theseextradegreesof freedomcanbeusedtoimprovetheabilityof themanipulatorsinrobotics in both kinematic and dynamic contexts [29, 30, 32, 28, 15, 3, 26]. These abilities includeavoiding one obstacle [23, 9, 13, 39, 16, 11, 5, 12, 35], satisfaction of joint limits [19], singularityavoidance [27], torque optimization [8, 14], dexterity optimization [17], etc. Chapter 4 talks aboutavoidance of one obstacle. Multiple obstacle avoidance is also considered in this work and will bedeveloped more in Section 5.3.CHAPTER1. INTRODUCTION 71.4 DynamicsUnfortunately, inverse kinematics of redundant system does not consider the physical laws of therealworld. Dynamicmethodsareoftenusedinroboticandengineeringelds, sincetheymusttake gravity and external forces into account.Motion generated by dynamic methods is more realistic since physical laws are considered. Inaddition to the kinematic denition of a model, for a dynamic model segment descriptions mustinclude such physical attributes as the center of mass, the total mass, and the moments of inertia.Forward dynamicsapply explicit time-varying forces and torques to objects. Using NewtonslawF= ma, we can gure out object accelerations from the given forces and masses. Then withthepositionandvelocityknownattheprevioustimestep, wecanintegratetheaccelerationatwice to determine a new velocity and position for each object segment in the current time step.Forwarddynamicsprovidesonlyindirectcontrol of objectmovement. Itischallengingtocalculate the time varying forces needed to produce a desired motion. Plus, there will always beone equation for each degree of freedom in the model, which leads to a large system of equationsto solve.Inverse dynamic methods are able to compute the force and torque functions needed to achievea specied goal. This is an interesting topic in robotics and engineering, and it can also producerealisticphysicalmotionincomputergraphics. However, ifthepositionandorientationoftheendeectoristhemainconcern, andthemotiontrajectoryandtimingoftheendeectorareknown beforehand, we normally would not care about the physical forces.1.5 DiscussionFrom above discussion, we see that no one approach is absolutely better than the other. Dependingon the application, incorporating all the four approaches to some degree seems to be a reasonablesolution.Inthefollowingchapters, theuseofinversekinematicsintheinteractivemanipulationofahighly articulated model is explored. The goal is to generalize robotic and engineering techniquesCHAPTER1. INTRODUCTION 8intothecomputergraphicscontext. Specically, wewill belookingattechniquessuitableforreal-time animation or for use in a modeling user interface.Chapter2InverseKinematicsSolversInthischapterIreviewseveralinversekinematicssolversbyotherresearchers. Thenumericalsolutions using matrix pseudoinverse is the work of Maciejewski [22]; the numerical solutions usingmatrix transpose is the work of Chiacchio et al [6]; and the rest of the solutions are summarizedin a paper of Korein and Norman [18].2.1 AnalyticSolutionsWhenthesystemof equationsarisingfromthespecicationof agoal forachainisperfectlyconstrained,wecansometimesndananalyticsolution. Ideally, analyticsolutionsproduceallpossible satisfactory solutions.As asimple example, consider the three-segment planar chainshowninFigure 2.1(alsosee [18]). The proximal end is constrained to the origin. The segment lengths area1,a2, anda3,respectively. Theangularjointvariablesareq1, q2andq3. Forconvenience,wegiveeachjointthe same name as the joint variable, each segment the same name as its length variable, and callthe chains distal terminalq4.We can obtain the position of any jointqiby examing the projections of segments on theXand Yaxes, marked o in Figure 2.1. Let xi and yi be the x and y components of the position of9CHAPTER2. INVERSEKINEMATICSSOLVERS 10asin(q+ q+ q)3 132aaa123asin(q+ q)asin q21 211acos q1 1acos(q+ q)1 22acos(q+ q+ q)1 3 2 3Yq4q1q2q32xx3x4Xy2yy43Figure 2.1: A three-segment planar chainjointqi, we see from the gure thatx2 = a1 cos(q1)y2 = a1 sin(q1)x3 = a1 cos(q1) +a2 cos(q1 +q2) (2.1)y3 = a1 sin(q1) +a2 sin(q1 +q2) (2.2)x4 = a1 cos(q1) +a2 cos(q1 +q2) +a3 cos(q1 +q2 +q3)y4 = a1 sin(q1) +a2 sin(q1 +q2) +a3 sin(q1 +q2 +q3).We can also obtain the orientation i of segment ai by simply accumulating proximal joint angles:1 = q1CHAPTER2. INVERSEKINEMATICSSOLVERS 112 = q1 +q23 = q1 +q2 +q3. (2.3)Thesepositionandorientationequationscanbegeneralizedtoany2Dkinematicchainwitharbitrary number of segments and arbitrary segment lengths.Let thegoal betomovejoint q3(not thetipq4) toapoint (kx, ky). This imposes twoconstraints:x3 = kxy3 = ky.Combining these constraints with Equation 2.1 and Equation 2.2, we getkx = a1 cos(q1) +a2 cos(q1 +q2)ky = a1 sin(q1) +a2 sin(q1 +q2).We now have two equations in two unknowns, namelyq1andq2. These equations can be solvedanalytically forq1andq2.We may also add to the goal a constraint on the orientation of the last link:3 = k.Combining this with Equation 2.3 we havek = q1 +q2 +q3.Sinceq1andq2are already constrained, this combination gives the solution for orientation.Unfortunately, if thesystemisnotperfectlyconstrained, nouniquesolutionexists. How-ever, Abdel-Rahman[1] developedageneralizedpractical methodfortheanalyticsolutionofconstrained inverse kinematics problems with redundant manipulators. This method rst yields aCHAPTER2. INVERSEKINEMATICSSOLVERS 12numeric solution proceeding under the guidance of a selected redundancy resolution criterion. Itthen also yields analytic expressions for computing the nonredundant joint rates in terms of theredundant joint rates. This generalized recursive method can systematically derive the analyticexpressions for all possible solutions of any redundant manipulator.2.2 NumericalSolutionsEven though the position and orientation equations are non-linear, the relationship between thevelocity of the distal end and the velocities of the joint angles is linear. If the forward kinematicproblem is stated by x = f(q), then numerical solutions to the inverse kinematic problem typicallyinvolve dierentiating the constraint equations to obtain a Jacobian matrixJ =fq,and solving the linear matrix system x = J q,where x=dxdtand q=dqdt . ThematrixJmapschangesinthejointvariablesqtochangesinthe end eector position (and orientation) x. The matrixJis anm n matrix, wheren is thenumberofjointvariablesandmisthedimensionoftheendeectorvectorx,whichisusuallyeither 3 for a simple positioning task, or 6 for a position and orientation task.The ith column of J, ji, represents the incremental change in the end eector due to the jointvariableqi. In other words, it is the direction and scale of the resulting innitesimal end eectorvelocity for an innitesimal unit rotational velocity at ith joint [22]. The columns of Jare closelyrelated to the vector dened from a joints axis to the end eector, denoted by piin Figure 2.2.In particular, the magnitudes of the jis and pis are equal, and their directions are perpendicular.The relation can be extended to three dimensions easily by using the cross product of a unit vectorCHAPTER2. INVERSEKINEMATICSSOLVERS 13123qqqX1pp2p3j3jj12Yq4Figure 2.2: A physical interpretation of the columns of the Jacobian matrixalong the axis of rotation aiwith the vector pito obtain ji:ji = aipi.CHAPTER2. INVERSEKINEMATICSSOLVERS 142.2.1 MatrixPseudoinverseThe most widely adopted method to solve inverse kinematic problems uses the pseudoinverse ofthe Jacobian: q = J+ x. (2.4)The pseudoinverse gives the unique least-squares solution when the system is redundant.Iterative schemes are used to compute the desired positional solution from the solution for thevelocities. At each iteration a desired x can be computed from the current and desired end eectorpositions. Thejointvelocity qcanthenbecomputedusingthepseudoinverseoftheJacobian,andintegratedtondanewjointcongurationvectorq. Theprocedurerepeatsuntiltheendeector has reached the desired goal. Since the linear relationship represented byJis only validfor small perturbations in the manipulator conguration, J must be recomputed at each iteration.Fortunately, whenthetimeinterval issmall, thenitedisplacementrelationshipisnearlylinear [19]:q J+x,andtheassumptionthat Jisconstantovertheinterval of thedisplacementisvalid. Thisisequivalent to Euler integration of the dierential equation represented by Equation 2.4, and is theapproach used in my work.Foraredundantsystem, thejointvelocitiescorrespondingtoagivenendeectorvelocitycan be computed using a null-space projection technique. This will be discussed in Chapter 4 indetail.2.2.2 MatrixTransposeOneshortcomingofthepseudoinversesolutionisthatithasnorepeatability. Inotherwords,a closed or cyclic work space trajectory will not generally produce a closed or cyclic joint spacetrajectory [21]. This non-repeating property is not desired in most practical robot applications.Chiacchio, Chiaverini, Sciavicco,and Siciliano [6] uses the Jacobian transpose to resolve closed-loop inverse kinematic problems. While in an interactive goal-position specied computer graphicsCHAPTER2. INVERSEKINEMATICSSOLVERS 15application it is dicult to lead the kinematic chain back to its original conguration to form acyclic work space trajectory, and this might lead to usability problems, we will not consider thisdiculty further here.2.3 LagrangianMethodsAnotherapproachtosolvingaredundant(underconstrained)systemistoproposeanobjectivefunctiontobeminimizedandtoapplyoptimizationtechniques. LagrangianmethodscanbeusedtoextendanunderconstrainedredundantsystemtoaperfectlyconstrainedsystemusingLagrange multipliers.Theobjectivefunctionisapplicationdependent. ExamplesbasedonjointlimitconstraintfunctionsaregiveninSection2.5.1andSection2.5.2. Supposewearegivenanobjectivefunc-tionP(q),whereqisthejointvector(q1, ..., qn),andwewanttominimizeitsubjecttothemconstraintsc1(q) = 0...cm(q) = 0.We introduce a vector of variables u = (u1, ..., um), called the Lagrange multipliers, and writedown the Lagrangian function:L(q, u) = P(q) (u1c1(q) +u2c2(q) +... +umcm(q)).Bysettingthepartial derivativesof Lwithrespecttoq1throughqntozero, wecreatennewequations:L/q1 = 0...CHAPTER2. INVERSEKINEMATICSSOLVERS 16L/qn = 0.This results in a system of m+n equations in m+n unknowns (qs and us), which is a perfectlyconstrained system.By rewriting the constraints as a single vector-valued function and performing algebraic ma-nipulation directly on it, it is sometimes possible to avoid solving such a large system of equations.Suppose we write the original constraints as follows:c(q) = 0. (2.5)The Lagrangian, which is scalar-valued, can be rewritten asL(q, u) = P(q) uTc(q).Setting the derivative with respect to the vector q to zero, we get the vector equation:Lq=(P(q))q(uTc(q))q= 0. (2.6)Equation2.5andEquation2.6formtwovectorequationsintwovectorunknowns(qandu). Ifwecaneliminateufromthesetwoequations, wecanobtaininasinglevectorequationconsisting ofn scalar equations in the originaln unknowns,q1, ..., qn.2.4 ReachHierarchyKoreinandBadler[18] proposedadierentapproachtosolvingpointgoal reachingproblems.The procedure relies on precomputed workspaces for the chain and each of its distal subchains.A distal subchain is a subset of a larger chain that shares its distal end.We still use the three-segment planar chain in Figure 2.1 as our example. Let the chain be C1,and its workspace beW1. Let the subchain with just the most proximal joint (q1) and segment(a1)deletedbeC2withworkspaceW2, andsoon. TheneachworkspaceWiisconstructedbyCHAPTER2. INVERSEKINEMATICSSOLVERS 17q3W3q4 W4q2W2q1W1Figure 2.3: Workspace of a three-segment planar chainCHAPTER2. INVERSEKINEMATICSSOLVERS 18pWi+1qipWi+1qi(a) (b)Figure 2.4: Basic(a) and dual(b) joint adjustment problems. In the basic problem, we adjust Wi+1to includesp; in the dual case, we look for the intersection between theWi+1and the trajectoryofp.sweepingWi+1about jointqi, as shown in Figure 2.3.Given these workspaces, the algorithm proceeds as follows:If goalp is not inW1, thenit is not reachable: give upOtherwise:fori := 1 to number of joints inC1:adjustqionly as much as is necessary so that the next workspaceWi+1includes the goalp.We can carry out the adjustment step without iteration by solving the dual adjustment problemof bringing the goal into the workspace as shown in Figure 2.4. This becomes a problem of ndingthe intersection between the workspace boundary and the trajectory of the goal, which is a circlefor a revolute joint and a line for a translational joint.The disadvantage of this method is that it requires precomputation and storage of workspaces.A workspace can be geometrically very complex in the case of a large number of constrained joints.When the goal and orientation spaces are of high dimensionality, this method is dicult to use.2.5 ConstrainedInverseKinematicProblemsThe most common constraint on inverse kinematic systems is joint limits. This is also the con-straint I considered in my application. Joint constraints are represented by inequality constraints.CHAPTER2. INVERSEKINEMATICSSOLVERS 19Several dierent methods can be used to make sure a solution satises them.2.5.1 TheLagrangeApproachThe Lagrange approach will nd all minima for the objective function,subject to equality con-straintsasstatedinSection2.3. Thoseminimathatdonotsatisfythejointlimitinequalitiescan be discarded immediately. Any new minima that arise from the inequalities must lie on theboundary of the region dened by those limits;that is,when one or more of the joint variablestakeextremevalues[18]. Therefore, bysettingoneqitoitslowerboundorupperboundeachtime, those minima can be found by solving 2n smaller problems, each involving one less variable,qi, than the original.2.5.2 IntroducingNewVariablesWe can also introduce new variables transforming each inequality into an equality constraint [7,18]. Assuming theith joint angle, qi, has upper limituiand lower limitsli[41], we can add 2nnewvariablesyilandyiuforithjointtotransformeachinequalityintoanequalityconstraint.The old inequality constraints forith joint areli qiui qi.These inequality constraints can be transformed to two new nonlinear equality constraints:uiy2iu = qiqiy2il = li,wherethe squares of yiuandyilensure the original inequalities. Now we canuse theLagrangeapproach to solve the original problem plus 2n new variables and 2n new equality constraints.CHAPTER2. INVERSEKINEMATICSSOLVERS 202.5.3 PenaltyFunctionMethodsAnothermethodaddspenaltyfunctionstotheobjectivefunction. Thealgorithmlooksfortheminimum value of the objective function, so the penalty causes the value of the objective functionto increase as joints approach their limits. The desired result is that the objective function itselfeectivelyprohibitsjointlimitviolations. Wecanaddthepenaltyfunctionsintotheequationsystem, so as to add joint limits to the inverse kinematic constraints. This method is also calledlimitspring[2] constraints, whichisusedtodiscouragethejointsfromreachingtheirlimitingvalue. The springs give a high value or energy level to the fully extended angle and they can betunedtoanyangle. Unfortunately, penaltyfunctionmethodsarenotthatstablesinceasmallchange may force the objective function to a large value.Several dierent penalty functions exist [40]. With an inequality-constrained problemmin P(q)such thatgi(q) 0, i = 1, 2, ..., r,we can dene the new objective function asP(q; K) = P(q) +r

i=1Ki[gi(q)]2ui(gi),whereui(gi) =___0 ifgi(q) 0,1 ifgi(q) > 0,andKi>0. As Kiisincreasedfromzerotoinnity, moreandmoreweightisattachedtosatisfyingtheithconstraint. WhenKi=0, theconstraintisignored, andwhenKi= , theconstraint must be satised exactly. Some care must be taken in the application of this penalty-function method, since the algorithm may converge to a ctitious solution if the problem is notproperly posed.CHAPTER2. INVERSEKINEMATICSSOLVERS 21Thereisanotherimportantclassof penalty-functionmethod, calledtheinteriormethods,becausetheyproceedtowardtheconstraintboundaryfrominsidethefeasibleregion. Withaninequality-constrained problemmin P(q)such thatgi(q) > 0, i = 1, 2, ..., r,we can dene the new objective function asP(q; K) = P(q) +Kr

i=11gi(q)orP(q; K) = P(q) Kr

i=1log gi(q)forK> 0. Note that the inequalities are strict since the penalty is innite at the boundary.All these solutions discussed so far have overkilling computations. A simple solution to jointlimits is used in my application. This will be discussed in Section 5.1.Chapter3SingularValueDecompositionInthischapterIreviewtheworkof Maciejewski [22, 24], andForsytheetal [10]. Iwill alsodescribe my work on the SVD based on the work of Maciejewski.3.1 PseudoinverseandSingularValueDecompositionTheJacobianmatrixinaredundantsystemisnon-square, soitsinversedoesnotexistintheusual sense. Ageneralizedinversemustbeused. Therearemultipledenitionsofgeneralizedinverses for dierent purposes. Denoting the generalized inverse ofJwithJ+, we can categorizethe generalized inverse by the following four properties, which are shared with normal inverses:JJ+J = J (3.1)J+JJ+= J+(3.2)(J+J) = J+J (3.3)(JJ+) = JJ+. (3.4)22CHAPTER3. SINGULARVALUEDECOMPOSITION 23Theoperation denotescomplexconjugatetranspose. If J+satisesEquation3.1, itiscalledageneralizedinverseof J. If J+alsosatisesEquation3.2, itiscalledareexivegeneralizedinverseof J. IfEquation3.3issatised, J+iscalledaleftweakgeneralizedinverse, andJ+JisHermitian. Finally, if J+satisesall fourrelationships, itiscalledapseudoinverseortheMoore-Penrose generalized inverse, whose existence and uniqueness is proved by Penrose [33]. IfJis a square and nonsingular matrix,thenJ+=J1. The pseudoinverseis the one that suitsour needs best. In practice, a Singular Value Decomposition (SVD) can be used to robustly ndthe pseudoinverse.The SVD theorem states that any matrix can be written as the product of three non-uniquematrices:J = UDVT,whereDis a diagonal matrix with non-negative diagonal elements known as singular values. Ifoneormoreofthesediagonal elementsiszero, thentheoriginal matrixisitselfsingular. Thecolumns ofUandVare called the left and right singular vectors. Depending on how the SVD isdened, for anmn matrixJ,D could be ann n,mm, or even anmn matrix. In fact,the size ofD does not matter that much, since the singular values on the diagonal ofD are whatwe are looking for.Thesingularvalueshavephysical interpretations[22]. Considerthe2D3-segmentchaininFigure 2.1 again. The set of all possible dierent combinations of joint velocities of unit magnitudefor jointq1,q2, andq3 can be represented as a sphere in joint space. Because of the directionallydependentscalingof theJacobiantransformation, thevelocityattheendeectorq4resultingfrom all these possible inputs will generally be described by an ellipse. We now choose our newcoordinate systems axes u1and u2as the major axis and minor axis of the ellipse respectively,as shown in Figure 3.1.This new coordinate system can be viewed as a simple rotation of the old coordinate systemby an angle , so vectors dened in one system can be transformed to the other using the rotationCHAPTER3. SINGULARVALUEDECOMPOSITION 24121122321312qqq1xx uuJ vvv11Figure 3.1: A geometric interpretation of the singular value decompositionmatrixUgiven byU=_u1u2_=__cos sin sin cos __.Rotatingthecoordinatesystemforjointspace, wecandeneanewcoordinatesystemgivenby the unit vectorv1, v2, v3. An input alongv1results in motion of the end eectoralongu1.Aninputalongv2resultsinmotionoftheendeectoralongu2. Aninputalongv3resultsinachangeinthechainsconguration, withoutproducinganyendeectormotion. Thiscanbemathematically represented in matrix form asV=_v1v2v3_.If we reformulate the inverse kinematic equation x =J q for the new coordinate systems, wegetUT x = DVT q, (3.5)whereUT x represents the desired end eector velocity in the u1 and u2 coordinate system;VT qrepresents the joint velocity in the v1, v2, and v3 coordinate system;D is a diagonal matrix withCHAPTER3. SINGULARVALUEDECOMPOSITION 25ion the diagonal and zero in other places:D =__10 00 20__.The value1and2are equal to one half the length of the major and minor axes of the ellipsein Figure 3.1 respectively.Notethat Uis orthonormal, soUUT=I. Multiplyingbothsides byUwecanrewriteEquation 3.5 as x = UDVT q.We can see that the three matricesU,D, andVare the SVD ofJ:J = UDVT.It is also common to write the singular value decomposition as the summation of vector outerproducts [22], which for an arbitrary Jacobian would result inJ =min(m,n)

i=1iuivTi,wherem andn are the number of rows and columns ofJ,and the singular values are typicallyordered from largest to smallest.We then can nd the pseudoinverse solution from the singular value decomposition by takingthe reciprocal of all nonzero singular values. In particular, the pseudoinverseJ+is given byJ+=r

i=11iviuTi,wherer is the rank ofJ. By denition, the rank is the number of nonzero singular valuesi.The pseudoinverse solution q = J+ x (3.6)CHAPTER3. SINGULARVALUEDECOMPOSITION 26qu1 u2x + 12xJ+v1v2v31/12 1/q + Figure3.2: Thetransformationof relativeuncertaintyinthevelocityof theendeector touncertainty in the calculated velocity of the joints.minimizestheresidualgivenby || x J q||, whichphysicallymeansthatthevelocitywillbeasclose to the desired velocity as possible [22]. Minimization of the residual does not guarantee anunique solution, but the pseudoinverse solution also minimizes the norm of the solution, || q||; thatis, it minimizes the total joint motion under the constraint of minimization of the residual.3.2 DampedLeast-SquaresMethodThe singular values are crucial in determining how error is magnied [22], since they specify howatransformationscalesdierentvectorsbetweentheinputspaceandtheoutputspace. Thecondition number of a transformation is dened as =maxmin.It provides a bound on the worst case magnication of relative errors. If the uncertainty in x isdenoted by x, then the range of possible values of x+ x denes a circle in the end eector space,as illustrated in Figure 3.2. Transforming this circle back into the joint space results in an ellipseinthejointspacewithminorandmajoraxesalignedwithv1andv2andofmagnitudesequalCHAPTER3. SINGULARVALUEDECOMPOSITION 27Figure 3.3: An example of an ill-conditioned Jacobian: a small change in the position of the handrequires a large change in the position of the shoulder joint.to the reciprocalsoftheir respectivesingular values. Therelativeuncertaintyinthe solution isbounded by|| q|||| q|| || x|||| x||.Theconditionnumberisalsoameasureofhowill-conditionedthematrixJis[22]. Whenitsreciprocal approachesmachineprecisionlimits, wesayitistoolargeandthematrixisill-conditioned. Thepseudoinverseofanill-conditionedmatrixwillgeneratealargejointvelocity.AsimpleexampleisgivenbyMaciejewski[22]. Considerthemotionofthehumanarminthesagittal plane illustrated in Figure 3.3. If the hand is to be placed slightly under the shoulders ason the left, the elbow must be located behind the back of the gure. If the hand is slightly overtheshouldersasontheright,theelbowmustbeinfrontofthegure. Thus,foranextremelysmall change in the vertical direction of the hand, the joint in the shoulder must travel throughCHAPTER3. SINGULARVALUEDECOMPOSITION 28almost its entire range of motion. If there is any small variation in the calculation of the handsposition, the error is magnied.From the above, we see that using the pure pseudoinverse solution for the equations describingthe motion of articulated gures may cause discontinuities [22]. This happens between singularand nonsingular transitions, even though physically the solution should be continuous. We againusethehumanarminthesagittal planeinFigure3.3asourexample. Whileall thesingularvalues remain nonzero, the pseudoinverse ofD, denoted byD+, will be given byD+=__1100120 0__.However, when the hand moves close to the shoulders, the columns of Jacobian that depend onthe orientation of the three segments upper arm, forearm, and hand (as analyzed in Section 2.2)become linearly dependent. We say the limb moves into a singular conguration and the smallestsingular value becomes zero. The pseudoinverse becomesD+=__1100 00 0__.Evenif wesetupalowerboundforthesingularvalues, adiscontinuitystill occurs. Theproblemwiththepseudoinversesolutionisthattheleast-squarecriterionofachievingtheendeectortrajectoryminimizing || x J q|| takespriorityoverminimizingthejointvelocity || q||.Thus one method of removing this discontinuity, and also limiting the maximum solution norm,is to consider both criteria simultaneously.Thedampedleast-squaresmethodhasbeenusedforsolvingill-conditionedequations[22].The damped least-squares criterion is based on nding the solution that minimizes the sum|| x J q||2+2|| q||2,CHAPTER3. SINGULARVALUEDECOMPOSITION 29whereisreferredasthedampingfactorandweightstheimportanceof minimizingthejointvelocity with respect to minimizing the residual. This results in the augmented system of equations__JI__ q =__ x0__,where the solution can be obtained by solving the consistent normal equations(JTJ +2I) q = JT x.The damped least-squares solution is q()= (JTJ +2I)1JT x =r

i=1i2i+2viuTi x,whichistheuniquesolutionmostcloselyachievingthedesiredendeectortrajectoryfromallpossiblecombinationsofjointvelocitiesthatdonotexceed || q()||. Fromthiswecanseethatthepseudoinverseisaspecialcaseofthedampedleast-squaresformulationwith = 0. Inthefollowing part of my thesis, I use the damped least-squares solution for my pseudoinverse:J+=r

i=1i2i+2viuTi.Thedampedleast-squaressolutioncanbeconsideredasafunction ofthe singularvaluesasshowninFigure3.4[22]. Ifasingularvalueismuchlargerthanthedampingfactor, thenthedamped least-squares formulation has little eect, becausei2i+2 1i,which is identical to the solution obtained using the pseudoinverse. For the singular values on theorder of, the term in the denominator limits the potentially high norm of that component ofCHAPTER3. SINGULARVALUEDECOMPOSITION 3012Damped Least-SquaresLeast Squares (Pseudoinverse)Norm of Joint Velocity Singular Value ( )Figure 3.4: A comparison of the damped least-squares solution to least-squares solutionthe solution. The maximum scaling for this component is limited by q()i xi12,wherethesubscript i denotes thecomponents associatedwiththe ithsingular value. If thesingular value becomes much smaller than the damping factor, theni2i+2 i2,which approaches zero as the singular value approaches zero. This demonstrates continuity in thesolution, despite the change in rank at the singularity. The damped least-squares formulation canbe made arbitrarily well conditioned by choosing an appropriate damping factor.In this thesis, the user can change the value of via the interface to see its dierent eects.By experimentation I found out that the larger the damping factor is, the more resistance theCHAPTER3. SINGULARVALUEDECOMPOSITION 31jointswillhavewhileinmotion. Theoretically, thisisbecausethelargerthevalueof is, thesmaller the maximum norm of the joint velocity || q()||, given by12, is.3.3 Golub-Reinsch(LAPACK)A very ecient method to compute an SVD was developed by Golub and Reinsch [10].There are two stages in the Golub-Reinsch algorithm. The rst stage involves using a seriesof Householder transformations to reduceJto bidiagonal formB, which is a matrix whose onlynonzero elements are on the diagonal and the rst superdiagonal (or the rst subdiagonal):J = U1BVT1,whereU1andV1are orthogonal.Thesecondstageis aniterativeprocess inwhichthesuperdiagonal (or thesubdiagonal)elements are reduced to a negligible size, leaving the desired diagonal matrixD:B = U2DVT2,whereU2andV2are orthogonal and the singular vectors ofJare the columns ofU= U1U2andV= V1V2.An implementation of this SVD algorithm is included in many numerical computation softwarepackages. LAPACK is one of them. It is written in Fortran, and the code is quite optimized. Iused the LAPACK double-precision dgesvd SVD subroutine through a C++ wrapper in my workto compare the speed and robustness of the other SVD techniques.3.4 Column-wiseGivensRotationsTheGolub-Reinschalgorithm isgenerallyregardedasthemostecientandnumericallystabletechnique for computing the SVD of an arbitrary matrix. But in our case, the current JacobianCHAPTER3. SINGULARVALUEDECOMPOSITION 32is only a small perturbation of the previous one. Using Givens rotations for computing the SVDincrementally using a previous solution takes advantage of this [24].Givens rotations are orthogonal transformations of the formVij=__1 . .. . .1 . .. . . cos . . . sin . . . i. 1 .. . .. 1 .. . . sin . . . cos . . . j. . 1. . .. . 1__,i jwhere all those elements not shown are ones on the diagonal and zeros elsewhere. This transfor-mation can be geometrically interpreted as a plane rotation of in the i-j plane. Givens rotationsonly aect two rows or columns of the matrix with which they are multiplied.We can choose an orthogonal matrixVcomposed of successive Givens rotations, such that ifwe multiplyJbyV ,JV= B, (3.7)thenthecolumnsof Bwillbepairwiselyorthogonalized. ThematrixBcanalsobewrittenasthe product of an orthonormal matrixUand a diagonal matrixD,B = UD, (3.8)CHAPTER3. SINGULARVALUEDECOMPOSITION 33by letting the columns ofUbe equal to the normalized versions of the columns ofB,ui =bi||bi||,and dening the diagonal elements ofD to be equal to the norm of the columns ofBdii = ||bi||.By substituting Equation 3.8 into Equation 3.7 and solving forJ, we obtainJ = UDVT,which is the SVD ofJ.Thecritical stepintheaboveprocedureforcalculatingtheSVDistondanappropriatematrixVas a product of Givens rotations. Considering the currentith andjth columns ofJ, aiand aj, multiplication by a Givens rotation oni-jplane results in new columns a

iand a

jgivenbya

i = ai cos +aj sin (3.9)a

j = aj cos ai sin . (3.10)The constraint that these columns be orthogonal gives usa

Tia

j = 0. (3.11)Substituting aiand ajin Equation 3.11 by Equation 3.9 and Equation 3.10 respectively yieldsaTi aj(cos2 sin2) + (aTj aj aTi ai) sin cos = 0. (3.12)Adding equationcos2 + sin2 = 1, (3.13)CHAPTER3. SINGULARVALUEDECOMPOSITION 34we can solve for the two unknowns sin and cos using Equation 3.12 and Equation 3.13. Thisis done for each pair of columns in the matrixJ.If theGivensrotationusedtoorthogonalizecolumns i andj isdenotedbyVij, thentheproduct of a set ofn(n 1)/2 rotations is denoted byVk =n1

i=1_n

j=i+1Vij_.This is referred to as a sweep. Unfortunately, a single sweep generally will not orthogonalize allthecolumnsofamatrix,sincesubsequentrotationscandestroytheorthogonalityproducedbyprevious ones. However, the procedure can be shown to converge [31], so Vcan be obtained fromV=l

k=1Vk,where the number of sweepsl is not known a priori. Orthogonality is measured by =(aiTaj)2(aiTai)(ajTaj).If for two columns is below a threshold, then these two columns are considered orthogonalizedand the rotation does not need to be performed.In my application, the user interactively species the goal position of the end eector, and theJacobianJis only a small perturbation of the previous one. If we use the previous resultingVas the starting matrix for the currentVinstead of an identity matrix, in most of the cases onlyone sweep is needed. As a result, in practice checking for orthogonality can be eliminated.Maciejewski and Klein [24] also found that if the vectors aiand ajare not equal in length,will be small, and we can approximate cos with 1 and sin with. Conversely, if their lengthsare almost equal, both cos and sin can be assigned 2/2. This observation reduces the requirednumber of double precision multiplies by half.Problems still exist in this algorithm. First of all, we will get accumulated errors by reusing thepreviousV . One simple solution to this problem is to resetVto the identity matrix periodically.CHAPTER3. SINGULARVALUEDECOMPOSITION 35Another solutionis toalternatelyorthogonalizecolumns androws of matrixJ. Thesecondproblem is correctness. In my applications, the Jacobian is a 3 n matrix, so there are supposedto be at most three non-zero singular values. By orthogonalizing and normalizingn columns ofJ, it is possible to getn nonzero singular values instead of three. However, the biggest problemis performance. WhentheJacobianhas alargenumber of columns, whichis typical inmyapplication, this algorithm is far too slow.3.5 Row-wiseGivensRotationsTo overcome the shortcomings of the column-wise Givens rotation technique, I use Givens rotationsrow-wisely. IorthogonalizetherowsoftheJacobianmatrixinsteadofcolumns. Sincethereisonly three rows, the matrix calculation in the row-wise Givens rotations method is much less thanthat of the standard column-wise Givens rotations technique,so the program runs much faster.Mymethodalsoguaranteesthattherearenomorethanthreenon-zerosingularvalues, sincethere are only three rows inJ.To orthogonalize J by rows, we need to nd an orthogonal 33 matrix UT. Then we multiplyJtoUTto get the matrixB:UTJ = B.After normalizing the rows ofB, we getB = DVT.This still gives usJ = UDVT.3.6 PerformanceComparisonThe time performance of all these techniques is discussed in detail in Section 5.2. Basically, therow-wise Givens Rotations method has the same performance as the optimized SVD subroutine inCHAPTER3. SINGULARVALUEDECOMPOSITION 36LAPACK. And they are both much faster than the raw Golub-Reinsch algorithm and column-wiseGivens rotations technique.Chapter4ObstacleAvoidanceRedundancy in a system can be used to accomplish various secondary tasks. Obstacle avoidanceis oneof themandit is theoneconsideredinmywork. This chapter will reviewdierentapproachesforavoidingoneobstacle, includingthetaskpriorityapproachof Maciejewski [23]and Pourazady [35], the cost function technique of Marchand and Courty [25], and the objectivefunction method of Samson et al [38]. These techniques will then be extended to avoiding multipleobstacles in Section 5.3.Recall the inverse kinematics problem, where the linear relationship between the end eectorvelocity, describedbyathree-dimensional vector x, andthejointvelocities, denotedbyan-dimensional vector q, wheren is the number of degrees of freedom, is described by the equation x = J q, (4.1)whereJis the Jacobian matrix. It can be shown [20] that the general solution to Equation 4.1 isgiven by q = J+ x + (I J+J)z, (4.2)whereIis ann n identity matrix and z is an arbitrary vector in q-space. The resultant jointangle velocities can be decomposed into a combination of the damped least-squares solutionJ+ x37CHAPTER4. OBSTACLEAVOIDANCE 38plus a homogeneous solution (I J+J)z. The projection operator (I J+J) describes the degreesof redundancy of the system. It maps an arbitrary z into the null space of the transformation.By applying various functions to compute the vector z, reconguration of the manipulator canbeobtainedtoachievesomedesirablesecondarycriterion, suchasobstacleavoidance, withoutaecting the specied end eector velocity.4.1 AvoidingOneObstacleThere are three major methods to nd a proper vector z for Equation 4.2, for the case of avoidingone obstacle. They are discussed in following subsections.4.1.1 TaskPriorityApproachIn the task priority approach [23, 35, 30, 6], the rst step is to identify for each period of time theshortest distance between the manipulator and the obstacle. As shown in Figure 4.1, the closestpoint to the obstacle on the manipulator R is referred to as the critical pointwith xR as its worldcoordinates; theclosestpointtothemanipulatorontheobstacleSiscalledtheobstaclepointwithxSasitsworldcoordinates; andthedistancebetweentheobstaclepointandthecriticalpointiscalledthecritical distancedenotedbyd(q, t). Inthesecondstepthecritical pointisassigned a velocity in a direction away from the obstacle.The primary goal of specied end eector velocity and the secondary goal of obstacle avoidanceare described by the equationsJe q = xe(4.3)Jo q = xo, (4.4)whereJeistheendeectorJacobian, Joisthecritical pointJacobian, xeisthespeciedendeector velocity, and xois the specied critical point velocity.One simple way to nd a common solution to these two equations, called task space augmen-tation, is to adjoin both the two matrices on the left hand side and the vectors on the right handCHAPTER4. OBSTACLEAVOIDANCE 39BaseXeObstaclexylll2n112nEnd EffectorObstacleSRXoPointCriticalPointFigure 4.1: Primary and secondary goal of a redundant manipulatorside into a single matrix equation [6, 23]:__JeJo__ q =__ xe xo__.However, it is not desirable to treat the end eector and obstacle avoidance velocity in the sameway. Ataskprioritycanbeusedinthiscasetorstsatisfytheprimarygoal of endeectorvelocity and then use the systems redundancy to best match the secondary goal of critical pointvelocity.Substituting the general solution Equation 4.2 of primary goal Equation 4.3 to the secondarygoal Equation 4.4 yieldsJoJ+e xe +Jo(I J+eJe)z = xo.CHAPTER4. OBSTACLEAVOIDANCE 40From this we can solve for zz = [Jo(I J+eJe)]+( xoJoJ+e xe). (4.5)Replacing z in Equation 4.2 with Equation 4.5, the nal answer can be written as q = J+e xe + (I J+eJe)[Jo(I J+eJe)]+( xoJoJ+e xe).Since the projection operator is both Hermitian and idempotent, the result can be simplied [23]to q = J+e xe + [Jo(I J+eJe)]+( xoJoJ+e xe)or alternatively q = J+e xe +h[Jo(I J+eJe)]+(o xoJoJ+e xe), (4.6)where xoisnowconsideredasanunitvectorindicatingthedirectionmovingthemanipulatoraway from the obstacle, which is dened from the obstacle point to the critical point as shown inFigure 4.1. The factorois the magnitude of the secondary goal velocity, and the valuehis again term for the amount of the homogeneous solution to be included in the total solution.EachtermintheEquation4.6hasaphysical interpretation[23]. Asdiscussedearlier, thepseudoinversesolutionJ+e xeensures theexact desiredendeector velocityintheredundantsystemwiththeminimumjointvelocitynorm. Theaddedhomogeneoussolutionsacricestheminimum norm solution to satisfy the secondary obstacle avoidance goal. The matrix composedoftheobstacleJacobiantimestheprojectionoperator, Jo(I J+eJe),representsthedegreesoffreedomavailabletomovethecriticalpointwhilecreatingnomotionattheendeector. ThismatrixisusedtotransformthedesiredobstaclemotionfromCartesianobstaclevelocityspaceinto the best available solution in joint velocity space, again through the use of the pseudoinverse.Finally, the vector describing the desired critical point motion, o xo, obtained from environmentalinformation, is modied by subtracting the motion caused at the critical point due to satisfactionof the end eector velocity constraint,JoJ+e xe.CHAPTER4. OBSTACLEAVOIDANCE 41DistanceGainVmax hod d dta ug soiFigure 4.2: The form of the homogeneous term gainhand the obstacle avoidance gainoas afunction of the critical distance.The functions o and h can be described by polynomials of the form shown in Figure 4.2 [23].From the gure we can see that there are three distances that characterize the changes in the valueofthegainfunctions. Thesedistancesaredenedasthetaskabortdistancedta,theunitgaindistance dug, and the sphere of inuence distance dsoi. These distances dene four zones for eachobstacle. If the manipulator is further from the obstacle than the distance dsoi, then the obstaclehas no inuence on the manipulator. Between the distancedsoiand the distancedug, there is asmooth transition from the obstacle inuence being fully considered to totally ignored. Inside thedistancedug, the gain factor is constant. If the critical distance reachesdta, then further motionwill cause a collision.The value ofocould be any function that is inversely related to the distance, as long as therst derivative of this function atdugis zero.Pourazady and Ho [35] came up with an inuence function forothat is a function of boththe critical distance and the relative velocity of the manipulator with respect to the obstacle. Ifthecriticalpointismovingawayfromtheobstacle, meaningtheapproachingvelocityvislessthan zero, the inuence function is then dened as zero. On the other hand, if the manipulatorCHAPTER4. OBSTACLEAVOIDANCE 42is approachingtheobstacle, theinuencefunctionis increasedfromzerotoinnityandtheacceleration needed to avoid the obstacle increases from zero to the acceleration limitamax.The minimum avoidance timeis the time to stop the manipulator by applying the maximumallowable acceleration. From the equation of motionvf= v +amaxwherevfis zero, we have= vamax.Ontheotherhand, anarbitraryconstantaccelerationlessthanamaxhastobeappliedtothecritical point to stop the manipulator before it has traversed the critical distance d. The maximumavoidance timeTis given byT=2dv.The reserve avoidance time is the dierence between the maximum and minimum avoidance time,T . The inuence functionPis dened as the reciprocal of the reserve avoidance time whenthe manipulator is approaching the obstacle:P=__0 ifv< 0amaxv2damaxv2ifv 0Thenoin Equation 4.6 is dened byo = (1 eP)vmax.Unfortunately, this technique is based on the physical limitation of a robot, such as its maximumacceleration and its maximum velocity. It turns out to be a poor solution for my program, sincethis introduces new parameters to be tuned and they are both application dependent.CHAPTER4. OBSTACLEAVOIDANCE 434.1.2 CostFunctionApproachMarchand and Courty [25] use a cost function to avoid obstacles in their camera control problem.The cost function is dened ashs =1d2,whered is the critical distance. The arbitrary vector z in Equation 4.2 is dened asz = h2s(x, y, z, 0, ..., 0)T, (4.7)where is a positive factor, and (x, y, z) is the vector xSxR.Comparingthecostfunctiontechniquewiththetaskpriorityapproach, computationofthecost function technique is much simpler. Also, the cost function technique gives a smoother jointtransition than the task priority approach. For example, in my application, the kinematic chainwill remain a smooth curve while using the cost function technique to avoid obstacles, but the taskpriority approach will sometimes distort the chain into a kinked form as shown in Figure 5.5.Unfortunately, thecostfunctiontechniquehasnoclearphysical interpretation; furthermore, itonly uses the rst three columns of the projection operator (I J+J). This makes it dicult togeneralize to the avoidance of multiple obstacles.4.1.3 ObjectiveFunctionApproachYetanotherredundancyresolutionschemecomputeszasthegradientofanobjectivefunctionP(q, t) and projects it to the null space of the Jacobian. The equation can be rewritten as q = J+ x +(I J+J)Pq, (4.8)where should be a positive gain factor ifPis to be maximized, or a negative gain factor ifPisto be minimized.The objective functionPis dened according to the desired secondary criterion. In terms ofobstacleavoidance, thefunctioncanbedenedtomaximizethedistancebetweentheobstacleCHAPTER4. OBSTACLEAVOIDANCE 44RnnsrdSFigure 4.3: Using objective function method to avoid an obstacleand manipulator [5], minimize the joint angle availability [19], represent a certain aspect of robotdexterity [12], minimize a performance index [16], or maximize some areas between the links andthe obstacles [26].Samson, Le Borgne, and Espiau [38] discussed a minimal distance method to avoid obstacles.Given an objective function of the formP(q, t) = dk(q, t)with > 0k N,the gradient ofPisPq= kd(k+1)(q, t)dq(q, t). (4.9)This in turn requires the calculation ofdq(q, t). To do this, we dene the unitary vectors nrandnsas shown in Figure 4.3, wherenr =1d(q, t)(xSxR) (4.10)CHAPTER4. OBSTACLEAVOIDANCE 45andns = nr.Then the critical distance can be written asd(q, t) = nr, RS ,where a, b = aTb, andd = nr, VSVR = nr, VR +nr, VS . (4.11)Rewriting the equation of the second goalJo q = xoyieldsVR = Jo q. (4.12)Finally, recall thatd =dq q +dt. (4.13)SubstitutingVRand nrin Equation 4.11 by Equation 4.12 and Equation 4.10 respectively,andthen substituting the result into the left hand side of Equation 4.13, we obtain_1d(q, t)(xSxR), Jo q_+nr, VS =dq q +dt. (4.14)By denition,nr, VS =dt.so the equation can be simplied to_1d(q, t)(xSxR), Jo q_=dq q. (4.15)CHAPTER4. OBSTACLEAVOIDANCE 46Writing the vectors and matrix on the left hand side of Equation 4.15 by their elements, we get_1d(q, t)__xyz__,__a11a12... a1na21a22... a2na31a32... a3n____ q1 q2... qn___= 1d(q, t)[x(a11 q1+a12 q2+...+a1n qn)+y(a21 q1+a22 q2+...+a2n qn)+z(a31 q1+a32 q2+...+a3n qn)]=_1d(q, t)__a11a21a31a12a22a32. . .a1na2na3n____xyz__,__ q1 q2... qn___,from which we can solve fordq(q, t):dq(q, t) = 1d(q, t)JTo (xS xR).Substituting this back to Equation 4.9, the gradient ofPisPq= kd(k+2)(q, t)JTo (xSxR).Iassignkas2andas1inmyapplication. Thisresultsinasmoothtransitionbetweenframes with a relatively simple calculation.CHAPTER4. OBSTACLEAVOIDANCE 474.2 AvoidingMultipleObstaclesOneadvantageof theobjectivefunctionmethodisthatitcanbeusedformultipleobstacleswithout any change. Unfortunately, extending the other two techniques to avoid multiple obstaclesis non-trivial. I tried several approaches that will be discussed in Section 5.3, and it appears thatthe objective function method is best for my application.Chapter5ResultsTo test the idea of the preceding chapters, I implemented an interactive program for animating ahighly articulated model - an elephant using C, Tcl/Tk, and OpenGL. The implementation detailsof this application are stated in Appendix A. Figure 5.1 is a screen shot of the application. Theelephant is placed in a complex environment including a table and a tree. The elephant trunk isserved as the highly articulated kinematic chain. By default it is formed by a series of 30 spheres,andeachpairof spheresislinkedby3rotational joints. Thebasicideaistomakethetipofthe elephant trunk follow the mouse cursor within its joint limits, while avoiding obstacles in theenvironment. This chapter compares and discusses my results,including joint limit constraints,the implementation of the SVD and pseudoinverses, and obstacle avoidance techniques. I compareexisting techniques with some of the new approaches I have developed.Iimplementedtwojointlimitsmethods: clippingandprojection. Theprojectiontechniqueworks better thantheclippingtechnique, becausetheclippingtechniquecaneasilylockthekinematic chain when one of its joints solutions is out of the limit and the solution for the nexttime frame continues in the same direction. Four algorithms for the SVD were compared. Twoof themwerebasedontheGolub-Reinschalgorithm. Thethirdoneusedcolumn-wiseGivensrotations. IdevelopedaSVDalgorithmusingGivensrotationsworkingincrementallyovertherows of the Jacobian matrices. This method is as robust and fast as the Golub-Reinsch algorithm48CHAPTER5. RESULTS 49Figure 5.1: A snapshot of the applicationon abFigure 5.2: Two joint limits methods: projection and clipping. When the new position n is withinone joint limits and out of another, we can either projectn back to the boundary at pointa orclip it at the pointb where the joint constraints are about to be exceeded.CHAPTER5. RESULTS 50implemented in LAPACK, but it is a much simpler computation to implement. I also tried severalmultiple obstacle avoidance techniques. They are variations of three major approaches: the taskpriority approach, the cost function approach, and the objective function approach. Among them,the objective function technique suits my application most.5.1 JointLimitsMethodsComparisonAsimplesolutiontojointlimitsisusedinmyapplication. Recall thatmyinversekinematicsolution is incremental; a new joint conguration is calculated every time a new goal position isgiven. During this calculation, the joint limits are ignored until a proposed solution violates them.The joint angles are then mapped back to the boundaries of the solution space. I implementedtwo mapping methods, projection and clipping. The dierence between them is demonstrated inFigure 5.2.Assume that a new conguration involves the modication of two joint angles. Both of themarebounded, shownasarectangleinthesolutionspaceinFigure5.2. Theoldcongurationmaps the joint angles to positiono, and the new conguration maps the joint angles to positionn. When we try to bound the joint angles to a solution that respects the boundaries, we havetwochoices. Therstoneisprojection. Weprojecttheanglethatisoutofboundsbackontotheclosestboundarypoint, aspoint ashows. Thesecondmethodisclipping. Wefollowthesame trajectory and scale both angles movement to make the approximate solution lie within theboundary, as pointb shows.Practical dierences exist between these two boundary methods. By projecting the exceededjoint angle back to the boundary, we obtain a more exible solution than clipping the joint anglesat a portion of their trajectory. With clipping, if the intended movement continues when one ofthe joint angles is at its boundary, the entire chain will lock at that position, since all the anglesare still trying to follow the same trajectory. This is undesirable. With projection, the kinematicchain canstill be recongured in above situation,and givecontinuous approximate feedback tothe user.CHAPTER5. RESULTS 515.2 SVDTechniquesComparisonIn Chapter 3, I presented three techniques for singular value decomposition. The rst one is theclassic Golub-Reinsch algorithm, where any arbitrary matrix is reduced rst to a bidiagonal form,then to a diagonal matrix. The secondapproach uses Givens rotations taking advantage of thefact that in the inverse kinematic problem the new Jacobian is a perturbation of previous one. Theoriginal idea was to orthogonalize the Jacobian by columns. However, as a Jacobian of a highlyredundant system, its number of columns is much larger than the number of rows. Orthogonalizingcolumnsisanexpensivecomputation. Anotherdisadvantageof thestandardGivensrotationsapproach is that it usually results in more singular values than the Jacobian actually has. Basedontheoriginal Givens rotations technique, I developedanalternativeincremental evaluationmethod. ThismethodorthogonalizestherowsoftheJacobian, whichgivesusareliableresultwith less computation.The time performance of all these techniques is shown in Figure 5.3. The x axis is the numberof spheres in the kinematic chain of my application. Each pair of spheres in the chain is linkedbythreerotational joints. Theyaxisisthetimespentsolving q=J+ xindoubleprecision,innanoseconds. TheexperimentswererunonaSGIOctanewith1175MHZIP30processor,MIPS R10000 CPU, and 384 Mbytes main memory size. We can see that as the number of jointsincreases, the basic incremental Givens rotation technique takes much more time to compute theSVD than the other methods. The raw Golub-Reinsch algorithm, which I coded in C based on aprocedure from Numerical Recipes [36], runs slower than the optimized code in LAPACK.Asthecurveforrow-wiseGivensrotationsmethodoverlapsthecurveof LAPACKinFig-ure 5.3, I present a closer look at these two methods in Figure 5.4. From the curves, we can seethat row-wise incremental Givens rotations evaluation has almost exactly the same performance asLAPACK. The times are so similar, in fact, that I suspect that both algorithms are bottleneckedby memory access and not CPU operations. However,the row-wise Givens rotations method iseasier to program, and would even be suitable for a high-performance hardware implementationif desired.CHAPTER5. RESULTS 520 5 10 15 20 25 3000.511.522.533.5x 105number of spheresinverse kinematic timeRowwise Givens RotationsGolubReinsch (nr)LAPACKColumnwise Givens RotationsFigure 5.3: The time performance comparison of all the SVD techniques discussed.0 5 10 15 20 25 300100020003000400050006000number of spheresinverse kinematic timeRowwise Givens RotationsLAPACKFigure 5.4:The time performance comparison of LAPACK and row-wise Givens rotations method.CHAPTER5. RESULTS 535.3 AvoidingMultipleObstaclesApproachesSection 4.1 talked about three dierent approaches for avoiding one obstacle using the equation q = J+ x + (I J+J)z, (5.1)whichisadampedleast-squaressolutionJ+ xplusahomogeneoussolution(I J+J)z. Thescientists who developed these techniques also suggested extensions for avoiding multiple obstacles.Thefollowingsectionstalkabouttheseoriginal extensionideas, theirproblems, andmyownextensions.5.3.1 TaskPriorityApproachUsingthetaskpriorityapproach, multiplesecondarygoalscanbeconsideredbyweightingthehomogeneous solutions of each of them [23]. In the case of obstacle avoidance, we can ignore largecritical distances and concentrate on worst case(s). Since the use of a single worst-case obstaclepoint may result in oscillation for some congurations or environments, two worst-case obstaclesare considered. The solution is modied to q = J+e xe +1(d2/d1)h1 +2(d2/d1)h2, (5.2)wherehiistheithhomogeneoussolution, iisitscorrespondinggain, anddiisthecriticaldistancetotheobstacle. Thesubscript 1denotes theworst-caseobstacle. Thegreater thedierence between d1 and d2, the closer 1 approaches to unity and the closer 2 approaches zero.If d1isapproximatelyequal tod2, then1and2areboth0.5withtheoverall homogeneoussolution split between the two worst-case goals.Thismethodunfortunatelyrequiresextensivecomputation. Italsogeneratesanon-smoothsolution chain in some circumstances, as shown in Figure 5.5.CHAPTER5. RESULTS 54Figure5.5: Theelephanttrunkmaybeinakinkedformwhenthetaskprioritytechniqueisused to avoid obstacles.5.3.2 CostFunctionApproachThe cost function for multiple obstacles suggested by Marchand and Courty [25] simply adds thecost of the critical distances for each obstacle together:hs =

1d2i.Theproblemwiththismethodisobvious. Firstly, thereisnopriorityforthemosturgentclosest critical distances; secondly, the inz = h2s(x, y, z, 0, ...0)T(5.3)must be tuned whenever the number of obstacles is changed, otherwise the obstacle inuence willbecome larger as the number of obstacles grows.CHAPTER5. RESULTS 55To address these problems, I came up with a new cost function, which I call blending, formultiple obstacles:hs =n

i=1_1n 1.

d di

d_ 1d2i. (5.4)Now the gain of each critical distance depends on how urgent the situation is. The most urgentone, whichhasthesmallest di, will havethelargestgain; also, thesumof thegainsisunity.Theoretically, once we have tuned the parameter in Equation 5.3, we can keep using it no matterhowmanyobstacleswehave. Butthisstill cannotovercomethefactthatonlytherstthreecolumns of the projection operator are used, which results in lack of of physical interpretation.5.3.3 ObjectiveFunctionApproachIn Section 4.1.3 I used a critical distance objective function to avoid one obstacle. The formulationthat I used in my implementation was q = J+ x + 2d4 (I J+J)JTo (xSxR), (5.5)recallingthat disthecritical distance, JistheendeectorJacobian, Joisthecritical pointJacobian,xSis theworld coordinates ofthe obstaclepoint,andxRisthe world coordinates ofthecritical point. Thisformulationstillworkswithmultipleobstacles. Theintheequationdoes not need to be tuned each time the number of obstacles or number of DOFs changes. Thekinematic chain moves in between the obstacles and the solution remains smooth.5.3.4 OtherApproachesThefollowingareapproachesthatItriedbasedontheoriginalmultipleobstacleavoidanceal-gorithmssuggestedinthepapers. Unfortunately, theyarenot goodinpracticeandarenotrecommended in their current form.CHAPTER5. RESULTS 56TaskSpaceAugmentationToavoidexpensivematrixcomputations, Itriedtosimplyaugmenttheoriginal matricesandvectors.In the task priority technique, assuming each obstacle i has Jacobian Joi and obstacle velocity xoi, we can replace the secondary goalJo q = xowith__Jo1Jo2..__ q =__ xo1 xo2..__.In the cost function approach, we can write the arbitrary vector z asz = __x1/d41y1/d41z1/d41x2/d42y2/d42z2/d42..__,where(xi, yi, zi)isthevectorxSi xRiforeachobstaclei, anddiisthedistancebetweenthem.Thisaugmentationmethodgiveshighperformance, buttheresultisnotsatisfying, sinceithandleseveryobstacleinthesamewayregardlessofitsurgency. Italsolimitsthenumberofobstacles, since the number of rows of these matrices and vectors cannot be more than that of theend eector Jacobian. To address the rst problem of prioritization, I multiplied the correspondingCHAPTER5. RESULTS 57element in vector z by the blending factors dened in Equation 5.4. Unfortunately, the limits onthe number of obstacles remain.AdaptiveCostFunctionAnother cost function I tried washs =

e2d2i,wheredi is the critical distance for theith obstacle. The vector z in Equation 5.1 is then denedasz = hs(x, y, z, 0, ..., 0)T,where (x, y, z) is the vector xSxR for the worst critical distance. After each critical distanceiscalculated, thevalueof andcanbeadjustedbycomparingthecritical distancetoduganddta. If the distance is larger than thedugdened in Section 4.1.1, the situation is not thaturgent,so is reduced to a half andis reduced to a quarter,by which the integration of thecost function remains the same. If the distance is smaller than thedta, will be doubled andwill be quadrupled. Otherwise, andremain the same.Unfortunately, this gave an even worse result than those techniques with no automatic param-eter adjustment; furthermore, it gives us two new parameters to tune.PriorityReversingFromthepointofviewofauser, notpenetratingintoobstaclesisveryimportant, oftenmoreimportant than reaching the goal. One more thing I tried was to reverse the primary end eectorvelocity goal and secondary obstacle avoidance goal by changing Equation 4.6 to q = J+o xo +h[Je(I J+oJo)]+(e xeJeJ+o xo).This did not succeed. The manipulator just looks like it is dancing freely in 3D space. This maybe because the obstacle avoidance approach is based on trying to maximize the distance betweenobstaclesandtheobject, andthisdominatesthesolutionnomatterhowbigorhowsmall theCHAPTER5. RESULTS 58OA Techniques Performance Smoothness Parameters Physical ExtensionTask Priority slowest fair o,h,dta,dug,dsoiyes fairCost Function fastest good no badObjective Function medium good (once for all) yes goodTable 5.1: The comparison of the three one-obstacle avoidance techniques.critical distance is. Since there is no stationary maximum critical distance for the system to settleinto, the result is not stable.5.4 ObstacleAvoidanceTechniquesComparisonIn Chapter 4 three major existing methods for avoiding one obstacle were discussed. Their originalsuggestedextensions toavoidingmultipleobstacles andmyownextensions fromacomputergraphics point of view are presented in Section 5.3.Table5.1isacomparisonoftheseone-obstacleavoidancetechniquesindierentcategories,including their performance, kinematic chain (the elephant trunk in my application) smoothnessafter applying these methods, parameters to be tuned by the user or automatically by the program,the existence of physical interpretation, and their availabilities to be extended to avoid multipleobstacles.Fromthetablewecanseethatalthoughtheobjectivefunctiontechniqueisnotthefastesttechnique, it produces a smooth kinematic chain, and its physical interpretation makes the tech-niqueeasilyextendibletothemultipleobstaclessituation. Thebestpartof thistechniqueisthattheuseronlyneedstoadjustasingleparameteronceatthebeginningandthencanusethisvalueforanynumberofobstaclesandanynumberofdegreesoffreedom. Whilethecostfunctionapproachhasthehighestperformanceamongthethree, ithasnoreasonablephysicalinterpretation, whichmakesithardtoextendittoavoidingmultipleobstacles. Ontheotherhand, while the task priority approach has a clear physical interpretation, it is slow and has moreparameters1to be tuned than other methods. Another disadvantage of the task priority approach1Thereisalsoanotherdampingfactorforthepseudoinverseforallthreemethods. Howeveritdoesnotaecttheobstacleavoidancethatmuch.CHAPTER5. RESULTS 59OA Techniques Performance Smoothness Parameters Physical ObstaclesOriginal Task Pri-orityslow fair dta, dug, dsoi,o,hyes 2Task AugmentedTask Priority *slow fair dta, dug, dsoi,o,hno < # of DOFsOriginal CostFunctionfast good no noneTask AugmentedCost Function *fast good no < # of DOFsBlending * fast good no noneBlended TaskAugmented CostFunction *fast good no < # of DOFsAdaptive CostFunction *fast good , no noneOriginal Objec-tive Functionmedium good (once for all) yes noneTable 5.2: The comparison of multiple obstacles avoidance techniques. The techniques with * aremy extensions to the original approaches in the papers.is that it sometimes may generate a kinked kinematic chain as shown in Figure 5.5.Althoughroboticscientistsandengineersdevelopedtheabovethreetechniquesforavoidingone obstacle, most of them also suggested on how to extend their algorithm to avoiding multipleobstacles. By analyzing and implementing these extensions, I also came up with my own extensionsthat is more suitable for the more general computer graphics applications. Table 5.2 summarizesthecomparisons amongthem. Theperformancespeeds of thesemultipleobstacleavoidancealgorithms are not compared quantitatively as I did for SVD algorithms, because besides speed,thereareotherimportantissuessuchasnumberof parameterstobetunedwhilerunningtheprogram and the limitation on the number of obstacles.From Table 5.2 we can see that the original extension of the task priority technique has toomany uncertain parameters and it runs slowly. The task space augmentation is not a good idea foravoiding an arbitrary number obstacles, since it sets a upper limit on the number of obstacles, andit also involves heavy matrix computations when the number of obstacles is close to the number ofdegrees of freedom in the kinematic chain. The original suggested extension of the cost functionCHAPTER5. RESULTS 60approach is simple and fast, but the parameter has to be tuned whenever there is a change inthe number of obstacles and the number of degrees of freedom in the kinematic chain. Based onthe original cost function, I created a new cost function called blending. Theoretically, the valueof doesnotneedtobetunedwhenthenumberofobstaclesischanged, butitdidnotworkthat way in my application. Then I decided to use a completely new adaptive cost function andtriedtoadjustitsparametersautomatically. Butthisadaptivecostfunctiontechniquedidnotgive me a satisfying result. After all these trials,I came back to the original objective functionmethod,and proved its physical interpretation in Section 4.1.3. Surprisely,this method gives agood obstacle avoidance behavior at a reasonable speed, and the user only needs to tune its singleparameter once at the beginning.5.5 DiscussionNot all of the results of the above methods were as good as expected. The main reason is that thesubspace projection operator is very sensitive and there were often too many uncertain parametersin the obstacle avoidance formulations. Taking the homogeneous gain term i in the task prioritytechnique(Equation5.2)orthefactor inthecostfunction(Equation5.3)ortheobjectivefunctionapproaches(Equation5.5)asexamples, tuningthesescalarshasproventobeanon-trivial issue [4]. These parameters give the weight of the obstacle avoidance velocity in the naljoint angle solution. If the weight is too strong, when the critical distance is small it will resultin some oscillations, in other words, there will be big jumps in the solution. On the other hand,if the weight is too weak, the change in the conguration will occur when the homogeneous termbecomes large with respect to the primary task, which may be too late and the manipulator maygo through the obstacles. Without appropriate parameters, the end eector will not even followthe goal position given by the mouse cursor.Even when those parameters are manually tuned for one special case, they have to be adjustedwhenthenumberofDOFsorthenumberofobstaclesischanged. Inmostcasesinroboticsorengineering, theyaresetbasedontrial anderror. ChaumetteandMarchand[4] developedanCHAPTER5. RESULTS 61automatic parameter tuning engine to adjust the inuence of the joint limits secondary goal withrespect to the positioning primary goal. However, its extension to other secondary goals such asobstacle avoidance still needs to be investigated.Generally, roboticscientistsandengineersuseo-linecomputationtointegratethevelocityoftheendeector, whichgivesthemanipulatoramuchmoreaccuratetrajectory. Sensorsareused to detect the critical distance between obstacles and the manipulator. Their techniques aretested with a small number of DOFs,fewer obstacles,and with no intention to get close to theobstacles. One more thing that needs to be pointed out is that most of their techniques are testedon 2D kinematic chains. Not much analysis is done in 3D.5.5.1 ObjectiveFunctionMethodFrommyexperiments, theobjectivefunctionmethodsuitscomputergraphicsapplicationsthemost. It does not require as expensive a computation as the task priority approach, which givesthe program higher performance. It also takes full advantage of the null space projection operatorandthecritical pointJacobian. IthasthemostconvenienceforausersincethereisnoneedtotunethegainfactorwhilechangingthenumberofobstaclesorthenumberofDOFsinthekinematic chain. Finally,there isthe possibilitythatinthe future theobjectivefunction couldbe extended to target additional secondary goals.In my program, when the objective function method is invoked to avoid obstacles, the elephanttrunk tip will follow the mouse cursor while the trunk itself will try to avoid obstacles. When thetrunk is about to go through an obstacle, the elephant trunk will get as close as possible to theobstacle rst, then keep its position there for next few frames even if the user insists on draggingthe trunk in that direction. If the user ignores the resistance of the trunk and continues draggingthe trunk through the obstacles, the primary positioning goal will win over the secondary obstacleavoidancegoal,andthetrunkwilljumpovertheobstaclestoreachitsnewposition. However,the trunk will never interpenetrate obstacles.Chapter6Conclusion6.1 SummaryWe have examined solutions to the inverse kinematics constraint problem for highly articulatedmodelsinthecontextofcomputergraphics. Thepseudoinversewasusedtosolvetheproblemrobustly and a singular value decomposition was applied to get the pseudoinverse for a Jacobianmatrix. Based on existing SVD algorithms, I developed a fast and simple incrementally approachto evaluate the SVD. This algorithm uses Givens rotations to orthogonalize the rows of a JacobianbasedontheresultofpreviousSVD.WiththesizeoftheJacobianmatricesinmyapplicationbeing3 n, wheren 3, row-wiseGivensrotationsmethodgivesaperformanceasfastandrobust as the highly optimized commercial numerical package LAPACK, but the algorithm itselfis much simpler.Various techniques were implemented and created to use the redundant degrees of freedom ofa highly articulated model to accomplish secondary obstacle avoidance goals besides the primaryend eector positioning inverse kinematic goal. After experimenting with several methods in thecourseof thiswork, onethingbecomeapparent: theyall exhibitproblemsof onetypeoran-other; no one approach seems uniformly superior to others with respect to performance measures.Dierentapproachesaresuitedtodierentapplications. Theobjectivefunctionmethodsuits62CHAPTER6. CONCLUSION 63the generalized applications characteristic of computer graphics the most. It gives relatively fastperformance with intuitive behavior and also has reasonably stable parameters.6.2 ConclusionFromthediscussionabovewecanconcludethatroboticandengineeringinversekinematicso-lutionsforconstrainedredundantsystemcanbeusedbycomputergraphicsapplicationswithappropriate porting. Even though those solutions are not designed for generalized problems suchas a kinematic chain with arbitrary number of degrees of freedom that can avoid arbitrary numberof obstacles in a complex environment,as long as they have clear physical interpretations,theycan usually be extended to reasonable solutions. However,dierent methods will have dierentperformancesunderdierentvaluesofparametersandindierentenvironments. Theseuncer-tainties are not wanted in computer graphics applications. A good solution should have relativelyhigh speed and few parameters to be tuned during changes to the environment and the kinematicchain itself.I did research on three major approaches in robotics and engineering on obstacle avoidance.Instead of blindly trying to extend these three methods to avoid multiple obstacles, I started fromanalyzing their physical interpretation and proving their correctness. Based on the interpretationI found dierent solutions to the generalized problem. Although some of the techniques did notgivegoodresults, thisresearchstillprovidesananalysisofwhytheyarenotsatisfying. Thesereasons include limitations on number of obstacles, lack of physical interpretations, kinks in thekinematic chains, and the disadvantages of having parameters to be tuned.The objective function method I implemented used the most common obstacle and manipulatordistance objective function. It gave a good result with relatively high performance. The best thingabout this method is that the user does not not need to tune its parameter during environmentchanges. This simple solution satises our computer graphics needs nicely, although there are stillsome disadvantages. The formulations of this metho