ADVANCES IN ROBOT KINEMATICS Advances in Robot Kinematics Mechanisms and Motion Edited by JADRAN LENAR I Jožef Stefan Institute Ljubljana, Slovenia and B ROTH Stanford University California, U.S.A A C.I.P Catalogue record for this book is available from the Library of Congress ISBN-10 ISBN-13 ISBN-10 ISBN-13 1-4020-4940-4 (HB) 978-1-4020-4940-8 (HB) 1-4020-4941-2 (e-book) 978-1-4020-4941-5 (e-book) Published by Springer, P.O Box 17, 3300 AA Dordrecht, The Netherlands www.springer.com Printed on acid-free paper All Rights Reserved © 2006 Springer No part of this work may be reproduced, stored in a retrieval system, or transmitted in any form or by any means, electronic, mechanical, photocopying, microfilming, recording or otherwise, without written permission from the Publisher, with the exception of any material supplied specifically for the purpose of being entered and executed on a computer system, for exclusive use by the purchaser of the work Printed in the Netherlands Preface This is the tenth book in the series of Advances in Robot Kinematics Two were produced as workshop proceedings, Springer published one book in 1991 and since 1994 Kluwer published a book every two years without interruptions These books deal with the theory and practice of robot kinematics and treat the motion of robots, in particular robot manipulators, without regard to how this motion is produced or controlled Each book of Advances in Robot Kinematics reports the most recent research projects and presents many new discoveries The issues addressed in this book are fundamentally kinematic in nature, including synthesis, calibration, redundancy, force control, dexterity, inverse and forward kinematics, kinematic singularities, as well as over-constrained systems Methods used include line geometry, quaternion algebra, screw algebra, and linear algebra These methods are applied to both parallel and serial multi-degree-of-freedom systems The results should interest researchers, teachers and students, in fields of engineering and mathematics related to robot theory, design, control and application All the contributions had been rigorously reviewed by independent reviewers and fifty three articles had been recommended for publication They were introduced in seven chapters The authors discussed their results at the tenth international symposium on Advances in Robot Kinematics which was held in June 2006 in Ljubljana, Slovenia The symposium was organized by Jozef Stefan Institute, Ljubljana, under the patronage of IFToMM - International Federation for the Promotion of Mechanism and Machine Science We are grateful to the authors for their contributions and for their efficiency in preparing the manuscripts, and to the reviewers for their timely reviews and recommendations We are also indebted to the personnel at Springer for their excellent technical and editorial support Jadran Lenarˇiˇ and Bernard Roth, editors cc Contents Methods in Kinematics J Andrade-Cetto, F Thomas Wire-based tracking using mutual information G Nawratil The control number as index for Stewart Gough platforms 15 C Innocenti, D Paganelli Determining the 3×3 rotation matrices that satisfy three linear equations in the direction cosines 23 P.M Larochelle A polar decomposition based displacement metric for a finite region of SE(n) 33 J.-P Merlet, P Donelan On the regularity of the inverse Jacobian of parallel robots 41 P Fanghella, C Galletti, E Giannotti Parallel robots that change their group of motion 49 A.P Murray, B.M Korte, J.P Schmiedeler Approximating planar, morphing curves with rigid-body linkages 57 M Zoppi, D Zlatanov, R Molfino On the velocity analysis of non-parallel closed chain mechanisms 65 Properties of Mechanisms H Bamberger, M Shoham, A Wolf Kinematics of micro planar parallel robot comprising large joint clearances 75 H.K Jung, C.D Crane III, R.G Roberts Stiffness mapping of planar compliant parallel mechanisms in a serial arrangement 85 viii Y Wang, G.S Chirikjian Large kinematic error propagation in revolute manipulators Contents 95 A Pott, M Hiller A framework for the analysis, synthesis and optimization of parallel kinematic machines 103 Z Luo, J.S Dai Searching for undiscovered planar straight-line linkages 113 X Kong, C.M Gosselin Type synthesis of three-DOF up-equivalent parallel manipulators using a virtual-chain approach 123 A De Santis, P Pierro, B Siciliano The multiple virtual end-effectors approach for human-robot interaction 133 Humanoids and Biomedicine J Babiˇ, D Omrˇen, J Lenarˇiˇ c c cc Balance and control of human inspired jumping robot 147 J Park, F.C Park A convex optimization algorithm for stabilizing whole-body motions of humanoid robots 157 R Di Gregorio, V Parenti-Castelli Parallel mechanisms for knee orthoses with selective recovery action 167 S Ambike, J.P Schmiedeler Modeling time invariance in human arm motion coordination 177 M Veber, T Bajd, M Munih Assessment of finger joint angles and calibration of instrumental glove 185 R Konietschke, G Hirzinger, Y Yan All singularities of the 9-DOF DLR medical robot setup for minimally invasive applications 193 G Liu, R.J Milgram, A Dhanik, J.C Latombe On the inverse kinematics of a fragment of protein backbone 201 V De Sapio, J Warren, O Khatib Predicting reaching postures using a kinematically constrained shoulder model 209 Contents ix Analysis of Mechanisms D Chablat, P Wenger, I.A Bonev Self motions of special 3-RPR planar parallel robot 221 A Degani, A Wolf Graphical singularity analysis of 3-DOF planar parallel manipulators 229 C Bier, A Campos, J Hesselbach Direct singularity closeness indexes for the hexa parallel robot 239 A Karger Stewart-Gough platforms with simple singularity surface 247 A Kecskemthy, M Tăndl e a A robust model for 3D tracking in object-oriented multibody systems based on singularity-free Frenet framing 255 P Ben-Horin, M Shoham Singularity of a class of Gough-Stewart platforms with three concurrent joints 265 T.K Tanev Singularity analysis of a 4-DOF parallel manipulator using geometric algebra 275 R Daniel, R Dunlop A geometrical interpretation of 3-3 mechanism singularities 285 Workspace and Performance J.A Carretero, G.T Pond Quantitative dexterous workspace comparisons 297 E Ottaviano, M Husty, M Ceccarelli Level-set method for workspace analysis of serial manipulators 307 M Gouttefarde, J -P Merlet, D Daney Determination of the wrench-closure workspace of 6-DOF parallel cable-driven mechanisms 315 G Gogu Fully-isotropic hexapods 323 P Last, J Hesselbach A new calibration stategy for a class of parallel mechanisms 331 M Krefft, J Hesselbach The dynamic optimization of PKM 339 x J.A Snyman On non-assembly in the optimal synthesis of serial manipulators performing prescribed tasks Contents 349 Design of Mechanisms W.A Khan, S Caro, D Pasini, J Angeles Complexity analysis for the conceptual design of robotic architecture 359 D.V Lee, S.A Velinsky Robust three-dimensional non-contacting angular motion sensor 369 K Brunnthaler, H.-P Schrăcker, M Husty o Synthesis of spherical four-bar mechanisms using spherical kinematic mapping 377 R Vertechy, V Parenti-Castelli Synthesis of 2-DOF spherical fully parallel mechanisms 385 G.S Soh, J.M McCarthy Constraint synthesis for planar n-R robots 395 T Bruckmann, A Pott, M Hiller Calculating force distributions for redundantly actuated tendon-based Stewart platforms 403 P Boning, S Dubowsky A study of minimal sensor topologies for space robots 413 M Callegari, M.-C Palpacelli Kinematics and optimization of the translating 3-CCR/3-RCC parallel mechanisms 423 Motion Synthesis and Mobility C.-C Lee, J.M Herv´ e Pseudo-planar motion generators 435 S Krut, F Pierrot, O Company On PKM with articulated travelling-plate and large tilting angles 445 C.R Diez-Mart´ ınez, J.M Rico, J.J Cervantes-S´nchez, a J Gallardo Mobility and connectivity in multiloop linkages 455 K Tcho´, J Jakubiak n Jacobian inverse kinematics algorithms with variable steplength for mobile manipulators 465 Contents xi J Zamora-Esquivel, E Bayro-Corrochano Kinematics and grasping using conformal geometric algebra 473 R Subramanian, K Kazerounian Application of kinematics tools in the study of internal mobility of protein molecules 481 O Altuzarra, C Pinto, V Petuya, A Hernandez Motion pattern singularity in lower mobility parallel manipulators 489 Author Index 497 Methods in Kinematics J Andrade-Cetto, F Thomas Wire-based tracking using mutual information G Nawratil The control number as index for Stewart Gough platforms 15 C Innocenti, D Paganelli Determining the 3×3 rotation matrices that satisfy three linear equations in the direction cosines 23 P.M Larochelle A polar decomposition based displacement metric for a finite region of SE(n) 33 J.-P Merlet, P Donelan On the regularity of the inverse Jacobian of parallel robots 41 P Fanghella, C Galletti, E Giannotti Parallel robots that change their group of motion 49 A.P Murray, B.M Korte, J.P Schmiedeler Approximating planar, morphing curves with rigid-body linkages 57 M Zoppi, D Zlatanov, R Molfino On the velocity analysis of non-parallel closed chain mechanisms 65 460 C.R Diez-Martínez et al D F E C A J B G Figure H K Graph of the kinematic chain proposed by Fayet and used by Wohlhart From this graph, and following the loops, it is possible to find the velocity analysis equation for the kinematic chain given, in matrix form, by J ω = or ⎡ 5a $ ⎢ 5a $5b ⎢ ⎢ 5b $4 ⎢ ⎢ $4a ⎢ ⎢ 4a 4b ⎢ $ ⎢ 4b ⎢ ⎢ $2 ⎢ $ ⎢ ⎢ $2a ⎢ ⎢ 2a $1 ⎢ ⎢ 1a ⎢ $ ⎢ 1a ⎢ $ ⎢ ⎢ $0 ⎢ ⎢ $ ⎢ ⎢ $ ⎢ ⎢ ⎢ $0 ⎢ ⎢ $0 ⎢ ⎢ $0 ⎢ ⎢ $0 ⎢ ⎣ $0 $0 − $5a − 5a $5b − 5b $4 $0 $0 $0 $0 $0 $0 − $1a − 1a $5 $1b 1b $1c 1c $6 $6a 6a $4 $0 $0 $0 $0 $0 $0 $0 − $4a − 4a $4b − 4b $3 $0 $0 $0 $0 $0 $0 $0 $0 − $6a − 6a $4 $8 $7 $7a 7a $3 ⎤T ⎡ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ω5a 5a ω5b 5b ω4 ω4a 4a ω4b 4b ω3 ω2 ω2a 2a ω1 ω1a 1a ω5 ω1b 1b ω1c 1c ω6 ω6a 6a ω4 ω8 ω7 ω7a 7a ω3 ⎤ ⎡ ⎤ ⎥ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ = ⎢ ⎥, ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎢ ⎥ ⎥ ⎣ ⎦ ⎥ ⎦ (7) 461 Mobility and Connectivity where $0 is a six dimensional vector whose elements are all equal to zero The solution of the velocity analysis solution, given by Eq 7, is found to be ω5a = 5b ω4 = 4a ω4b = ω2a = 1c ω6 = 6a ω4 = ω7a = ω8 = 0, − 4b ω3 , − ω1b − 4b ω3 , − ω1a , − 4b ω3 , ω1b + 4b ω3 , ω1a − 1b ω1c − 4b ω3 − ω1b , ω1b , 5a ω5b = ω4a = = 2a ω1 = ω6a = ω7 = 7a ω3 = ω2 ω1b + 4b ω3 − 1a ω5 , 0, ω1b + 4b ω3 , − ω1b − 4b ω3 , ω1a − 1b ω1c , 4b ω3 , − ω1b , where, ω1b , 1b ω1c , ω1a , 1a ω5 and 4b ω3 can be selected arbitrarily Therefore, the first order mobility is, M1 = Furthermore, the first order connectivity matrix is given by ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ I C = ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 2 3 3 ⎤ ⎥ 2 3 ⎥ ⎥ ⎥ 2 3 ⎥ ⎥ ⎥ 2 2 2 ⎥ ⎥ ⎥ ⎥ 2 2 3 ⎥ (8) ⎥ ⎥ 3 ⎥ ⎥ ⎥ 3 2 ⎥ ⎦ 3 3 1 The acceleration analysis equation has a solution, if and only if, the augmented matrix, Ja , obtained by augmenting the coefficient matrix, T J, with the column given by LS = $L1 $L2 $L3 , or Ja = J LS (9) where, $L1 , $L2 and $L3 are the Lie screws of the three loops of the kinematic chain, satisfy the condition Rank(Ja ) = Rank(J) (10) This condition yields 4b ω3 = ω1b = (11) Therefore, the solution of the velocity analysis that takes into consideration the acceleration analysis condition is given by Eq 11 and the 462 C.R Diez-Martínez et al following additional results ω5a ω4a ω2a ω6a ω7a = = = = = 0, 0, − ω1a , ω1a − 1b ω1c , ω1a − 1b ω1c , 5a ω5b 4a ω4b 2a ω1 6a ω4 7a ω3 = = = = = − 1a ω5 , 0, 0, 0, 0, 5b ω4 = = 1c ω6 = ω7 = ω8 = ω2 0, 0, 0, 0, 0, Thus, only ω1a , 1a ω5 and 1b ω1c can be arbitrarily selected Hence, the second order infinitesimal mobility is M2 = Further, the second order infinitesimal mobility matrix is given by ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ C II = ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 1 ⎤ ⎥ 0 1 1 ⎥ ⎥ ⎥ 0 1 1 ⎥ ⎥ ⎥ 0 1 1 ⎥ ⎥ ⎥ ⎥ 1 2 ⎥ (12) ⎥ ⎥ 1 1 0 ⎥ ⎥ ⎥ 1 1 0 ⎥ ⎦ 1 1 0 Finally, it will be shown that the results obtained up to this point, allow to determine the finite mobility and connectivity of this multiloop kinematic chain For that purpose, recent results on the mobility of parallel platforms, Rico et al., 2005, will be adapted for the task The infinitesimal mechanical liaisons associated with all the possible four paths, I, II, III, IV , between links 1, regarded as the fixed platform, and 5, regarded as the moving platform, are the column spaces of the matrices 1V I = $1a 1a $5 1V III = $2a 2a $2 $2 4b $3 4a $4b 4a $4 5b $4 5a $5b $5a VIV = 1V II , $1b 1b $1c 1c $6 $6a 6a $4 $5b 5a $5b $5a = , , 1b 1b 1c 1c 6 8 7 7a 7a 4b 4a 4b 4a 5b 5a 5b 5a $ $ $ $ $ $ $ $ $ $ $ $ $ The absolute mechanical liaison is given by 5 5 Va5 = V I ∩ V II ∩ V III ∩ V IV = 0 0 0 0 T (13) Mobility and Connectivity 463 It is easy to recognize Va5 as the subalgebra, of the Lie algebra of the Euclidean group, se(3), associated with cylindrical displacements along the y axis This result accounts for two of the degrees of freedom, from the three determined by the second order infinitesimal mobility They are the finite displacements associated with the screws $1a and 1a $5 located in point E The remaining degree of freedom is related to the translational motion, along the same axis y, and produced by the screws 1b $1c , located in point C, and $7a , located in point J, while the revolute joints located between them remain inactive This degree of freedom is passive when the fixed and moving platforms are links and The conclusion is that the finite mobility of the multiloop linkage is MF = M2 = Therefore, the finite connectivities among the diferent links, CF (i, j) are given by the elements of the second order infinitesimal connectivity matrix C II Conclusions This contribution has shown that it is possible to provide higher-order definitions of infinitesimal mobility and connectivity that are congruent with the usual definitions of finite mobility and connectivity They provide a guide for the computation of finite mobility of general multiloop linkages, this is, in the opinion of the authors, the most difficult task in mobility computations The results have been verified using Adams c Acknowledgements The first author thank Conacyt for the support of his M Sc studies The authors thank Concyteg for the support of several projects, including a thesis scholarship for the first author This work is based on his M Sc thesis at the Instituto Tecnol´gico de Celaya o References Baker, J E (1981), On Mobility and Relative Freedoms in Multiloop Linkages and Structures, Mechanism and Machine Theory, vol 16, no 6, pp 583-597 Bentley, D L., and Cooke, K L (1971), Linear Algebra with Differential Equations, New York: Holt, Rinehart and Winston Fayet, M (1995), M´canismes Multi-Boucles I D´termination des Espaces de Torseurs e e Cin´matiques Dans un M´canisme Multi-Boucles Quelconque, Mechanism and Mae e chine Theory, vol 30, no 2, pp 201-217 Galletti, C., and Fanghella, P (2001), Single-Loop Kinematotropic Mechanisms,Mechanism and Machine Theory, vol 36, no 6, pp 743-761 Gogu, G (2005), Mobility of Mechanisms: a Critical Review, Mechanism and Machine Theory, vol 40, no 9, pp 1068-1097 464 C.R Diez-Martínez et al Rico, J M., Gallardo, J., and Duffy, J (1999), Screw Theory and Higher Order Analyses of Open Serial and Closed Chains, Mechanism and Machine Theory, vol 34, no 4, pp 559-586 Rico, J M., Aguilera, L D., Gallardo, J., Rodriguez, R., Orozco, H., and Barrera, J M (2006), A More General Mobility Criterion for Parallel Platforms, ASME Journal of Mechanical Design, vol 128, no 1, pp 207-219 Wohlhart, K (1999), Degrees of Shakiness, Mechanism and Machine Theory, vol 34, no 7, pp 1103-1126 Wohlhart, K (2000), Architectural Shakiness or Architectural Mobility of Platforms, On Advances in Robot Kinematics, Lenarˇiˇ, J and Staniˇi´, eds Dordrecht: cc sc Kluwer Academic Publishers, pp 365-374 Wohlhart, K (2004), Screw Spaces and Connectivities in Multiloop Linkages, On Advances in Robot Kinematics, Lenarˇiˇ, J and Galletti, C eds Dordrecht: Kluwer cc Academic Publishers, pp 97-104 JACOBIAN INVERSE KINEMATICS ALGORITHMS WITH VARIABLE STEPLENGTH FOR MOBILE MANIPULATORS Krzysztof Tcho´ n Institute of Computer Engineering, Control and Robotics Wroclaw University of Technology tchon@ict.pwr.wroc.pl Janusz Jakubiak Institute of Computer Engineering, Control and Robotics Wroclaw University of Technology Janusz.Jakubiak@pwr.wroc.pl Abstract We study Jacobian inverse kinematics algorithms for mobile manipulators composed of a nonholonomic mobile platform and a holonomic onboard manipulator In general, the Jacobian algorithms converge locally, often producing weird end effector and platform trajectories In the paper we use the existing theory of Newton algorithms in order to improve the quality of convergence of the Jacobian algorithms Specifically, we examine a strategy of adjusting the steplength in the Jacobian pseudoinverse algorithm that results from the affine covariant Lipschitz condition imposed on the mobile manipulator’s Jacobian The affine covariant strategy is verified by extensive computer simulations and compared with the constant length step and a simple Armijo strategies Keywords: Mobile manipulator, inverse kinematics, Jacobian algorithm Introduction In this paper by a mobile manipulator we call a robotic system composed of a nonholonomic mobile platform and a stationary manipulator fixed to the platform The nonholonomic constraints imposed on the platform and the holonomic kinematics of the onboard manipulator are modelled as a driftless control system with outputs The end-point map of this system determines the kinematics of the mobile manipulator Tcho´ and Jakubiak, 2003 The Jacobian inverse kinematics algorithm n for mobile manipulators are conveniently devised using the continua465 J Lenar i and B Roth (eds.), Advances in Robot Kinematics, 465–472 © 2006 Springer Printed in the Netherlands 466 h K Tchon and J Jakubiak tion method Chitour and Sussmann, 1998; Divelbiss et al., 1998 In this framework, an inverse kinematics algorithm is determined by a dynamic system operating in an inifinite dimensional Hilbert configuration space Computable inverse kinematics algorithms make use of a Ritz approximation of platform controls by truncated orthogonal series The Jacobian algorithms usually have local convergence and often produce unrealistic platform and end effector trajectories Tcho´ and Jakubiak, n 2003 The fact that inverse kinematics algorithms for stationary manipulators and mobile platforms are of the Newton type is well known, see e.g Isobe et al., 1992; Divelbiss et al., 1998; Duleba and Sasiadek, 2003 In particular in Divelbiss et al., 1998 the line searching has been mentioned as a method of adjusting the steplength of the inverse kinematics algorithm for a mobile platform Motivated by these circumstances in this paper we are attempting at improving the quality of convergence of Jacobian inverse kinematics algorithms by appropriate steplength adjustment Our approach relies on existing results for Newton algorithms Deulfhard, 2004 We consider a general Jacobian inverse kinematics algorithm associated with a given right inverse of the Jacobian Then we formulate the affine covariant condition and obtain a prediction-correction strategy of the steplength adjustment This strategy is applied to a mobile manipulator composed of a kinematic car platform and an RTR onboard manipulator, driven by the inverse kinematics algorithm based on the Jacobian pseudoinverse The affine covariant strategy is examined by computer simulations and compared with two other strategies Our conclusions are twofold First: the variable steplength improves both the speed and the quality of convergence of the Jacobian pseudoinverse algorithm in comparison to the case of constant steplength Second, although generally profitable, the affine covariant strategy is sometimes overpassed by a simple Armijo choice of the steplength from among a finite set of possibilities The composition of this paper is the following Section recalls basic concepts of the endogenous configuration space approach Section presents the affine covariant steplength adjustment strategy for a general Jacobian inverse kinematics algorithm This strategy is specified to the Jacobian pseudoinverse algorithm and examined by computer simulations in section The paper is concluded with section Basic Concepts We shall study mobile manipulators whose kinematics are represented by a driftless control system with outputs, q = G(q)u = ˙ m i=1 gi (q)ui , y = k(q, x) (1) 467 Jacobian Inverse Kinematics Algorithms In (1) q ∈ Rn , x ∈ Rp and y ∈ Rr denote, respectively, generalized coordinates of the platform, joint positions of the manipulator, and the end effector location in the taskspace The control functions u(t) of the platform and joint positions x of the onboard manipulator, acting over a control time horizon T > 0, define the endogenous configuration space X = L2 [0, T ] × Rp The space X equipped with the inner product m T < (u1 (·), x1 ), (u2 (·), x2 ) >= uT (t)u2 (t)dt+xT x2 becomes an infinitely 1 dimensional Hilbert space with induced norm ||(u(·), x)||2 = >(u(·), x), (u(·), x) > (2) An endogenous configuration (u(·), x) ∈ X determines a platform trajectory q(t) = ϕq0 , t (u(·)) and an end effector trajectory y(t) = k(q(t), x) The instantaneous kinematics Kq0 ,T : X → Rr of the mobile manipulator are defined as the end-point map of (1), i.e Kq0 ,T (u(·), x) = y(T ) = k(ϕq0 ,T (u(·)), x) (3) The kinematics (3) define end effector positions and orientations of the mobile manipulator steered by the control (u(·), x) that can be reached at T By differentiation of the kinematics Kq0 ,T (u(·), x) we obtain the analytic Jacobian T Jq0 ,T (u(·), x)(v(·), w) = C(T, x) Φ(T, s)B(s)v(s)ds+D(T, x)w (4) The Jacobian may be interpreted as the output reachability map at T of the linear approximation ˙ ξ = A(t)ξ + B(t)v, η = C(t, x)ξ + D(t, x)w, (5) to system (1) along the control-state trajectory (u(t), x, q(t)), initialized at ξ0 = The matrices appearing in (5) have the standard ∂ form A(t) = ∂q (G(q(t))u(t)), B(t) = G(q(t)), C(t, x) = ∂k (q(t), x), ∂q ∂k D(t, x) = ∂x (q(t), x), while matrix Φ(t, s) in (4) satisfies the evolution ∂ equation ∂t Φ(t, s) = A(t)Φ(t, s) with initial condition Φ(s, s) = In Let us recall that an endogenous configuration is regular, if the analytic Ja cobian Jq 0,T (u(·), x) : X →Rr is surjective This is equivalent to the full rankness of the dexterity matrix Dq0 ,T (u(·), x) = T Φ(T, s)B(s)B T(s)ΦT (T, s)ds C T(T, x) + D(T, x)DT (T, x); C(T, x) 468 h K Tchon and J Jakubiak otherwise (u(·), x) is referred to as singular The inverse kinematic problem for the mobile manipulator (1) consists in determining an endogenous configuration (ud (·), xd ) such that for a given location yd of the end effector Kq0 ,T (ud (·), xd ) = yd Jacobian inverse kinematics algorithms for mobile manipulators are conveniently derived using the continuation method Among them, the most often used is the Jacobian pseudoinverse algorithm Tcho´ and Jakubiak, 2003; its alternative is n the extended Jacobian algorithm Tcho´ and Jakubiak, 2005 The Jacon bian pseudoinverse algorithm relies on the Jacobian pseudoinverse # (Jq0 ,T (u(·), x)η)(t) = B T (t)ΦT (T, t)C T (T, x) −1 Dq0 ,T (u(·), x)η DT (T, x) and assumes the following form d dθ uθ (t) x(θ) # = −γJq0 ,T (uθ , x(θ))e(θ))(t), (6) where γ > denotes the steplength, and e(θ) = Kq0 ,T (uθ (·), x(θ)) − yd is the taskspace error For the algorithm exploits the inversion of the dexterity matrix, it is defined only for regular configurations of the mobile manipulator The dynamic system (6) produces a trajectory (uθ (·), x(θ)) ∈ X parameterized by θ ∈ R A solution (ud (t), xd ) to the inverse kinematic problem is computed as a limit (ud (t), xd ) = limθ→+∞ (uθ (t), x(θ)) Steplength Adjustment Let K = Kq0 ,T : X −→ Y defined by (3) describe the kinematics of a mobile manipulator For simplicity of notation, in this section we shall denote endogenous configurations by z = (u(·), x) and omit subscripts At a regular configuration z ∈ X the Jacobian (4) has a right inverse D K # (z) : Rr → X that determines an associated Jacobian inverse kinematics algorithm d z(θ) (7) = −γD K # (z(θ))e(θ), z(0) = z0 dθ As before, the solution of the inverse kinematic problem is obtained as # a limit zd = limθ→ +∞ z(θ) In particular, when D K # = Jq0 ,T , the algorithm (7) coincides with the Jacobian pseudoinverse algorithm (6) After discretization the algorithm (7) becomes a Newton algorithm z(k + 1) = z(k) − γD K # (z(k))e(k), k = 0, 1, 2, (8) The quantity D K # (z(k))e(k) appearing in (8) is called the Newton direction at the step k It is known that the Newton direction guarantees Jacobian Inverse Kinematics Algorithms 469 a gradual decrease of the taskspace error from step to step, provided that the steplength γ is chosen properly Relying on Deulfhard, 2004 we shall consider two strategies of the steplength choice in (8) The first strategy is called the Armijo strategy that consists in defining for a fixed integer m a set Γm = {1, , , 21 } of candidate steplengths and choosing at m every step of the algorithm γ = argminα∈Γm ||K(z − αDK # (z)e) − yd ||2 A more sophisticated strategy may be based on the affine covariant Lipschitz condition imposed on D K, ||D K # (z)(D K(y) − D K(w))v|| ≤ ω||y − w||||v||, (9) where ||·|| denotes the norm (2) in X , v, w, y, z ∈ X , and ω > is a Lipschitz constant It may be checked that in a singularity-free region of the endogenous configuration space the affine covariant condition is satisfied However, for the reason that usually the Lipschitz constant ω in (9) is not available, the computation of γ relies on some estimates achieved in a prediction-correction procedure Deulfhard, 2004, culminating in the following algorithm of the steplength adjustment The algorithm is ini0 tiated at z0 with γ1 = At the step k the algorithm returns z (k), (k) = K(z (k)) − y , and makes a prediction γ Then computes e d k the predicted value of γk is subject to a series of corrections Suppose that after the ith correction step the algorithm proposes a steplength i i γk Then the algorithm finds z i (k) = z (k) − γk D K # (z (k))e0 (k), comi (k) = K(z i (k)) − y , and accomplishes the (i + 1)st correction putes e d i+1 i as γk = min{γk /2, 1/hi+1 }, where k hi+1 = k i 2||D K # (z (k))ei (k) − (1 − γk )D K # (z (k))e0 (k)|| i (γk )2 ||D K # (z (k))e0 (k)|| These corrections are made a preassumed number of times yielding an i∗ i∗ i∗ such that γk = argmin i ||ei (k)||2 Next, given γk the algorithm sets ∗ z = z (k+1) = z i (k), e0 = e0 (k+1) = K(z (k+1))−yd , and computes a prediction for the step k + as γk+1 = min{1, 1/h0 }, where k+1 h0 = k+1 ||D K # (z )(D K(z ) − D K(z (k)))D K # (z )e0 || i∗ γk ||D K # (z (k))e0 (k)|| In the next section we shall apply the above procedure to the Jacobian pseudoinverse algorithm (6) driving a specific mobile manipulator Computer Simulations In simulations we shall use a Ritz approximation of platform control functions by truncated Fourier series: u1 (t) = λ10 + λ1 2j−1 sin jωt+ j=1 470 h K Tchon and J Jakubiak λ1 2j cos jωt, u2 (t) = λ20 + λ21 sin ωt + λ22 cos ωt, where ω = 2π For T a suitable matrix P (t) the control can be written as u(t) = P (t)λ, where the control parameters λ = (λ10 , λ14 , λ20 , λ21 , λ22 ) ∈ R8 The Ritz approximation defines a finite-dimensional, band-limited endoge˜ = nous configuration space X ∼ R11 , and yields the band-limited kine˜ q ,T (λ, x) and the band-limited analytic Jacobian matics K ˜ ˜ Jq0 ,T (λ, x) = C(T, x) T ˜ ˜ ˜ Φ(T, s)B(s)P (s)ds, D(T, x) In consequence, a discrete band-limited Jacobian pseudoinverse inverse kinematics algorithm (6) will take the following form λk+1 xk+1 = λk xk ˜T ˜ −1 − γ Jq0 ,T (λk , xk )Dq0 ,T (λk , xk )˜k , e (10) ˜ ˜ ˜T ˜ where e = Kq0 ,T (λ, x) − yd and Dq0 ,T (λ, x) = Jq0 ,T (λ, x)Jq0 ,T (λ, x) ˜ The inverse kinematics algorithm (10) has been applied to a mobile manipulator composed of a kinematic car platform carrying an RTR manipulator, portrayed in Figure The vector q = (q1 , q2 , q3 , q4 ) = Figure RTR manipulator mounted on kinematic car (x, y, ϕ, ψ) ∈ R4 includes the position and the orientation of the platform and the heading angle of its front wheels The vector x = (x1 , x2 , x3 ) ∈ R3 denotes joint positions of the onboard manipulator Cartesian positions of the end effector y = (y1 , y2 , y3 ) ∈ R3 serve as taskspace coordinates The representation (1) of kinematics, excluding the side-slip of platform wheels, takes the following form ˙ ˙ ˙ q1 = u1 cos q3 cos q4 , q2 = u1 sin q3 cos q4 , q3 = u1 sin q4 , q4 = u2 , ˙ y = (q1 + L cos(q3 + x1 ), q2 + L sin(q3 + x1 ), x2 + l3 sin x3 ), where L = l2 + l3 cos x3 In simulations the length of the car l = 1, the link lengths of the onboard manipulator l2 = 0.5, l3 = 1, and the control 471 Jacobian Inverse Kinematics Algorithms time horizon T = The initial platform coordinates q0 = (0, 10, 0, 0), the initial joint positions x0 = (0, 2, π/4), the desired taskspace point yd = (0, 0, 1) The algorithm has been initiated at diverse λ10 , λ20 lying in a square [−100, +100]2 , while remaining λij = Table displays the number of steps after which yd has been reached within the accuracy of 10−6 The symbol > 2000 means that within 2000 iterations the convergence has not been observed For comparison, the Jacobian pseudoinverse algorithm has been examined with constant steplength γ = and with the steplength governed by the Armijo strategy with m = Figure shows platform and end effector trajectories obtained for λ20 = −λ10 = 0.01 using the constant step and the affine covariant strategies Conclusions Assuming the affine covariant Lipschitz condition we have obtained a strategy of steplength adjustment in Jacobian inverse kinematics algorithms for mobile manipulators This strategy has been implemented for the Jacobian pseudoinverse algorithm and examined by computer simulations The examination has demonstrated that using a variable steplength improves both the speed and the quality of convergence of Table Convergence of the Jacobian pseudoinverse algorithm λ10 λ20 γ=1 Arm cov λ10 λ20 γ=1 Arm cov 100 10 0.1 0.01 0.001 –100 –10 –1 – 0.1 – 0.01 – 0.001 100 10 0.1 0.01 0.001 100 10 0.1 0.01 0.001 – 100 – 10 –1 – 0.1 – 0.01 – 0.001 –100 – 10 –1 – 0.1 – 0.01 – 0.001 136 11 100 295 152 40 126 26 138 241 115 177 509 78 75 801 318 120 14 11 27 25 28 32 160 1158 60 22 86 427 310 573 26 61 40 30 13 11 29 113 52 57 >2000 117 20 47 54 46 344 142 1538 78 58 62 –100 –10 –1 – 0.1 – 0.01 – 0.001 –100 – 10 –1 – 0.1 – 0.01 – 0.001 100 10 0.1 0.01 0.001 100 10 0.1 0.01 0.001 0 0 0 0 0 0 17 37 276 249 1307 830 90 90 91 101 205 332 189 353 194 64 171 207 22 70 132 331 211 33 27 334 302 30 40 29 360 29 76 70 90 247 16 40 94 65 59 58 62 145 26 89 58 55 62 167 79 90 58 56 K Tcho and J Jakubiak 472 Figure Trajectory for constant steplength (left) vs affine covariant (right) the algorithm in comparison to the case of constant steplength When the prediction-correction procedure produces very small steplenghts the affine covariant strategy may be overpassed by a simple Armijo strategy The fact that no lost of convergence has been observed along with the result of Chitour and Sussmann, 1998 on global convergence of the Jacobian pseudoinverse algorithm for the mobile platform from Figure may suggest that the examined algorithm also converges globally Acknowledgements This research was supported by the Foundation for Polish Science References Chitour, Y., and Sussmann, H J (1998), Motion planning using the continuation method, In: Essays on Mathematical Robotics, ed by J Baillieul, S S Sastry and H J Sussmann, Springer-Verlag, New York, pp 91-125 Deuflhard, P (2004), Newton Methods for Nonlinear Problems, Springer-Verlag, Berlin, 2004 Divelbiss, A., Seereeram, S., and Wen, J.T (1998), Kinematic path planning for robots with holonomic and nonholonomic constraints In: Essays on Mathematical Robotics, ed by J Baillieul, S S Sastry and H J Sussmann, Springer-Verlag, New York, pp 127-150 Duleba, I., and Sasiadek, J Z (2003), Nonholonomic motion planning based on Newton algorithm with energy optimization, IEEE Trans Contr Syst Technology, vol 11, no 3, pp 355-363 Isobe, T., Nagasaka, K., and Yamamoto, S (1992), A new kinematic control of simple manipulators, IEEE Trans Syst., Man, Cybernet., vol 22, no 5, pp 1116-1124 Tcho´, K., and Jakubiak, J (2003), Endogenous configuration space approach to n mobile manipulators: a derivation and performance assessment of Jacobian inverse kinematics algorithms, Int J Control, vol 76, no 14, pp 1387-1419 Tcho´, K., and Jakubiak, J (2005), A repeatable inverse kinematics algorithm with n linear invariant subspaces for mobile manipulators, IEEE Trans Syst., Man, Cybernet., – Part B Cybernetics, vol 35, no 5, pp 1051-1057 KINEMATICS AND GRASPING USING CONFORMAL GEOMETRIC ALGEBRA Julio Zamora-Esquivel and Eduardo Bayro-Corrochano Center of research and advanced studies of IPN, Unit Guadalajara Geovis Laboratory Jalisco, Mexico jzamora,edb@gdl.cinvestav.mx Abstract In this paper we introduce the conformal geometric algebra in the field of robot grasping It help us to tackle problems of object modelling, hand kinematics and vision system using a unifying geometric language We present an grasp algorithm using velocity control Introduction As the technology advances new mechanisms emerge that enable us interact with a wider variety of objects, in this way new challenges related to object grasping planning arises There are many approaches to deal with this kind of problems based in shape primitives [Miller, 2003], forces equilibrium [Borst, 1999] for cite two examples We are working in how to obtain the optimal grasping points by means of mathematical modelling of the object and the manipulator In this manner we also obtain the hand (manipulator) pose Conformal geometric Algebra has been used in this work due to its powerful geometric representation and algebraic richness of which helps greatly in the process of modelling mechanical structures like the Barrett HandT M The Geometric Algebra of n-D Space In this paper we will specify a geometric algebra Gn of the n dimensional space by Gp,q,r , where p, q and r stand for the number of basis vector which squares to 1, and respectively and fulfill n = p + q + r We will use ei to denote the vector basis i In a Geometric algebra Gp,q,r , the geometric product of two basis vector is defined as ei ej = −1 ei ∧ ej f or f or f or f or i = j ∈ 1, · · · , p i = j ∈ p + 1, · · · , p + q i = j ∈ p + q + 1, · · · , p + q + r i=j Geometric algebra G4,1 can be used to treat conformal geometry in a very elegant way In this geometry an Euclidean vector space R3 473 J Lenar i and B Roth (eds.), Advances in Robot Kinematics, 473–480 © 2006 Springer Printed in the Netherlands 474 J Zamora-Esquivel and E Bayro-Corrochano is represented in R4,1 This space has orthonormal vector basis given by {ei } and eij = ei ∧ ej are bivectorial basis where e23 , e31 and e12 correspond to the Hamilton basis The unit Euclidean pseudo-scalar Ie := e1 ∧ e2 ∧ e3 , a pseudo-scalar Ic := Ie E and the bivector E := e4 e5 The conformal geometry is related to a stereographic projection in Euclidean space A stereographic projection is a mapping taking points lying on a hypersphere to points lying on a hyperplane In this case, the projection plane passes through the equator and the sphere is centered at the origin To make a projection, a line is drawn from the north pole to each point on the sphere and the intersection of this line with the projection plane constitutes the stereographic projection The points are mapped to a point of conformal geometry by x = x + x e∞ + eo More information about the conformal geometry in [Hestenes, 2001] Barrett Hand Forward Kinematics The direct kinematics involves the computation of the position and orientation of the robot end-effector given the parameters of the joints The direct kinematics can be easily computed provided that the lines of the axes of screws are given for more information [Bayro, 2000] In order to introduce the kinematics of the Barrett HandT M , we show the kinematic of one finger, in this example we will assume that the finger is totally extended Note that such an hypothetical position is not reachable in normal operation We points x1o , x2o and x3o to describe the position of each union and the end of the finger in the Euclidean space, see the figure Here we use Aw , A1,2,3 and Dw as the dimension of the finger’s components x1o = Aw e1 + A1 e2 + Dw e3 , x2o = Aw e1 + (A1 + A2 )e2 + Dw e3 , x3o = Aw e1 + (A1 + A2 + A3 )e2 + Dw e3 Figure Barrett hand hypothetical position (1) (2) (3) ... Unfortunately, this kind of systems are very expensive, their J Lenar i and B Roth (eds.), Advances in Robot Kinematics, 3–14 © 2006 Springer Printed in the Netherlands 4 J Andrade-Cetto and F Thomas... parameters which satisfies Eq (5) and has infinite magnitude Finding all real solutions of Eq (5) – both finite and at infinity – has been thus reduced to determining all real finite solutions of Eq (6),... Bobrow and Park, 1995 and Martinez and Duffy, 1995 However, a metric that is independent of these choices, 33 J Lenarcic and B Roth (eds.), Advances in Robot Kinematics, 33 40 © 2006 Springer Printed