Seite - 211 - in Proceedings - OAGM & ARW Joint Workshop 2016 on "Computer Vision and Robotics“

Thestartingpoint is theProjectionEquationofanentirearmasakinematicchain Nj∑ b=1 [ ( ∂vs ∂q˙j )T ( ∂ωs ∂q˙j )T ] b [ p˙+ ω˜Rp− fe L˙+ ω˜RL−Me ] b (1) with indexj=1,2,3 foreacharm.Nj is thenumberofbodiesand q˙j= ( q˙p,j q˙a,j )T isdescribing velocity of each subsystem. Furthermore, vs,ωs are the absolute velocities of the center of gravity (CoG),ωR is the angular velocity of a chosen reference frame,p,L are the linear and angularmo- menta, respectively,while fe,Me are theapplied forces of eachbody. Equation1 leads to themotion equationofeacharmmodeledasa subsystem Mjq¨j+Cjq˙j−Qj=uj. (2) Mj is themassmatrix,Cj is the Coriolis andCentrifugalmatrix,Qj are the remaining forces and uj= ( 0 Mj )Twith themotor torqueMj describes the control forces of each arm. Furthermore, the equations of each arm (Eq. 1) can be assembled to themotion equation of the unconstrained system M(q)q¨+C(q, q˙)q˙+Q(q, q˙)=u, (3) withqas thegeneralizedcoordinateswritten inanarbitrarysequence, f.e. q= ( qp,1 qp,2 qp,3 qa,1 qa,2 qa,3 )T . (4) MoreoverM is themassmatrix ,C isCoriolis andCentrifugalmatrix ,Qare the remainingand u= ( 0 c ) , u∈Rn, c∈Rm, c=(M1 M2 M3 )T . (5) are thecontrol forces.Vectorccontains the threemotor torques. Detailedcalculationsaboutdynamicalmodelingof subsystemscanbefound in [1], [3]. 3.2. SubsystemConstraints As described in the section before, the arms are modeled bymeans of subsystemmodeling. Af- terwards, these motion equations are assembled to an entire unconstrained system. Note that the sequence of joint coordinates q (Eq. 4) is arbitrary. In the unconstrainedmodel the arms are not connected to theplatform.Therefore,rgeometric h(q)=0, h∈Rr (6) respectivelykinematicconstraints (with theJacobianmatrixJ) h˙(q)= ( ∂h ∂q ) q˙=Jq˙=0, J∈Rr,n (7) have tobebuilt toconnect themtogether. Thegeometricalconstraints represents the linkagebetween the revolute joints and theEE.Thus, two independent loops, eachwith two independent constraints (⇒ r = 4) can be located. Finally, after installing the constraint forces JT(q)λ into themotion equationof theunconstrainedsystem, theentiremodelhasa structure like M(q)q¨+C(q, q˙)q˙+Q(q, q˙)+JT(q)λ = u (8) Jq˙ = 0. (9) Equation8 is theLagrangianmotionequationoffirst kind. 211