Page - 60 - in Joint Austrian Computer Vision and Robotics Workshop 2020

tions. 2.DynamicModeling In general, a multibody model describes the full dynamical behavior of a system. The equations of motion can be developed by the Projection Equa- tion [2]. This method is efficient to derive the dy- namicof recurrent subsystems. A typical subsystem in robotics consists of structural elements andactua- tors. 2.1.SubsystemModeling Asalreadymentioned,modelingbymeansofsub- systemsisuseful forroboticsystems.Moreover, con- straint forces and torquesQc for coupling subsys- tems can be determined without additionally effort. TheProjectionEquation insubsystemrepresentation isgivenby Nsub∑ n=1 ( ∂y˙n ∂q˙ )⊤ {Mny¨n+Gny˙n−Qn}︸ ︷︷ ︸ Qcn =0, (1) Qcn= Nn∑ i=1 [( ∂vc ∂y˙n )⊤(∂ωc ∂y˙n )⊤] i ×[ p˙+ ω˜Rp− fe L˙+ ω˜RL−Me ] i (2) withNsub subsystems andNn bodies. The absolute velocity of the center ofmass and the angular veloc- ityof the i-thbodyare represented byvc,i∈R3 and ωc,i ∈ R3,ωR,i ∈ R3 is the angular velocity of a chosen body fixed reference frame. The vector of linearmomentumand thevector of angularmomen- tumaregivenbypi=mivc,i andLi=Jciωc,i.Mass and inertia tensor are denotedmi andJci ∈R3,3, re- spectively. Impressed forces and torques are given by fei ∈R3 andMei ∈R3. The vectorq∈RN rep- resent theNminimalcoordinates of thesystem. The describingvelocities of each subsystemaregivenby y˙n= ( v⊤0 ω ⊤ F q˙ )⊤ n ∈R7, (3) where v0,n is the translational velocity of the cou- pling point,ωF,n is the guidance rotational velocity and q˙ is the relative joint velocity of the n-th sub- system. In this paper, the 3 rotational coordinates q=(q1 q2 q3)⊤ are introduced asDOFs.Moreover, 3 subsystems are considered to derive the equations of motion. The describing velocities of the second subsystemcanbe seen inFig. 2. 2.2. JointForcesandTorques Asshownby2.1, theoccurring reactionforcesand torques of then-th subsystemcanbedeterminedby Qcn=Mny¨n+Gny˙n−Qn, (4) with themassmatrix of the subsystemMn ∈R7,7, the matrix of centrifugal and Coriolis forcesGn ∈ R7,7 and thevector of forceson the subsystemQn∈ R7. Without projection into minimal coordinates, joint forces and torques regarding the three subsys- temsaregivenby  1Qc2Qc 3Qc   =   E T⊤21 T⊤310 E T⊤32 0 0 E     Qc1Qc2 Qc3   . (5) Thematrix Tnp=   Rnp Rnppr˜⊤pn Rnppr˜⊤pneD0 Rnp RnpeD 0 0 0   (6) mapsaquantityfromthepredecessor framep intothe frameof interestn.Rnp∈R3,3 is the rotationmatrix to transformcoordinatevectors resolved in the frame p into framen, prpn∈R3 is thedisplacement vector fromthecouplingpoint at thepredecessor framep to thaton framenand thevectoreD∈R3 is theaxisof rotation. The transformationmatrix is a result of the kinematical chain [4]. 3.ProblemDefinition This section reports on different optimization tasks for point-to-point (PTP) trajectory planning. In this paper, the optimal dynamic motion problem is transformed into a static parametric optimization problem. The joint trajectories are representedbyB- splinecurvesparameterizedbyasetofcontrolpoints d = (d1,1 ··· d1,nd2,1 ··· d2,nd3,1 ··· d3,n)⊤ , i.e. q = q(d) and thus q˙ = q˙(d) and q¨ = q¨(d). For practical applications, several physical restric- tions of the robotic systemhave tobe considered. In this paper, constraints regarding to initial and final state, minimal andmaximal joint angles as well as maximalmotor velocities and torques are regarded. Themathematical formulation of the constraints are given in Eq. (8)–(14). The task of trajectory opti- mization leads to a non-linear optimization problem (NLP).Different cost functions are presented in the following. 60