3 -28 Robotics and Automation Handbook /* This is the forward reduction. */ for(i=0;i<n;i++) { coef = a[i][i]; for(j=n-1;j>=0;j ) { y[i][j] /= coef; a[i][j] /= coef; } for(k=i+1;k<n;k++) { coef = a[k][i]/a[i][i]; for(j=n-1;j>=0;j ) { y[k][j] -= coef*y[i][j]; a[k][j] -= coef*a[i][j]; } } } /* This is the back substitution. */ for(i=n-1;i>=0;i ) { for(k=i-1;k>=0;k ) { coef = a[k][i]/a[i][i]; for(j=0;j<n;j++) { y[k][j] -= coef*y[i][j]; a[k][j] -= coef*a[i][j]; } } } return y; } B.6 File ‘‘matrixproduct.c’’ /* This file is "matrix product.c". * * This file multiples a and b (both square, n by n matrices) and * returns a pointer to the matrix c. * * Copyright (C) 2003 Bill Goodwine. * */ double** matrixproduct(double **a,double **b,double **c,int n) { int i,j,k; for(i=0;i<n;i++) for(j=0;j<n;j++) c[i][j] = 0.0; Copyright © 2005 by CRC Press LLC Inverse Kinematics 3 -29 for(i=0;i<n;i++) for(j=0;j<n;j++) for(k=0;k<n;k++) c[i][j] += a[i][k]*b[k][j]; return c; } B.7 File ‘‘inversekinematics.h’’ /* * Copyright (C) 2003 Bill Goodwine. */ #include<stdio.h> #include<stdlib.h> #include<math.h> #define MAX_ITERATIONS 1000 #define EPS 0.0000000001 #define PERTURBATION 0.001 #define DIAGONAL_EPS 0.0001 #define N 6 double** forwardkinematics(double *alpha, double *a, double *d, double *theta, double **T); double** matrixinverse(double **J, double **Jinv, int n); double** pseudoinverse(double **J, double **Jinv); double** computejacobian(double* alpha, double* a, double* d, double* theta, double** T, double** J); double** matrixproduct(double **a,double **b, double **c, int n); double** homogeneoustransformation(double alpha, double a, double d, double theta, double **T); B.8 File ‘‘dh.dat’’ 000 -9000 0246 -90224 9000 -9000 Copyright © 2005 by CRC Press LLC 3 -30 Robotics and Automation Handbook B.9 File ‘‘Tdes.dat’’ -0.707106 0 0.707106 12 0-1012 0.707106 0 0.707106 -12 0001 B.10 File ‘‘theta.dat’’ 24.0 103.0 145.0 215.0 29.2 30.0 References [1] Craig, J.J. (1989). Introduction to Robotics Mechanics and Control. Addison-Wesley, Reading, MA. [2] Denavit, J. and Hartenberg, R.S. (1955). A kinematic notation for lower-pair mechanisms based on matrices, J. Appl. Mech., 215–221. [3] Murray, R.M., Zexiang, L.I., and Sastry, S.S. (1994). A Mathematical Introduction to Robotic Manip- ulation, CRC Press, Boca Raton, FL. [4] Gupta, K.C. (1997). Mechanics and Control of Robots. Springer-Verlag, Heidelberg. [5] Paden, B. and Sastry, S. (1988). Optimal kinematic design of 6 manipulators. Int. J. Robotics Res., 7(2), 43–61. [6] Lee, H.Y. and Liang, C.G. (1988). A new vector theory for the analysis of spatial mechanisms. Mechanisms and Machine Theory, 23(3), 209–217. [7] Raghavan, M. (1990). Manipulator kinematics. In Roger Brockett (ed.), Robotics: Proceedings of Symposia in Applied Mathematics, Vol. 41, American Mathematical Society, 21–48. [8] Isaacson, E. and Keller, H.B. (1966). Analysis of Numerical Methods. Wiley, New York. [9] Naylor, A.W. and Sell, G.R. (1982). Linear Operator Theory in Engineering and Science. Springer- Verlag, Heidelberg. Copyright © 2005 by CRC Press LLC 4 Newton-Euler Dynamics of Robots Mark L. Nagurka Marquette University Ben-Gurion University of the Negev 4.1 Introduction Scope • Background 4.2 Theoretical Foundations Newton-Euler Equations • Force and Torque Balance on an Isolated Link • Two-Link Robot Example • Closed-Form Equations 4.3 Additional Considerations Computational Issues • Moment of Inertia • Spatial Dynamics 4.4 Closing 4.1 Introduction 4.1.1 Scope The subject of this chapter is the dynamic analysis of robots modeled using the Newton-Euler method, one of several approaches for deriving the governing equations of motion. It is assumed that the robots are characterized as open kinematic chains with actuators at the joints that can be controlled. The kinematic chain consists of discrete rigid members or links, with each link attached to neighboring links by means of revolute or prismatic joints, i.e., lower-pairs (Denavit and Hartenberg, 1955). One end of the chain is fixed to a base (inertial) reference frame and the other end of the chain, with an attached end-effector, tool, or gripper, can move freely in the robotic workspace and/or be used to apply forces and torques to objects being manipulated to accomplish a wide range of tasks. Robot dynamics is predicated on an understanding of the associated kinematics, covered in a sepa- rate chapter in this handbook. An approach for robot kinematics commonly adopted is that of a 4 × 4 homogeneous transformation, sometimes referred to as the Denavit-Hartenberg (D-H) transformation (Denavit and Hartenberg, 1955). The D-H transformations essentially determine the position of the origin and the rotation of one link coordinate frame with respect to another link coordinate frame. The D-H transformations can also be used in deriving the dynamic equations of robots. 4.1.2 Background The field of robot dynamicshas a rich history with manyimportant developments. There is a wide literature baseofreportedwork,withmyriadarticlesinprofessionaljournalsandestablishedtextbooks(Featherstone, 1987; Shahinpoor, 1987; Spong and Vidyasager, 1989; Craig, 1989; Yoshikawa, 1990; McKerrow, 1991; Copyright © 2005 by CRC Press LLC Newton-Euler Dynamics of Robots 4 -5 where the mass moments of inertia are with respect to each link’s centroidal frame. Superscript 0 is used to denote the inertial reference frame R. Equation (4.9) can be used to find the torques at the joints, as follows: 0 T 1,2 = 0 T 2,3 + 0 d 1,2 × m 2 0 a 2 + 0 p 1,2 × 0 F 2,3 − 0 d 1,2 × m 2 0 g + I 2 0 α 2 + 0 ω 2 × I 2 0 ω 2 (4.12) 0 T 0,1 = 0 T 1,2 + 0 d 0,1 × m 1 0 a 1 + 0 p 0,1 × 0 F 1,2 − 0 d 0,1 × m 1 0 g + I 1 0 α 1 + 0 ω 1 × I 1 0 ω 1 (4.13) An expression for 0 F 1,2 can be found from Equation (4.7) and substituted into Equation (4.13) to give 0 T 0,1 = 0 T 1,2 + 0 d 0,1 × m 1 0 a 1 + 0 p 0,1 × m 2 0 a 2 + 0 p 0,1 × 0 F 2,3 − 0 p 0,1 × m 2 0 g − 0 d 0,1 × m 1 0 g + I 1 0 α 1 + 0 ω 1 × I 1 0 ω 1 (4.14) Foraplanartwo-linkrobotwith two revolute joints,asshownin Figure4.3, moving in a horizontal plane, i.e., perpendicular to gravity, several simplifications can be made in Equation (4.11) to Equation (4.14): (i) the gyroscopic terms 0 ω n ×( C I n 0 ω n ) for n =1, 2 can be eliminated since the mass moment of inertia matrix is a scalar and the cross product of a vector with itself is zero, (ii) the gravity terms 0 d n−1,n ×m n 0 g disappear since the robot moves in a horizontal plane, and (iii) the torques can be written as scalars and act perpendicular to the plane of motion. Equation (4.12) and Equation (4.14) can then be written in scalar form, 0 T 1,2 = 0 T 2,3 + 0 d 1,2 × m 2 0 a 2 · ˆ u z + 0 p 1,2 × 0 F 2,3 · ˆ u z + I 2 0 α 2 (4.15) 0 T 0,1 = 0 T 1,2 + 0 d 0,1 × m 1 0 a 1 · ˆ u z + 0 p 0,1 × m 2 0 a 2 · ˆ u z + 0 p 0,1 × 0 F 2,3 · ˆ u z + I 1 0 α 1 (4.16) where the dot product is taken (with the terms in parentheses) with the unit vector ˆ u z perpendicular to the plane of motion. A further simplification can be introduced by assuming the links have uniform cross-sections and are made of homogeneous (constant density) material. The center of mass then coincides with the geometric center and the distance to the center of mass is half the link length. The position vectors can be 0 F 0,1 0 p 0,1 0 T 1,2 0 T 2,3 0 F 2,3 0 p 1,2 0 F 1,2 0 T 0,1 q 1 q 2 x 1 x 2 x 0 y 0 y 1 y 2 FIGURE 4.3 Two-link robot with two revolute joints. Copyright © 2005 by CRC Press LLC 4 -6 Robotics and Automation Handbook written as, 0 p 0,1 = 2 0 d 0,1 , 0 p 1,2 = 2 0 d 1,2 (4.17) with the components, 0 p 0,1x 0 p 0,1y = 2 0 d 0,1x 0 d 0,1y = l 1 C 1 l 1 S 1 , 0 p 1,2x 0 p 1,2y = 2 0 d 1,2x 0 d 1,2y = l 2 C 12 l 2 S 12 (4.18) where the notation S 1 = sin θ 1 , C 1 = cosθ 1 , S 12 = sin(θ 1 + θ 2 ); C 12 = cos(θ 1 + θ 2 ) is introduced; and l 1 , l 2 are the lengths of links 1,2, respectively. The accelerations of the center of mass of each link are needed in Equation (4.15) and Equation (4.16). These accelerations can be written as, 0 a 1 = 0 α 1 × 0 d 0,1 + 0 ω 1 × 0 ω 1 × 0 d 0,1 (4.19) 0 a 2 = 2 0 a 1 + 0 α 2 × 0 d 1,2 + 0 ω 2 × 0 ω 2 × 0 d 1,2 (4.20) Expanding Equation (4.19) and Equation (4.20) gives the acceleration components: 0 a 1x 0 a 1y = − 1 2 l 1 S 1 ¨ θ 1 − 1 2 l 1 C 1 ˙ θ 2 1 1 2 l 1 C 1 ¨ θ 1 − 1 2 l 1 S 1 ˙ θ 2 1 (4.21) 0 a 2x 0 a 2y = − l 1 S 1 + 1 2 l 2 S 12 ¨ θ 1 − 1 2 l 2 S 12 ¨ θ 2 −l 1 C 1 ˙ θ 2 1 − 1 2 l 2 C 12 ( ˙ θ 1 + ˙ θ 2 ) 2 l 1 C 1 + 1 2 l 2 C 12 ¨ θ 1 + 1 2 l 2 C 12 ¨ θ 2 −l 1 S 1 ˙ θ 2 1 − 1 2 l 2 S 12 ( ˙ θ 1 + ˙ θ 2 ) 2 (4.22) Equation (4.15) gives the torque at joint 2: 0 T 1,2 = 0 T 2,3 + m 2 0 d 1,2x ˆ u x0 + 0 d 1,2y ˆ u y0 × 0 a 2x ˆ u x0 + 0 a 2y ˆ u y0 · ˆ u z + 0 p 1,2x ˆ u x0 + 0 p 1,2y ˆ u y0 × 0 F 2,3x ˆ u x0 + 0 F 2,3y ˆ u y0 · ˆ u z + I 2 0 α 2 where ˆ u x0 , ˆ u y0 are the unit vectors along the x 0 , y 0 axes, respectively. Substituting and evaluating yields, 0 T 1,2 = 0 T 2,3 + 1 2 m 2 l 1 l 2 C 2 + 1 4 m 2 l 2 2 + I 2 ¨ θ 1 + 1 4 m 2 l 2 2 + I 2 ¨ θ 2 + 1 2 m 2 l 1 l 2 S 1 ˙ θ 2 1 +l 2 C 12 0 F 2,3y −l 2 S 12 0 F 2,3x (4.23) Similarly, Equation (4.16) gives the torque at joint 1: 0 T 0,1 = 0 T 1,2 + m 1 0 d 0,1x ˆ u x0 + 0 d 0,1y ˆ u y0 × 0 a 1x ˆ u x0 + 0 a 1y ˆ u y0 · ˆ u z +m 2 0 p 0,1x ˆ u x0 + 0 p 0,1y ˆ u y0 × 0 a 2x ˆ u x0 + 0 a 2y ˆ u y0 · ˆ u z + 0 p 0,1x ˆ u x0 + 0 p 0,1y ˆ u y0 × 0 F 2,3x ˆ u x0 + 0 F 2,3y ˆ u y0 · ˆ u z + I 1 0 α 1 which yields after substituting, 0 T 0,1 = 0 T 1,2 + 1 4 m 1 l 2 1 + m 2 l 2 2 + 1 2 m 2 l 1 l 2 C 2 + I 1 ¨ θ 1 + 1 2 m 2 l 1 l 2 C 2 ¨ θ 2 − 1 2 m 2 l 1 l 2 S 2 ˙ θ 2 1 − 1 2 m 2 l 1 l 2 S 2 ˙ θ 2 2 − m 2 l 1 l 2 S 2 ˙ θ 1 ˙ θ 2 +l 1 C 1 0 F 2,3y −l 1 S 1 0 F 2,3x (4.24) Copyright © 2005 by CRC Press LLC 4 -10 Robotics and Automation Handbook Spong, M.W. and Vidyasager, M., Robot Dynamics and Control, John Wiley & Sons, New York, 1989. Swain, A.K. and Morris, A.S., A unified dynamic model formulation for robotic manipulator systems, J. Robotic Syst., 20, No. 10, 601–620, 2003. Vukobratovic, M., Potkonjak, V., and Matijevi, V., Dynamics of Robots with Contact Tasks, Kluwer, Dordrecht, 2003. Walker, M.W. and Orin, D.E., Efficient dyamic computer simulation of robotic mechanisms, Trans. ASME, J. Syst., Meas. Control, 104, 205–211, 1982. Yoshikawa, T., Foundations of Robotics: Analysis and Control, MIT Press, Cambridge, MA, 1990. Copyright © 2005 by CRC Press LLC 5 Lagrangian Dynamics Milo ˇ s ˇ Zefran University of Illinois at Chicago Francesco Bullo University of Illinois at Urbana-Champaign 5.1 Introduction 5.2 Preliminaries Velocities and Forces • Kinematics of Serial Linkages 5.3 Dynamic Equations Inertial Properties of a Rigid Body • Euler-Lagrange Equations for Rigid Linkages • Generalized Force Computation • Geometric Interpretation 5.4 Constrained Systems Geometric Interpretation 5.5 Impact Equations 5.6 Bibliographic Remarks 5.7 Conclusion 5.1 Introduction The motion of a mechanical system is related via a set of dynamic equations to the forces and torques the system is subject to. In this work we will be primarily interested in robots consisting of a collection of rigid links connected through joints that constrain the relative motion between the links. There are two main formalisms for deriving the dynamic equations for such mechanical systems: Newton-Euler equations that are directly based on Newton’s laws and Euler-Lagrange equations that have their root in the classical work of d’Alembert and Lagrange on analytical mechanics and the work of Euler and Hamilton on variational calculus. The main difference between the two approaches is in dealing with constraints. While Newton’s equations treat each rigid body separately and explicitly model the constraints through the forces required to enforce them, Lagrange and d’Alembert provided systematic procedures for eliminating the constraints from the dynamic equations, typically yielding a simpler system of equations. Constraints imposed by joints and other mechanical components are one of the defining features of robots so it is not surprising that the Lagrange’s formalism is often the method of choice in robotics literature. 5.2 Preliminaries The approach and the notation in this section follow [21], and we refer the reader to that text for additional details. A starting point in describing a physical system is the formalism for describing its motion. Since we will be concerned with robots consisting of rigid links, we start by describing rigid body motion. Formally, a rigid body O is a subset of R 3 where each element in O corresponds to a point on the rigid body. The defining property of a rigid body is that the distance between arbitrary two points on the rigid body remains unchanged as the rigid body moves. If a body-fixed coordinate frame B is attached to O,an Copyright © 2005 by CRC Press LLC 5 -2 Robotics and Automation Handbook arbitrary point p ∈O can be described by a fixed vector p B . As a result, the position of any point in O is uniquely determined by the location of the frame B. To describe the location of B in space we choose a global coordinate frame S. The position and orientation of the frame B in the frame S is called the configuration of O and can be described by a 4 ×4 homogeneous matrix g SB : g SB = R SB d SB 01 , R SB ∈ R 3×3 , d SB ∈ R 3 , R T SB R SB = I 3 ,det(R SB ) = 1 (5.1) Here I n denotes the identity matrix in R n×n . The set of all possible configurations of O is known as SE(3), the special Euclidean group of rigid body transformations in three dimensions. By the above argument, SE(3) is equivalent to the set of homogeneous matrices. It can be shown that it is a matrix group as well as a smooth manifold and, therefore, a Lie group; for more details we refer to [11, 21]. It is convenient to denote matrices g ∈ SE(3) by the pair (R, d) for R ∈ SO(3) and d ∈ R 3 (recall that SO(3) ={R ∈ R 3×3 |R T R = I 3 ,det(R) = 1}). Given a point p ∈ Odescribed by a vector p B in the frame B, it is natural to askwhat is thecorresponding vector in the frame S. From the definition of g SB we have p S = g SB p B where for a vector p ∈ R 3 , the corresponding homogeneous vector p is defined as p = p 1 The tangent space of SE(3) at g 0 ∈ SE(3) is the vector space of matrices of the form ˙ g(0), where g(t)is a curve in SE(3) such that g(0) = g 0 . The tangent space of a Lie group at the group identity is called the Lie algebra of the Lie group. The Lie algebra of SE(3), denoted by se(3), is se(3) = v 00 | ∈ R 3×3 , v ∈ R 3 , T =− (5.2) Elements of se(3) are called twists. Recall that a 3 ×3 skew-symmetric matrix can be uniquely identified with a vector ω ∈ R 3 so that for an arbitrary vector x ∈ R 3 , x = ω × x,where× is the vector cross product operation in R 3 . Each element T ∈ se(3) can be thus identifiedwitha6× 1vector ξ = v ω called the twist coordinates of T. We also write = ω and T = ξ to denote these transitions between vectors and matrices. An important relation between the Lie group and its Lie algebra is provided by the exponential map.It can be shown that for ξ ∈se(3), exp( ξ) ∈SE(3), where exp : R n×n → R n×n is the usual matrix exponen- tial. Using the properties of SE(3), it can be shown that, for ξ T = [ v T ω T ], exp( ξ) = I 3 ∅v 01 , ω = 0, exp( ω) 1 ω 2 [(I 3 − exp( ω))(ω ×v) + ωω T v] 01 , ω = 0 (5.3) where the relation exp( ω) = I 3 + sin ω ω ω + 1 −cos ω ω 2 ω 2 Copyright © 2005 by CRC Press LLC Lagrangian Dynamics 5 -5 For serial linkages the forward kinematics map has a particularly revealing form. Choose a reference configuration of the manipulator, i.e. the configuration where all the joint variables are 0, and let ξ 1 , , ξ n be the joint twists in this configuration expressed in the global coordinate frame S. If the ith joint is revolute and l ={p + λω ∈ R 3 | λ ∈ R, ω=1} is the axis of rotation, then the joint twist corresponds to the screw (l, 0, 1) and is ξ i = −ω × p ω (5.6) If the ith joint is prismatic and v ∈ R 3 , v=1, is the direction in which it moves, then the joint twist corresponds to the screw ({λv ∈ R 3 | λ ∈ R}, ∞, 1) and is ξ i = v 0 One can show that g SB i (q) = exp(ξ 1 q 1 ) ···exp(ξ i q i )g SB i ,0 (5.7) where g SB i ,0 ∈SE(3) is the reference configuration of the ith link. This is known as the ProductofExpo- nentials Formula and was introduced in [7]. Another important relation is that between the joint rates and the link body and spatial velocities. A direct computation shows that along a curve t → q(t), V s SB i (t) = J s SB i (q(t)) ˙ q(t) where J s SB i isaconfiguration-dependent 6 ×n matrix, called the spatial manipulator Jacobian, that for serial linkages equals J s SB i (q) = [ ξ S,1 (q) ξ S,i (q)0 0 ] Here the j th column ξ S, j (q) of the spatial manipulator Jacobian is the jth joint twist expressed in the global reference frame S after the manipulator has moved to the configuration described by q. It can be computed from the forward kinematics map using the formula: ξ S, j (q) = Ad (exp(ξ 1 q 1 )···exp(ξ j−1 q j−1 )) ξ j Similar expressions can be obtained for the body velocity V b SB i (t) = J b SB i (q(t)) ˙ q(t) (5.8) where J b SB i isaconfiguration-dependent 6 ×n body manipulator Jacobian matrix. For serial linkages, J b SB i (q) = [ ξ B i ,1 (q) ξ B i ,i (q)0 0 ] (5.9) wherethe jth column ξ B i , j (q), j ≤i isthe jthjointtwist expressedinthelink frameB i afterthemanipulator has moved to the configuration described by q and is given by ξ B i , j (q) = Ad −1 (exp(ξ j q j )···exp(ξ i q i )g SB i ,0 ) ξ j Copyright © 2005 by CRC Press LLC [...]... Bobrow, J.E., and Ploen, S.R., A Lie group formulation of robot dynamics, Int J Robotics Res., 14 (6):609– 618 , 19 95 [25] Park, F.C., Choi, J., and Ploen, S.R., Symbolic formulation of closed chain dynamics in independent coordinates, Mechanism and Machine Theory, 34 (5):7 31 7 51, 19 99 Copyright © 2005 by CRC Press LLC 5 -1 6 Robotics and Automation Handbook [26] Sciavicco, L and Siciliano, B., Modeling and Control... M., Dynamics of articulated open-chain active mechanisms, Math c Biosci., 28 : 13 7 17 0, 19 76 [ 31 ] Stokes, A and Brockett, R.W., Dynamics of kinematic chains, Int J Robotics Res., 15 (4) :39 3–405, 19 96 [32 ] Uicker, J.J., Dynamic force analysis of spatial linkages, ASME J Appl Mech., 34 : 418 –424, 19 67 Copyright © 2005 by CRC Press LLC 6 Kane’s Method in Robotics 6 .1 6.2 6 .3 Introduction The Essence of Kane’s... on Robotics and Automation, 826– 834 , San Francisco, CA, April 2000 [14 ] Hollerbach, J.M., A recursive Lagrangian formulation of manipulator dynamics and a comparative study of dynamics formulation, IEEE Trans Syst., Man, & Cybernetics, 10 (11 ): 730 – 736 , 19 80 [15 ] Kahn, M.E and Roth, B., The near-minimum-time control of open-loop articulated kinematic chains, ASME J Dynamic Syst., Meas., Control, 93 :16 4 17 2,... by CRC Press LLC 0, j = i, τi , j =i Lagrangian Dynamics 5 -1 5 References [1] Abraham, R and Marsden, J.E., Foundations of Mechanics, Addison-Wesley, Reading, MA, 2nd ed 19 87 [2] Arimoto, S., Control Theory of Non-linear Mechanical Systems: A Passivity-Based and Circuit-Theoretic Approach, Volume 49 of OESS, Oxford University Press, Oxford, 19 96 [3] Asada, H and Slotine, J.-J.E., Robot Analysis and. .. Vukobratovi´ , M., and Hartoch, G., Kinematic and kinetic analysis of c open-chain linkages utilizing Newton-Euler methods, Mathe Biosci., 43 :10 7 13 0, 19 79 [ 23] Ortega, R., Loria, A., Nicklasson, P.J., and Sira-Ramirez, H., Passivity-based control of EulerLagrange systems: mechanical, electrical and electromechanical applications, In: Communications and Control Engineering, Springer-Verlag, New York, 19 98 [24]... Dordrecht, 19 92 [19 ] Luh, J.Y.S., Walker, M.W., and Paul, R.P.C., On-line computational scheme for mechanical manipulators, ASME J Dynamic Syst., Meas., Control, 10 2(2):69–76, 19 80 [20] Marsden, J.E and Ratiu, T.S., Introduction to Mechanics and Symmetry, Springer-Verlag, New York, 19 94 [ 21] Murray, R.M., Li, Z., and Sastry, S.S., A Mathematical Introduction to Robotic Manipulation, CRC Press, Boca Raton, 19 94... published) [10 ] Craig, J.J., Introduction to Robotics: Mechanics and Control, Addison-Wesley, Reading, MA, 2nd ed., 19 89 [11 ] do Carmo, M.P., Riemannian Geometry, Birkhauser, Boston, 19 92 [12 ] Featherstone, R., Robot Dynamics Algorithms, Volume 22 of Kluwer International Series in Engineering and Computer Science, Kluwer, Dordrecht, 19 87 [ 13 ] Featherstone, R and Orin, D.E., Robot dynamics: equations and algorithms,... Manipulators, Springer-Verlag, New York, 2nd ed., 2000 [27] Selig, J.M., Geometrical Methods in Robotics, Springer-Verlag, Heidelberg, 19 96 [28] Silver, W.M., On the equivalence of Lagrangian and Newton-Euler dynamics for manipulators, Int J Robotics Res., 1( 2):60–70, 19 82 [29] Spong, M.W and Vidyasagar, M., Robot Dynamics and Control, John Wiley & Sons, New York, 19 89 [30 ] Stepanenko, Y and Vukobratovi´... Meas., Control, 93 :16 4 17 2, September 19 71 [16 ] Kozlov, V.V and Treshchev, D.V., Billiards: A Generic Introduction to the Dynamics of Systems with Impacts, Volume 89 of Translations of Mathematical Monographs, American Mathematical Society, Providence, RI, June 19 91 [17 ] Lewis, A.D., Simple mechanical control systems with constraints, IEEE Trans Automatic Control, 45(8) :14 20 14 36 , 2000 [18 ] Lilly, K.W.,... 20 03 [7] Brockett, R.W., Robotic manipulators and the product of exponentials formula, In: Mathematical Theory of Networks and Systems (MTNS), 12 0 12 9, Beer Sheba, Israel, 19 83 [8] Brogliato, B., Nonsmooth Mechanics: Models, Dynamics and Control, Springer-Verlag, New York, 2nd ed., 19 99 [9] Bullo, F and Lewis, A.D., Geometric Control of Mechanical Systems, Texts in Applied Mathematics, Springer-Verlag, . components: 0 a 1x 0 a 1y = − 1 2 l 1 S 1 ¨ θ 1 − 1 2 l 1 C 1 ˙ θ 2 1 1 2 l 1 C 1 ¨ θ 1 − 1 2 l 1 S 1 ˙ θ 2 1 (4. 21) 0 a 2x 0 a 2y = − l 1 S 1 + 1 2 l 2 S 12 ¨ θ 1 − 1 2 l 2 S 12 ¨ θ 2 −l 1 C 1 ˙ θ 2 1 − 1 2 l 2 C 12 ( ˙ θ 1 + ˙ θ 2 ) 2 l 1 C 1 + 1 2 l 2 C 12 ¨ θ 1 + 1 2 l 2 C 12 ¨ θ 2 −l 1 S 1 ˙ θ 2 1 − 1 2 l 2 S 12 ( ˙ θ 1 + ˙ θ 2 ) 2 (4.22) Equation. ‘‘dh.dat’’ 000 -9 000 0246 -9 0224 9000 -9 000 Copyright © 2005 by CRC Press LLC 3 -3 0 Robotics and Automation Handbook B.9 File ‘‘Tdes.dat’’ -0 .70 710 6 0 0.70 710 6 12 0 -1 012 0.70 710 6 0 0.70 710 6 -1 2 00 01 B .10 . and Automation Handbook written as, 0 p 0 ,1 = 2 0 d 0 ,1 , 0 p 1, 2 = 2 0 d 1, 2 (4 .17 ) with the components, 0 p 0,1x 0 p 0,1y = 2 0 d 0,1x 0 d 0,1y = l 1 C 1 l 1 S 1 , 0 p 1, 2x 0 p 1, 2y =