Robot Manipulators 2011 Part 2 doc

Unit Quaternions: A Mathematical Tool for Modeling, Path Planning and Control of Robot Manipulators 27 matrix, but maps into two unit quaternions, representing antipodes in the unit hypersphere (Gallier, 2000): 3 SO(3) S ⎡⎤ ∈⇔±∈. ⎢⎥ ⎣⎦ R η ε (14) Also from (13) we can verify that ()() T RR η εηε ,− = , and, as 3 S T T η ε ⎡ − ⎤ ∈ ⎣⎦ is the conjugate (or inverse) of 3 S T T η ε ⎡⎤∈ ⎣⎦ , then we have that 3 SO(3) S ∗ ⎡⎤ ⎡ ⎤ ∈⇔±=±∈. ⎢⎥ ⎢ ⎥ − ⎣⎦ ⎣ ⎦ T R ηη εε Another important fact is that quaternion multiplication is equivalent to the rotation matrix multiplication. Thus, if 3 12 S ξξ ±,± ∈ are the Euler parameters corresponding to the rotation matrices 12 SO(3)RR,∈ , respectively, then: 12 12 ⇔± ⊗.RR ξξ (15) Moreover, the premultiplication of a vector 3 v ∈ \ by a rotation matrix SO(3)R ∈ produces a transformed (rotated) vector 3 wRv=∈\ . The same transformation can be performed with quaternion multiplication using wv ξ ξ ∗ =⊗⊗ , where 3 S ξ ∈ is the unit quaternion corresponding to R and quaternions v , ∈w H are formed adding a null scalar part to the corresponding vectors ( 0 T T vv ⎡ ⎤ = ⎣ ⎦ , 0 T T ww ⎡ ⎤ = ⎣ ⎦ ). In sum 34 11 ∗ ∈⇔⊗⊗∈.\\Rv v ξξ (16) In the case of a time–varying orientation, we need to establish a relation between the time derivatives of Euler parameters 4 T T ξηε ⎡⎤ =∈ ⎣⎦   \ and the angular velocity of the rigid body 3 ω ∈ \ . That relation is given by the so–called quaternion propagation rule (Sciavicco & Siciliano, 2000): 1 () 2 E ξξ ω =.  (17) where 43 () ( ) () T EE IS ε ξηε ηε × ⎡ − ⎤ =,= ∈. ⎢⎥ − ⎣⎦ \ (18) By using properties (2), (8) and the unit norm constraint (9) it can be shown that () () T EE I ξξ = , so that ω can be resolved from (17) as 2() T E ω ξξ =  (19) Robot Manipulators 28 3. Robot Kinematics and Dynamics Serial robot manipulators have an open kinematic chain, i.e., there is only one path from one end (the base) to the other (the end–effector) of the robotic chain (see Fig. 3). A serial manipulator with n joints, either translational (prismatic) or rotational, has n degrees of freedom (dof). This section deals only with the modeling of serial manipulators. As explained in (Craig, 2005), any serial robot manipulator can be described by using four parameters for each joint, that is, a total of 4n parameters for a n –dof manipulator. Denavit and Hartenberg (Denavit & Hartenberg, 1955) proposed a general method to establish those parameters in a systematic way, by defining 1n + coordinate frames, one per each link, including the base. The four Denavit–Hartenberg (or simply D–H) parameters for the i -th joint can then be extracted from the relation between frame 1i − and frame i , as follows: i a : Distance from axis 1i z − to i z , along axis i x . i α : Angle between axes 1i z − and i z , about axis i x . i d : Distance from axis 1i x − to i x , along axis 1i z − . i θ : Angle between axes 1i x − and i x , about axis 1i z − . Note that there are three constant D–H parameters for each joint; the other one ( i θ for rotational joints, i d for translational joints) describes the motion produced by such i -th joint. To follow the common notation, let i q be the variable D–H parameter corresponding to the i –th joint. Then the so–called joint configuration space is specified by the vector 1 2 T n n qqq q ⎡⎤ ⎢⎥ ⎣⎦ =∈,"\ while the pose (position and orientation) of the end–effector frame with respect to the base frame (see Fig. 3), denoted here by x , is given by a point in the 6–dimensional pose configuration space 33 M×\ , using whichever of the representations for 3 M m ⊂ \ . That is 33 3 M m p x φ + ⎡⎤ =∈×⊂ . ⎢⎥ ⎣⎦ \\ Figure 3. A 4-dof serial robot manipulator Unit Quaternions: A Mathematical Tool for Modeling, Path Planning and Control of Robot Manipulators 29 3.1 Direct Kinematics The direct kinematics problem consists on expressing the end–effector pose x in terms of q . That is, to find a function (called the kinematics function) 33 :M n h →×\\ such that () () . () pq xhq q φ ⎡ ⎤ == ⎢ ⎥ ⎣ ⎦ (20) Traditionally, direct kinematics is solved from the D–H parameters using homogeneous matrices (Denavit & Hartenberg, 1955). A homogeneous matrix combines a rotation matrix and a position vector in an extended 44× matrix. Thus, given 3 p ∈ \ and SO(3)R ∈ , the corresponding homogeneous matrix T is 44 SE(3) 01 T Rp T × ⎡⎤ =∈⊂. ⎢⎥ ⎣⎦ \ (21) SE(3) is known as the special Euclidean group and contains all homogeneous matrices of the form (21). It is a group under the standard matrix multiplication, meaning that the product of two homogeneous matrices 12 SE(3)TT,∈ is given by 21 21 2 21 12 21 SE(3) 0101 0 1 TT T RR RRR pp pp TT + ⎡⎤⎡⎤⎡ ⎤ ==∈. ⎢⎥⎢⎥⎢ ⎥ ⎣⎦⎣⎦⎣ ⎦ (22) In terms of the D–H parameters, the relative position vector and rotation matrix of the i -th frame with respect to the previous one are (Sciavicco & Siciliano, 2000): 113 SO(3) 0 ii ii i i iii ii i i i i ii i i i i CSCSS aC RS CC CS aS p SC d θα θα θ θ θθα θα θ α α ⎡⎤ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎣⎦ ⎡⎤ ⎢⎥ ⎢⎥ −− ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎣⎦ − =−∈,=∈.\ (23) where x C , x S stand for cos( ) x , sin( ) x , respectively. The original method proposed by (Denavit & Hartenberg, 1955) uses the expressions in (23) to form relative homogeneous matrices 1i i T − (each depending on i q ), and then the composition rule (22) to compute the homogeneous matrix of the end–effector with respect to the base frame, () SE(3)Tq∈ : 01 1 12 () () () SE(3) 01 n n T Rq pq Tq T T T − ⎡⎤ ==∈. ⎢⎥ ⎣⎦ " This leads to the following general expressions for the pose of the end–effector, in terms of ()pq and ()Rq 0 11 12 () n n R qRR R − = " (24) 0 1210132 210 12 1 12 2 1 121 () nn n n nn nn pq R R R R R R … R pppp −− − − −− − =+ +++"" (25) An alternative method for computing the direct kinematics of serial manipulators using quaternions, instead of homogeneous matrices, was proposed recently by (Campa et al., Robot Manipulators 30 2006). The method is inspired in one found in (Barrientos et al. 1997), but it is more general, and gives explicit expressions for the position 3 ()pq∈ \ and orientation 34 () Sq ξ ∈⊂\ in terms of the original D–H parameters. Let us start from the expressions for 1i i R − and 1i i p − in (23). We can use (12) and some trigonometric identities to obtain the corresponding quaternion 1i i ξ − from 1i i R − ; also, we can extend 1i i p − to form the quaternion 1i i p − . The results are 131 cos cos 22 0 cos sin cos( ) 22 S sin( ) sin sin 22 sin cos 22 ii ii ii ii i i ii ii i ii a p a d θα θα θ ξ θ θα θα ⎡⎤ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ −− ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎢⎥ ⎣⎦ ⎡⎤ ⎛⎞ ⎛ ⎞ ⎜⎟ ⎜ ⎟ ⎢⎥ ⎝⎠ ⎝ ⎠ ⎢⎥ ⎢⎥ ⎛⎞ ⎛ ⎞ ⎢⎥ ⎜⎟ ⎜ ⎟ ⎝⎠ ⎝ ⎠ ⎢⎥ =∈,=∈ ⎢⎥ ⎛⎞ ⎛⎞ ⎢⎥ ⎜⎟ ⎜⎟ ⎝⎠ ⎝⎠ ⎢⎥ ⎢⎥ ⎛⎞ ⎛ ⎞ ⎢⎥ ⎜⎟ ⎜ ⎟ ⎢⎥ ⎝⎠ ⎝ ⎠ ⎣⎦ H Then, we can apply properties (15) and (16) iteratively to obtain expressions equivalent to (24) and (25): 01 1 12 01 2 1 01 2 12 1 12 1 01 3 2 01 3 010*0 12 2 1 12 2 1 21 1 () () ( ) ( ) ()() . n n nn n nn n nn n nn n q pq p ppp ξξξ ξ ξξ ξ ξξ ξ ξξ ξ ξξ ξ ξ ξ − −− −∗ −− −− −∗ −− − =⊗⊗⊗ , =⊗⊗⊗ ⊗ ⊗⊗⊗⊗ + ⊗⊗⊗ ⊗ ⊗ ⊗⊗⊗ ++⊗ +   … ()q ξ is the unit quaternion (Euler parameters) expressing the orientation of the end–effector with respect to the base frame. The position vector ()pq is the vector part of the quaternion ()pq . An advantage of the proposed method is that the position and orientation parts of the pose are processed separately, using only quaternion multiplications, which are computationally more efficient than homogeneous matrix multiplications. Besides, the result for the orientation part is given directly as a unit quaternion, which is a requirement in task space controllers (see Section 5). The inverse kinematics problem, on the other hand, consists on finding explicit expressions for computing the joint coordinates, given the pose coordinates, that is equivalent to find the inverse funcion of h in (20), mapping 33 M×\ to n \ : 1 ()qx h − =. (26) The inverse kinematics, however, is much more complex than direct kinematics, and is not derived in this work. 3.2 Differential Kinematics Differential kinematics gives the relationship between the joint velocities and the corresponding end–effector’s linear and angular velocities (Sciavicco & Siciliano, 2000). Unit Quaternions: A Mathematical Tool for Modeling, Path Planning and Control of Robot Manipulators 31 Thus, if =∈  \ n d dt qq is the vector of joint velocities, 3 v ∈ \ is the linear velocity, and 3 ω ∈ \ is the angular velocity of the end–effector, then the differential kinematics equation is given by () () () p o Jq v qJqq Jq ω ⎡⎤ ⎡⎤ == ⎢⎥ ⎢⎥ ⎣⎦ ⎣⎦  (27) where matrix 6 () n Jq × ∈ \ is the geometric Jacobian of the manipulator, which depends on its configuration. () p J q , 3 () n o Jq × ∈ \ are the components of the geometric Jacobian producing the translational and rotational parts of the motion. Now consider the time derivative of the generic kinematics function (20): () () () () A pq q hq x qqJqq q q q φ ∂ ⎡⎤ ⎢⎥ ∂ ∂ ⎢⎥ == =. ⎢⎥ ∂ ∂ ⎢⎥ ∂ ⎣⎦   (28) Matrix (3 ) () mn A Jq +× ∈ \ is known as the analytic Jacobian of the robot, and it relates the joint velocities with the time derivatives of the pose coordinates (or pose velocities). In the particular case of using Euler parameters for the orientation, 34 () () Sqq φξ =∈⊂\ , and 7 () () n AA Jq J q ξ × =∈\ , i.e. () A p x Jqq ξ ξ ⎡⎤ == . ⎢⎥ ⎣⎦    (29) The linear velocity v of the end–effector is simply the time derivative of the position vector p (i.e. vp=  ); instead, the angular velocity ω is related with ξ  by (19). So, we have () R vp Jq ξ ω ξ ⎡ ⎤⎡⎤ = ⎢ ⎥⎢⎥ ⎣ ⎦⎣⎦   (30) where the representation Jacobian, for the case of using Euler parameters, is defined as 67 0 () 02(()) R T I Jq Eq ξ ξ ⎡⎤ ⎢⎥ × ⎢⎥ ⎢⎥ ⎣⎦ =∈\ (31) with matrix ()E ξ as in (18). Also notice, by combining (27), (29), and (30), that () () () RA J qJqJq ξξ = Whichever the Jacobian matrix be (geometric, analytic, representation), it is often required to find its inverse mapping, although this is possible only if such a matrix has full rank. In the case of a matrix nm J × ∈ \ , with nm> , then there exists a left pseudoinverse matrix of J , † mn J × ∈ \ , defined as () 1 † TT J JJ J − = Robot Manipulators 32 such that † mm JJ I × =∈\ . On the contrary, if nm< , then there exist a right pseudoinverse matrix of J , † mn J × ∈ \ , defined as 1 † TT JJJJ − ⎛⎞ ⎜⎟ ⎝⎠ = such that † nn JJ I × =∈\ . It is easy to show that in the case nm= (square matrix) then †† 1 J JJ − == . 3.3 Lagrangian Dynamics Robot kinematics studies the relations between joint and pose variables (and their time derivatives) during the motion of the robot’s end–effector. These relations, however, are established using only a geometric point of view. The effect of mechanical forces (either external or produced during the motion itself) acting on the manipulator is studied by robot dynamics. As pointed out by (Sciavicco & Siciliano, 2000), the derivation of the dynamic model of a manipulator plays an important role for simulation, analysis of manipulator structures, and design of control algorithms. There are two general methods for derivation of the dynamic equations of motion of a manipulator: one method is based on the Lagrange formulation and is conceptually simple and systematic; the other method is based on the Newton–Euler formulation and allows obtaining the model in a recursive form, so that it is computationally more efficient. In this section we consider the application of the Lagrangian formulation for obtaining the robot dynamics in the case of using Euler parameters for describing the orientation. This approach is not new, since it has been employed for modeling the dynamics of rigid bodies (see, e.g., Shivarama & Fahrenthold, 2004; Lu & Ren, 2006, and references therein). An important fact is that Lagrange formulation allows to obtain the dynamic model independently of the coordinate system employed to describe the motion of the robot. Let us consider a serial robot manipulator with n degrees of freedom. Then we can choose any set of mnk=+ coordinates, say m ρ ∈ \ , with 0k ≥ holonomic constraints of the form () 0 i γρ = ( 12i…k=,, , ), to obtain the robot’s dynamic model. If 0k = we talk of independent or generalized coordinates, if 1k ≥ we have dependent or constrained coordinates. The next step is to express the total (kinetic and potential) energy of the mechanical system in terms of the chosen coordinates. Let () ρρ ,  K be the kinetic energy, and () ρ U the potential energy of the system; then the Lagrangian function () ρρ ,  L is defined as ()()() ρρ ρρ ρ ,= ,−  LKU The Lagrange’s dynamic equations of motion, for the general case (with constraints), are expressed by () () () ⎧ ⎫⎛⎞ ∂, ∂, ∂ −=+ ⎨⎬ ⎜⎟ ∂∂ ∂ ⎩⎭ ⎝⎠   T d dt ρ ρρ ρρ γρ λ τ ρρ ρ LL (32) where m ρ τ ∈ \ is the vector of (generalized or constrained) forces associated with each of the coordinates ρ , vector () γρ is defined as Unit Quaternions: A Mathematical Tool for Modeling, Path Planning and Control of Robot Manipulators 33 γρ γρ γρ γρ ρ γρ × ⎡⎤ ⎢⎥ ∂ ⎢⎥ =∈, ∈ ⎢⎥ ∂ ⎢⎥ ⎣⎦ \\ # 1 2 () () () () sothat () kkm k and k λ ∈ \ is the vector of k Lagrange multipliers, which are employed to reduce the dimension of the system, producing only n independent equations in terms of a new set n r ∈\ of generalized coordinates. In sum, a general constrained system such as (32), with nk ρ + ∈ \ , can be transformed in an unconstrained system of the form () () r drr rr dt r r τ ∂, ∂, ⎧⎫ −= ⎨⎬ ∂∂ ⎩⎭   LL (33) Moreover, as explained by (Spong et al., 2006), equation (33) can be rewritten as: () ( ) () rr r r Mrr Crrr r g τ +,+ =    where () nn r Mr × ∈ \ is a symmetric positive definite matrix known as the inertia matrix, () nn r Crr × ,∈  \ is the matrix of centripetal and Coriolis forces, and () n r r g ∈ \ is the vector of forces due to gravity. These matrices satisfy the following useful properties (Kelly et al., 2005): • Inertia matrix is directly related to kinetic energy by 1 () () 2 T r rr M rr r ,=   K (34) • For a given robot, matrix () r Crr,  is not unique, but it can always be chosen so that () 2 ( ) rr M rCrr−,   be skew–symmetric, that is () 2 ( ) 0 n T rr Mr Crr x xrr x ⎡⎤ −,=,∀,,∈ ⎣⎦   \ (35) • The vector of gravity forces is the gradient of the potential energy, that is () () r r r g r ∂ = ∂ U (36) Now consider the total mechanical energy of the robot () () ()rr rr r,= ,+  EKU . Using properties (34)–(36), it can be shown that the following relation stands () T r rr r τ ,= .   E (37) And, as the rate of change of the total energy in a system is independent of the generalized coordinates, we can use (37) as a way of convert generalized forces between two different generalized coordinate systems. Robot Manipulators 34 For an n –dof robot manipulator, it is common to express the dynamic model in terms of the joint coordinates, i.e. n rq≡∈\ , this leads us to the well–known equation (Kelly et al., 2005): () ( ) () ( ) n M qq Cqqq gq qqq ττ + , + = , ,,,∈ .      \ (38) Subindexes have been omitted in this case for simplicity; τ is the vector of joint forces (and torques) applied to each of the robot joints. Now consider the problem of modeling the robot dynamics of a non–redundant ( 6n ≤ ) manipulator, in terms of the pose coordinates of the end–effector, given by 33 7 S T T T x p ξ ⎡⎤ =∈×⊂ ⎣⎦ \\ . Notice that in this case we have holonomic constraints. If 6n = then the only constraint is given by (9), so that: 2 () 1 T x γηεε =+ −; If 6n < then the pose coordinates require 6 n− additional constraints to form the vector 7 () n x γ − ∈ \ The equations of motion would be 7 () () ( ) () ( ) T xx x x x x M xx C xxx x xxx g x γ τλτ ∂ +,+ =+ ,,,,∈, ∂      \ (39) where matrices () x M x , () x Cxx, , () x x g can be computed from () M q , ()Cqq, , () g q in (38) using a procedure similar to the one presented in (Khatib, 1987) for the case of operational space. We get 1 1 1 †† () †††† † () † () () ( ()) () () () (())( ())() (())()() () (())() T xA A qh x TT x A AAAA qh x T A x qh x Mx J q MqJ q Cxx JqCqJqxJq Jq Mq q J x Jq gq g ξξ ξ ξξξξ ξ − − − = = = = ,= ,+ =    where () A J q ξ is the analytic Jacobian in (29). To obtain the previous equations we are assuming, according to (37), that () T TT T x Ax qx Jq q ξ ττ τ ==   so that 1 † () (()) T x A qh x Jq ξ τ τ − = =. Equation (39) expresses the dynamics of a robot manipulator in terms of the pose coordinates ( p , ξ ) of the end–effector. In fact, these are a set of seven equations which can be reduced to n , by using the 7 n− Lagrange multipliers and choosing a subset of n Unit Quaternions: A Mathematical Tool for Modeling, Path Planning and Control of Robot Manipulators 35 generalized coordinates taken from x . This procedure, however, can be far complex, and sometimes unsolvable in analytic form. In theory, we can use (32) to find a minimal system of equations in terms of any set of generalized coordinates. For example, we could choose the scalar part of the Euler parameters in each joint, i η , ( 12i…n=,, , ) as a possible set of coordinates. To this end, it is useful to recall that the kinetic energy of a free–moving rigid body is a function of the linear and angular velocities of its center of mass, v and ω , respectively, and is given by 11 () 22 TT vmvvH ωωω ,= + ,K (40) where m is the mass, and H is the matrix of moments of inertia (with respect to the center of mass) of the rigid body. The potential energy depends only on the coordinates of the center of mass, p , and is given by () , T pmp g =U where g is the vector of gravity acceleration. Now, using (30), (31), we can rewrite (40) as 1 () 2()() 2 T T T pmpEHE p ξξ ξ ξ ξ ξ ,, = +     K (41) Considering this, we propose the following procedure to compute the dynamic model of robot manipulators in terms of any combination of pose variables in task space: 1. Using (41), find the kinetic and potential energy of each of the manipulator’s links, in terms of the pose coordinates, as if they were independent rigid bodies. That is, for link i , compute () i i i i p ξ ξ ,,   K and (). i i p U 2. Still considering each link as an independent rigid body, compute the total Lagrangian function of the n –dof manipulator as 1 (,,) ( ) () n iii i ii i pp pp ξξ ξ ξ = ⎡ ⎤ ,= ,,− ⎣ ⎦ ∑    LKU where 7n ρ ∈ \ is a vector containing the pose coordinates of each link, i.e. ρ ξ ⎡⎤ ⎢⎥ ⎡ ⎤ ⎢⎥ =, = ⎢ ⎥ ⎢⎥ ⎣ ⎦ ⎢⎥ ⎣⎦ # 1 2 with i i i n x p x x x 3. Now use (9), for each joint, and the knowledge of the geometric relations between links to find 6n holonomic constraints, in order to form vector 6 () . n γρ ∈ \ 4. Write the constrained equations of motion for the robot, i. e. (32). 5. Finally, solve for the 6n Lagrange multipliers, and get de reduced (generalized) system in terms of the chosen n generalized coordinates. [...]... Velocity Inner Loop, Automatica, Vol 41, pp 1 423 -14 32 Kelly, R.; Santibáñez, V & Loría, A (20 05) Control of Robot Manipulators in Joint Space, Springer-Verlag, London Khalil, H K (20 02) Nonlinear Systems, Prentice–Hall, New Jersey Khatib, O (1987) A unified approach for motion and force control of robot manipulators: The operational space formulation IEEE Journal of Robotics and Automation Vol 3, pp 43–53... control tasks in robotics Proceedings of the VIII Mexican Congress on Robotics Mexico, D.F., October 20 06 Campa, R.; Camarillo, K & Arias, L (20 06) Kinematic modeling and control of robot manipulators via unit quaternions: Application to a spherical wrist Proceedings of the 45th IEEE Conference on Decision and Control, San Diego, CA, December 20 06 Craig, J J (20 05) Introduction to Robotics: Mechanics... ⎡ p V ( p,η , ε , v ) ≤ − ⎢ ε ⎥ Q ⎢ ε ⎢ ⎥ ⎢ ⎢ v ⎥ ⎢ v ⎣ ⎦ ⎣ ⎤ ⎥ − 1 αλ {K } 1 − η 2 ) ⎥ 2 m o ( ⎥ ⎦ where 1 ⎡ ⎤ 0 − α k Jp ⎥ ⎢αλm {K p } 2 ⎢ ⎥ 1 1 0 Q=⎢ αλm {K o } − α k Jo ⎥ ⎢ ⎥ 2 2 ⎢ ⎥ 1 ⎢ − 1α kJp λm {Kv } ⎥ − α kJo ⎢ 2 ⎥ 2 ⎣ ⎦ Unit Quaternions: A Mathematical Tool for Modeling, Path Planning and Control of Robot Manipulators 43 If α satisfies condition (59), then Q is positive definite matrix... (Campa, 20 05): V ( p, p,η , ε , ω ) = V p ( p, p) + Vo (η , ε , ω ) where V p ( p, p) and Vo (η , ε , ω ) stand for the position and orientation parts, given by V p ( p, p ) = ⎤ 1 T 1 T⎡ p p + p ⎢⎢⎢ K Pp + α KV p ⎥⎥⎥ p + α pT p, ⎣ ⎦ 2 2 1 − Vo (η , ε , ω ) = (η − 1) 2 + ε T ε + ω T K Po1ω + βε T ω, 2 and α , β , satisfy 0 < α < λm{KV p } − ⎧ 2kv λm {K Po }λm {K Po1} ⎫ ⎪ ⎪ − ⎨ ⎬ 0 < β < min ⎨ 2 m ⎧ K... critical review with experiments Robotica, Vol 16, pp 565– 573 Caccavale, F.; Siciliano, B & Villani, L (1999) The role of Euler parameters in robot control, Asian Journal of Control, Vol 1, pp 25 –34 Campa, R (20 05) Control de Robots Manipuladores en Espacio de Tarea Doctoral thesis (in Spanish), CICESE Research Center, Ensenada, Mexico, March 20 05 Campa, R & de la Torre, H (20 06) On the representations... & Sudhoff S D (20 02) Analysis of Electric Machinery and Drive Systems Wiley-Interscience Kuipers, J B (1999) Quaternions and rotation sequences Princeton University Press Princeton, NJ Lin, S K (1995) Robot control in Cartesian space, In: Progress in Robotics and Intelligent Systems, Zobrit, G W & Ho, C Y (Ed.), pp 85– 124 , Ablex, New Jersey 48 Robot Manipulators Lu, Y J & Ren G X (20 06) A symplectic... Dynamics, Vol 27 , No 1, pp 51-57 Luh, J Y S., Walker M W., Paul R P C (1980) Resolved–acceleration control of mechanical manipulators IEEE Transactions on Automatic Control Vol 25 , pp 486–474 Natale, C (20 03) Interaction Control of Robot Manipulators: Six degrees–of–freedom Tasks, Springer, Germany Rooney, J & Tanev, T K (20 03) Contortion and formation structures in the mappings between robotic jointspaces... jointspaces and workspaces Journal of Robotic Systems, Vol 20 , No 7, pp 341–353 Sciavicco, L & Siciliano, B (20 00) Modelling and Control of Robot Manipulators, Springer– Verlag, London Shivarama, R & Fahrenthold, E P (20 04) Hamilton’s equation with Euler parameters for rigid body dynamics modeling Journal of Dynamics Systems, Measurement and Control Vol 126 , pp 124 -130 Shoemake, K (1985) Animating rotation... Systems, Vol 10, no 2, pp 47-53 Xian, B.; de Queiroz, M S.; Dawson, D & Walker, I (20 04) Task–space tracking control of robot manipulators via quaternion feedback IEEE Transactions on Robotics and Automation Vol 20 , pp 160–167 Yuan, J S C (1988) Closed-loop manipulator control using quaternion feedback IEEE Journal of Robotics and Automation Vol 4, pp 434–440 3 Kinematic Design of Manipulators Marco... Planning and Control of Robot Manipulators 47 operational space, Proceedings of the IEEE International Conference on Robotics and Automation, pp 27 71 27 78, Nagoya, Japan, May 1995 Barrientos, A.; Peñín, L F.; Balaguer, C & Aracil, R (1997) Fundamentos de Robótica, McGraw-Hill, Madrid Caccavale, F.; Natale, C.; Siciliano, B & Villani, L (1998) Resolved–acceleration control of robot manipulators: A critical . and (16) iteratively to obtain expressions equivalent to (24 ) and (25 ): 01 1 12 01 2 1 01 2 12 1 12 1 01 3 2 01 3 010*0 12 2 1 12 2 1 21 1 () () ( ) ( ) ()() . n n nn n nn n nn n nn n q pq p ppp ξξξ. of ()pq and ()Rq 0 11 12 () n n R qRR R − = " (24 ) 0 121 01 32 210 12 1 12 2 1 121 () nn n n nn nn pq R R R R R R … R pppp −− − − −− − =+ +++"" (25 ) An alternative method. product of two homogeneous matrices 12 SE(3)TT,∈ is given by 21 21 2 21 12 21 SE(3) 0101 0 1 TT T RR RRR pp pp TT + ⎡⎤⎡⎤⎡ ⎤ ==∈. ⎢⎥⎢⎥⎢ ⎥ ⎣⎦⎣⎦⎣ ⎦ (22 ) In terms of the D–H parameters, the