Nội dung

Vietnam Journal of Mechanics, VAST, Vol. 41, No. 2 (2019), pp. 141 – 155 DOI: https://doi.org/10.15625/0866-7136/13073 KINEMATIC AND DYNAMIC ANALYSIS OF A SERIAL MANIPULATOR WITH LOCAL CLOSED LOOP MECHANISMS Chu Anh My∗ , Vu Minh Hoan Le Quy Don University of Technology, Hanoi, Vietnam ∗ E-mail: mychuanh@yahoo.com Received: 05 September 2018 / Published online: 01 April 2019 Abstract. This paper addresses the kinematic and dynamic modelling and analysis for a robot arm consisting of two hydraulic cylinders driving two revolute joints of the arm. The two cylinders and relevant links of the robot constitute two local closed kinematic chains added to the main robot mechanism. Therefore, the number of the generalized coordinates of the mechanical system is increased, and the mathematical modelling is more complex that requires a formulation of constraint equations with respect to the local closed chains. By using the Lagrangian formulation with Lagrangian Multipliers, the dynamic equations are first derived with respect to all extended generalized coordinates. Then a compact form of the dynamic equations is yielded by canceling the Multipliers. Since the obtained dynamic equations are expressed in terms of independent generalized coordinates which are selected according to active joint variables of the arm, the equations could be best suitable for control law design and implementation. The simulation of the forward and inverse kinematics and dynamics of the arm demonstrates the motion behavior of the robot system. Keywords: hydraulic robot; robot kinematics; robot dynamics; local closed mechanism. 1. INTRODUCTION Most of industrial manipulators commonly used in industries are usually actuated by electric motors, such as the welding robots, the assembly robots, etc. The use of electric motors actuating active robot joints possesses several advantages: easy to control, high positioning accuracy, and high flexibility. However, if a manipulator is designed to operate in a large workspace with high loading capability, the use of electric motors for the design could lead to a very heavy architecture of the robot. Counterweights could be added to balance to shaking forces. In that case, hydraulic cylinders driving robot joints is often used for the design. The presence of hydraulic cylinders increases the stiffness of robot structure so that the robot is capable of handling heavy parts in a larger operational space. Moreover, the counterweights could be avoided since the cylinders actuating revolute joints play a role of auxiliary links appended to the main structure. c 2019 Vietnam Academy of Science and Technology 142 Chu Anh My, Vu Minh Hoan Though there will be advantages when using hydraulic cylinders for robot designs, the presence of cylinders in a robot architecture involves complex procedures for the mathematical modelling, analysis and control. The addition of cylinders to the conventional serial kinematic chain of robot arm architecture could constitute local closed kinematic chains within the entire robot mechanism. Issues related to the hybrid serial–parallel feature of the robot structure, the geometry, the mass and the inertia of cylinders must be taken into account. In the literature, numerous works have been carried out to investigate several aspects related to the dynamic modeling and analysis of serial manipulators and parallel robots [1–21]. The fundamentals of kinematic and dynamic modelling and analysis of serial manipulators can be found in [1, 2], where Denavit-Hartenberg approach was mostly used for the kinematic modelling and D’Alembert–Lagrange Formulation for the dynamic modelling. As for more complex robotic systems, there has been a number of researches dealing with different issues related to the kinematics and the dynamics as well. The research presented in [3] studied the kinematic and dynamic modelling for closed chain manipulators, [4] addressed algorithms for the dynamic analysis of serial robots having a large number of joints, [5] investigated the inverse kinematics and dynamics of the redundant robots, [6] studied the dynamics of mobile serial manipulators. In parallel with researches concerning with the serial robot dynamics, a massive number of researches related to the dynamics and control of parallel robots has been addressed as well such as publications [7–21]. The researches [8,10,13] investigated methods for the inverse and forward dynamic modelling and analysis of the 3-PRS type parallel manipulators. The Screw theory was used in [9], and the matrix approach was employed in [11] for the dynamics and control of the parallel robots. A general solution to the problem of dynamic modelling and analysis of parallel robots was presented in [12]. For the issue of control law design and development, [14] investigated a model-based technique, [15–17] addressed the robust control algorithms, whereas the sliding mode control was designed in [18]. The earlier foundation for control law designed for the parallel robots one can find in [19], in which the computed torque technique was used as for those of the common serial manipulators. The control law design for a 6-DOF hexapod robot was presented in [20], and a technique to improve the control quality for parallel robots was investigated in [21]. In recent years, there have been several attempts to investigate the dynamic modelling and analysis of the hybrid serial–parallel robots [22–31]. Ibrahim & Khalil [22] studied recursive solutions for obtaining the inverse and direct dynamic models of hybrid robots that are constructed by serially connected non-redundant parallel modules. Pisla et al. [23] addressed geometric and kinematic models of a surgical hybrid robot used for camera and active instruments positioning. Tanev [24] investigated a kinematic analysis of a hybrid parallel–serial robot manipulator which consists of two serially connected parallel mechanisms. Each mechanism has three degrees of freedom. Xu et al. [25] presented a particular hybrid manipulator for computer controlled ultra-precision freeform polishing. This hybrid manipulator is composed of a 3-DOF parallel module, a 2-DOF serial module and a turntable providing a redundant DOF. Zeng & Fang [26] studied a method of structural synthesis and analysis of serial–parallel hybrid mechanisms with Kinematic and dynamic analysis of a serial manipulator with local closed loop mechanisms 143 spatial multi-loop kinematic chains based on the displacement group theory. Zeng & Ehmann [27] presented a design method for parallel hybrid-loop manipulators, which is based on the constrained motion properties of related spatial over constrained linkages and general parallel mechanisms. Zhang & Gao [28] investigated a method for performance optimization of a 5-DOF compliant hybrid parallel robot micromanipulator. The issues related to kinematics modelling and analysis for the robots were also addressed in [29–31]. Though there has been a number of researches dealing with the dynamic modelling and analysis of different robot architectures, little attention has been paid to robots actuated by hydraulic actuators. Issues related to the mathematical modelling of manipulators which have local closed kinematic chains constituted by adding hydraulic cylinders seem to be overlooked. In this paper, the dynamic modelling and analysis of such a robot arm having two hydraulic cylinders constituting two local closed chains appended to a conventional arm mechanism are addressed. First, the kinematic equations and constraints equations are derived, by using the notation of Denavit–Hartenberg. Second, Lagrange’s formulation is used to establish differential equations of motion, with the use of Lagrangian multipliers. Finally, based on the formulated DAEs, a technique of Lagrangian multipliers cancellation is employed to obtain ODEs describing the dynamics of the entire robot system. Finally, the dynamic analysis is carried out with the help of the symbolic and numeric computing Maple and Matlab softwares. As a result, the proposed investigation could be used further for the purposes of dynamic simulation and control law design for a class of hydraulic architecture robots. 2. KINEMATICS Let’s consider a robot in Fig 1. Different from the conventional robot arm architecture, the considering manipulator has two hydraulic cylinders. The first cylinder AC is to drive the revolute joint B (joint variable θ2 ), and the second one EG drives the joint F (joint variable θ5 ). Due to the use of the cylinders, two closed kinematic chains are constituted: ABCD and EFGH. The robot have three DOFs and three active joints. The first active joint is a revolute joint (joint 1) with joint variable θ1 . The second and the last active joints are prismatic joints (with the joint variables d4 and d7 ) because of the relative motion of the two parts of the hydraulic cylinders. Therefore, among eight joint variables θ1 , d4 , d7 , θ2 , θ3 , θ4 , θ5 and θ6 (see Fig. 1), only three joint variables θ1 , d4 and d7 are independent. In Fig. 1, Oxyz is defined as a reference frame, and OE x E y E z E as an end effector frame, of which the origin locates at point E. Other local coordinate systems attached to links are defined by using Denavit–Hartenberg (DH) notations. l1 , l10 , l11 , l2 , l21 , l22 , l23 , l3 , l5 , l51 , l6 are geometric parameters of links. The main mechanism of the arm includes three links: link 1, link 2, and link 5. The first local closed chain (ABCDA) involves link 1, link 2, link 3 and link 4. The second local closed chain (KFGHK) relates to link 2, link 5, link 6 and link 7. Based on all the coordinates in Fig. 1 which are defined with respect to the DH convention, the parameters of the robot links are presented in the following tables. 144 My, Vu Minh Hoan Kinematic and dynamic analysisChu of a Anh serial manipulator with local closed loop mechanisms 3 Fig 1. A hybrid serial-parallel robot arm Fig. 1. A hybrid serial-parallel robot arm In Fig. 1, O0x0y0z0 is defined as a reference frame, and OExEyEzE as an end effector frame, of which the origin locates at point E. Other local coordinate systems attached to links are defined by using Table 1. The DH parameters reTable 2. The DH parameters related to the main mechaDenavit – Hartenberg (DH) notations. l1 , l10 , l11 , l2 , l21 , l22 , l23 , l3 , l5 , l51 , l6 are geometric parameters of lated to the main mechanism nism links. The main mechanism of the arm includes three links: link 1, link 2, and link 5. The first local closed chain (ABCDA) involves link 1, link 2, link 3 and link 4. The second local closed chain (KFGHK) i relates θi to link di 2, link ai 5, linkα6i and link 7. Based i θon ai αi in Fig. 1i which θi aredidefined ai withαi the i all d i coordinates respect to the DH convention, the parameters of the robot links are presented in the following tables. 1 θ1 Tablel11. The0DH parameters π/2 3’ to the θ3 main0mechanism l3 0 1’ 0 0 0 −π/2 related 2 5 i l2 l5 0 0 θ2 θ5 0 3 0 π/2 0 1 π/2 θ d a 0 0 i d7 3’ l6 0 03 i α l11 0 l22 θ6 π/2 0 3 4 0 π/2 0 −π/2 l3 0 /2 0 0 d4 0 0 2 5 /2  / 2 π/2 0 θ di ai αi 0 0 0 l21 l51 0 0 i The DH parameters i i related to the i local closed chain ABCDA i Table 2. 6’ 6 7 0 i0 4 0 d4 0 −π/2 2 θ2 0 l1 0  / 2 1 1 0 l2 related 0 the local closed chain KFGHK 2 Table 3. The 2DH parameters l 0 0 5 5 5 i θ5 1’ 0 1 0 2 2 0 l11 0 0 0 l22  / 2 /2 0 Based on the parameters in Tabs. 1–3, the transformation matrices describing the motion of any two successive coordinate systems can be derived accordingly. For the 3. The DH parameters related the local closed KFGHK as follows mainTable mechanism, the cumulative matrix H0E ischain computed   H0E  =  cos θ1 cos (θ2 + θ5 ) − cos θ1 sin (θ2 + θ5 ) sin θ1 − cos θ1 (l5 sin θ2 sin θ5 − l5 cos θ5 cos θ2 − l2 cos θ2 ) sin θ1 cos (θ2 + θ5 ) − sin θ1 sin (θ2 + θ5 ) − cos θ1 − sin θ1 (l5 sin θ2 sin θ5 − l5 cos θ5 cos θ2 − l2 cos θ2 ) sin (θ2 + θ5 ) cos (θ2 + θ5 ) 0 l5 sin θ2 cos θ5 + l5 cos θ2 sin θ5 + l2 sin θ2 + l1 0 0 0 1  .  (1) Kinematic and dynamic analysis of a serial manipulator with local closed loop mechanisms 145 The matrix H0E describes the kinematics of motion of one active joint and two passive joints. In other words, H0E characterizes the kinematic relationship of the end link of the arm with the use of only one independent joint variable θ1 and other two dependent joint variables θ2 and θ5 . In order to determine the kinematic relationship with all independent joint variables, the constraints raised due to the local closed chains need to be taken into account. 2.1. Constraint equations Consider the first local closed chain (ABCDA). In order to write constraint equations regarding of this closed feature of the mechanism, a virtual cut at the joint C is made, so that transformation matrices written with respect to two routines ABC and ADC, respectively, are yielded as   cos θ2 − sin θ2 0 l22 cos θ2  sin θ2 cos θ2 0 l22 sin θ2 + l11  , H ABC =  (2)  0  0 1 0 0 0 0 1   0 d4 cos θ3 + l3 cos θ3 0 d4 sin θ3 + l3 sin θ3  . H ADC (3)  1 0 0 1   l22 cos θ2 In the first routine, the position C1 of the point C is C1 =  l22 sin θ2 + l11 , and in 0   d4 cos θ3 + l3 cos θ3 the second one, the point C is calculated as C2 =  d4 sin θ3 + l3 sin θ3 . Therefore, two 0 independent constraint equations involved in this closed chain are determined as − sin θ3 − cos θ3  cos θ3 − sin θ3 =  0 0 0 0 f 1 = l22 cosθ2 − (d4 + l3 ) cosθ3 = 0, f 2 = l22 sinθ2 − (d4 + l3 ) sinθ3 + l11 = 0, sin (θ3 ) = (l3 + d4 )2 + (l11 )2 − (l22 )2 , 2 (l3 + d4 ) l11 2 − l2 (l3 + d4 )2 − l11 22 sin (θ2 ) = . 2l11 l22 (4) (5) Similarly, the other two constraint equations involved in the second local closed chain KFGHK are obtained as f 3 = l51 cos θ5 + l21 − d7 cos θ6 − l6 cos θ6 = 0, f 4 = l51 sin θ5 − d7 sin θ6 − l6 sin θ6 = 0, (6) 4 11 22    3 sin 2    2  l3 2ld 4l  l112  l22 2  11 22  sin 2     2l11l22  equations involved in the second local closed chain KFGHK are Similarly, the other two constraint Similarly, obtained as the other two constraint equations involved in the second local closed chain KFGHK are obtained as  f3  l51 cos 5  l21  d7 cos 6  l6 cos 6  0   f3  l51 cos 5  l21  d7 cos 6  l6 cos 6  0  f4  l51 sin 5  d 7 sin 6  l6 sin 6  0 (6) (6)    f 4  l51 sin 5  d 7 sin 6  l6 sin 6  0 2 2 2   d7Anh l6 Chu  My, 2l51 VuMinh 2l21 Hoan2      l6  d7   l51   l21   cos  5 2l51 l21   cos 5   2l51 l21  (7)  2 22 2 2 (7) 2   l + − l − ( ) ) l  d  l  2d 2l(21 2 ( l21 )       6 7 51  6 7 51 cos l  d  l  l       cos ,  ( θ ) =  cos665  6 2l217l6 2d(517l51 ) (l2121)   2l21 l6  d7  (7) (l6 + d7 )2 − (l51 )2 + (l21 )2 cos (θ6 ) = . 2 (l21 ) (l6 + d7 ) Forward kinematicssimulation simulation Forwardkinematics 2.2. Forward kinematics simulation  π0.1 0.005 t and Based thethe inputs given as as1 t t  t ,dt4 t,d4 0.1 t   0.005 t and Based ononEqs. Eqs.(1,5,7), (1,5,7),with with inputs given 1 18 18as θ1 (t) = Based on Eqs. (1), (5) and (7), with the inputs given × t, d4 (t) = 0.1 + 18T T  xEtx yEtt yzEttz and dd70.005 0.02  t ,t ,thetheposition thethe endend effector t   0.1 0.02 0.1 positionof of effectorE  E  the 7 t  × E  end  EEt= t and d x E velocity t) y Evelocity [and (the (t) z E (t)]T  E effector 7 ( t ) = 0.1 + 0.02 × t, the position of the T T T and t can = [be ẋ Ebe be numerically computed and in the (tnumerically ) ẏ E (t) żcomputed (t)] can numerically and and shown in theinfollowing Figs. Figs. (2,3), andshown Fig. EE  xxEthe t t yvelocity shown the following (2,3), and Fig. yE Ett z EzEtĖ E computed E   can following Figs. 2, 3 and 4, respectively . 4,respectively respectively. . 4, 146 2 xE,yE,zE xE,yE,zE[m] [m] 2 1.8 1.6 z [m] z [m] 1.8 1.6 1.4 1.4 1.2 0.1 1.2 0.1 -0.3 0 0 -0.3 -0.2 -0.1 -0.2 -0.2 -0.1 -0.4 x [m] -0.3 x [m] -0.4 0 -0.1 y [m] y [m] 0 T 3. A trajectory of E in the workspace Anh Vu 3.Fig FigE2.=E[ x (xtE)yt  y(tE)tz zMy t T Chu, Hoan Minh Fig. A trajectory trajectoryofofE Einin workspace Et  T  Fig. 2. ( )] E E E Fig 3. A thethe workspace Fig 2. E   xE t  yE t  z E t  dxE,dyE,dzE [m/s] 6 -0.1 -0.3 -0.2 T Fig 4. E   x t  y t  z t  Fig. 4. Ė = [ ẋ E (E t) ẏ EE (t) żEE (t)] T Inverse kinematics simulation Given a trajectory of the end-effector in the work space, E   xE t  yE t  z E t  , the joint T variables are calculated as follows:   1/ 2    1  arcsin A / 1  A2      2 2   d 4  2l11l22 sin 2  l11  l22  l3 ,  (8) Kinematic and dynamic analysis of a serial manipulator with local closed loop mechanisms 147 2.3. Inverse kinematics simulation Given a trajectory of the end-effector in the work space, E = [ x E (t) y E (t) z E (t)] T , the joint variables are calculated as follows 
1/2  θ1 = arcsin A/ 1 + A2 , q 2 + l2 − l , (8) d4 = 2l11 l22 sin θ2 + l11 3 22 q 2 + l2 − l , d7 = 2l51 l21 cos θ5 + l51 6 21 y2M (z M − l1 )2 − l22 − l52 + = C, 2l2 l5 B2 2l2 l5 √ (9)
2
mn ± ∆0 0 2 2 2 2 2 ;∆ = m n − n + p m −p , sin(θ2 ) = ( n2 + p2 ) s p yE A2 2 ;B= ; m = z − l ; n = ( l C + l ) ; p = l where A = ( ) 2 5 1−C . E 1 with local 5closed loop 2serial manipulator Kinematic and dynamic analysis of a mechanisms 7 7 x 1 + A KinematicEand dynamic analysis of a serial manipulator with local closed loop mechanisms Figs. 5 and 6 show the values of θ1 (t) , d4 (t) , d7 (t), θ2 (t) and θ5 (t) computed with respect to attrajectory given as 2 2  2 55   t2  t  200  1  1  t 2 2 2 2   t    t   t  t   t t 55 2 92160000 cos  cos   2560000    t  100  10000    t  t  200   t  25  100 2522000  x E  xE   1  1    cos     t t cos 92160000 10000 2560000 2000                   1600 32 64 12800000 8000 2  18   18  cos(θ5 ) =    32 64  12800000   18 r  18   8000  2 1600    r  2 t (t + 200)  2 2 πt πt  2 2  xE = 1 2 1 + t2 + t − 55 2 92160000 − − 10000 − cos t + 100 ( ) t2  t  t12800000 200 1 t  12  t 1600  t 18 1 1 8000 2  64t   t18 t  200cos   t  2560000 − (t + 252 ) 2− 2000 2 , 2 t 55 t32 55 2      sin 92160000   t  100 10000   t 25 sin  sin  2560000 t 25 2000     2560000  2000  yE   t 100   10000  yE     r  2 32 sin    92160000  2 64 32  18r 8000 12800000    2 5518 11600  t 64 2     18πt t t12800000 (t + 200)  18 πt   2 8000  1  1600 2 2 sin sin 92160000 − (t + 100) − 10000 − 2560000 − (t + 25) − 2000 ,   yE = 8000 2 + 1600 + 32 − 64 3 18 3 2 4 2 4 18 12800000 2 2 2 2 t t t 377t377t 23t 23t 7 7 t 1 2 2 2 2  zE  1z E  2560000 r 92160000 r 2560000  25   2000 4   t  25 92160000   t 100  10000  3  2      t 100   10000    t2000 2 2 t12800000 1 t 51200 377t 512000 23t2560 7 10 10  z = 12800000  2 2 12800000 12800000 51200 + 2560000 − t + 25 − 2000 92160000 − t + 100 − 10000 + + 512000 − +2560 . ( ) ( )  E 12800000  12800000 51200 512000 2560 10                3 3 Teta Teta 2 2 Teta 5 Teta 5 2.5 Teta1[rad],d4,d7 [dm] Teta1[rad],d4,d7 [dm] 2.5 2 2 1.5 1.5 1 1 0.5 0.5 0 0 0 0 Fig 5. The curves 1 t , d4 t  and d7 t  Fig. 5. curves The curves d4 (td)7and 1 t ,θd1 4(tt), and Fig 5. The t  d7 (t) 5 5 10 times [s] 10 times [s] 15 15 Fig 6. The curves 2 t  and 5 t  Fig. and θ55 (tt) Fig6.6.The Thecurves curves θ22(tt) and To validate the inverse kinematic computation, a numerical experiment is carried out. The robot configurations at t = 0 sec and t = 10 sec are shown in Figs. 7 and 8. In Teta1[rad],d4,d7 [dm] Teta1[rad],d4,d7 [dm] 2 2 1.5 1.5 1 1 0.5 0.5 0 0 0 0 5 5 10 10 1515 times times [s] [s] 1 t1,td4, dt  and t  and d 7 dt7t  FigFig 5. The 5. The curves curves 4 148 Fig Fig 6. 6. The The curves curves22t  tand  and55tt  Chu Anh My, Vu Minh Hoan FigFig 7. The 7. The robot robot configuration configuration at tat=t 0= 0 Fig Fig 8. 8. The The robot robot configuration configuration atat t= t= 10sec 10sec Fig. 7. The robot configuration at t = 0 Fig. 8. The robot configuration at t = 10 sec Figs. 7 and 8, the two revolute joints θ2 and θ5 are driven with two hydraulic cylinders which represent the active joints d4 and d7 . All the parameters of the robot are shown in Figs. 7 and 8 as well. 3. DYNAMICS Let’s denote s = [θ1 d4 d7 θ2 θ3 θ5 θ6 ] T is the vector of redundant generalized coordinates; q = [θ1 d4 d7 ] T is the vector of independent generalized coordinates; z = [θ2 θ3 θ5 θ6 ] T is the vector of dependent generalized coordinates;  T f = f1 f2 f3 f4 is the vector of constraint equations; ∂f Φs = is the Jacobian matrix of constraint equations;  ∂s T λ = λ1 λ2 λ3 λ4 is the vector of Lagrangian Multipliers;  T τ = τ1 F4 F7 0 0 0 0 is the vector of applied torque/forces. By using the Lagrangian formulation, the system of equations of motion including constraint equations for the robot can be written as M (s, t) s̈ + C (s, ṡ, t) ṡ + g (s, t) + ΦsT λ = τ (t) , (10) f (s, t) = 0. (11) The global mass matrix can be computed as [1] 7 M (s, t) = ∑   mi JTTi JTi + JTRi Ai Ii AiT JRi , (12) i =1 where mi and Ii are the mass and the inertia of body i, respectively; JTi and JRi are translational and rotational Jacobian matrices of body i, accordingly. Kinematic and dynamic analysis of a serial manipulator with local closed loop mechanisms 149 The angular velocity of the links are calculated as follows:       θ̇2 sinθ1 θ̇3 sinθ1 0     ω1 =  0  , ω2 =  −θ̇2 cosθ1  , ω3 = ω4 =  −θ̇3 cosθ1  , θ̇1 θ̇1 θ̇1 

 sin θ1 θ̇2 + θ̇6 sin θ1 θ̇2 + θ̇5

   ω5 =  − cos θ1 θ̇2 + θ̇5  , ω6 = ω7 =  − cos θ1 θ̇2 + θ̇6  . θ̇1 θ̇1  The vectors of the mass center of the links are determined as follows:    1 1    2 l2 cos θ1 cos θ2   2 l3 cos θ1 cos θ3 0     1   1 rC1 =  0 , rC2 =  l2 sin θ1 cos θ2 , rC3 =  l3 sin θ1 cos θ3  2   2 l1 /2   1  1 l2 sin θ2 + l1 l3 sin θ3 + l10 2 2   rC4     =     l4 cos θ1 cos θ3 l3 + d 4 − 2   l4 l3 + d 4 − sin θ1 cos θ3 2   l4 l3 + d 4 − sin θ3 + l10 2      ,    1    2 l5 cos θ1 cos (θ2 + θ5 ) + l2 cos θ1 cos θ2     1 , r =  C5  l5 sin θ1 cos (θ2 + θ5 ) + l2 sin θ1 cos θ2   2   1  l5 sin (θ2 + θ5 ) + l2 sin θ2 + l1 2  1  2 l6 cos θ1 cos (θ2 + θ6 ) + l23 cos θ1 cos θ2     1  rC6 =  l6 sin θ1 cos (θ2 + θ6 ) + l23 sin θ1 cos θ2 ,  2    1 l6 sin (θ2 + θ6 ) + l23 sin θ2 + l1 2    l7 l + d − cos θ1 cos (θ2 + θ6 ) + l23 cos θ1 cos θ2 6 7  2     l7  =  l6 + d 7 − sin θ1 cos (θ2 + θ6 ) + l23 sin θ1 cos θ2  2     l7 sin (θ2 + θ6 ) + l23 sin θ2 + l1 l6 + d 7 − 2     ,    rC7 The matrix of Coriolis and Centrifugal terms is calculated as [1]  T ∂M (s, t) 1 ∂M (s, t) C (s, ṡ, t) = (En ⊗ ṡ) − (ṡ ⊗ En ) , ∂s 2 ∂s      .    (13) 150 Chu Anh My, Vu Minh Hoan The potential energy of the system can be calculated as follows ∏=         1 1 1 l m1 gl1 + m2 g l2 sin θ2 + l1 + m3 g l3 sin θ3 + l10 + m4 g l10 + l3 + d4 − 4 sin θ3 2 2 2 2     1 1 + m5 g l1 + l2 sin θ2 + l5 sin (θ2 + θ5 ) + m6 g l1 + l23 sin θ2 + l6 sin (θ2 + θ6 ) 2 2     l7 + m7 g l1 + l23 sin θ2 + l6 + d7 − sin (θ2 + θ6 ) . 2 Based on the calculation of the total potential energy of the entire system, the terms related to the gravity effect is calculated as  ∂Π g (s, t) = ∂s T . (14) In order to analyze the dynamic behavior of the robot system, the DAEs (10, 11) need to be transformed in a way that the Multipliers are cancelled. ∂f in the following form Rewrite Φs = ∂s Φs =  Φq Φz   = ∂f ∂q ∂f ∂z  . (15) Let’s consider the following expression T R =  E, −ΦqT  1 Φ− z T  . (16) Hence RT ΦsT = 0. (17) Note that      T Φs =      0 − cos θ3 0 −l22 sin θ2 l3 sin θ3 + d4 sin θ3 0 0 0 − sin θ3 0 l22 cos θ2 −l3 cos θ3 − d4 cos θ3 0 0 0 0 − cos θ6 0 0 −l51 sin θ5 l6 sin θ6 + d7 sin θ6 0 0 − sin θ6 0 0 l51 cos θ5 −l6 cos θ6 − d7 cos θ6      ,     (18)