Industrial Robot Analysis for Cutting Cycle

POLYTECHNIC OF MILAN

Robot Mechanics · University Project

Industrial Robot Analysis forCutting Cycle

Giorgio Fontana

11th July, 2013

Contents

Introduction 1

1 Direct Kinematics 61.1 Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61.2 Velocity and Acceleration . . . . . . . . . . . . . . . . . . . . . . . 9

1.2.1 Definition of the Jacobian . . . . . . . . . . . . . . . . . . . 91.2.2 Resolution with Matrices . . . . . . . . . . . . . . . . . . . . 9

1.3 Example of direct kinematics solution . . . . . . . . . . . . . . . . . 12

2 Inverse Kinematics 142.1 Analytic Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.2 Iterative Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162.3 Example of Inverse kinematics solution . . . . . . . . . . . . . . . . 17

3 Work Space 19

4 Singularity 21

5 Inverse Dynamics 22

6 Work Cycle 246.1 Motion between (P1 : P2) and (P3 : P1) . . . . . . . . . . . . . . . . 246.2 Motion between (P2 : Pi) and (Pi : P3) . . . . . . . . . . . . . . . . 26

A Laws of Motion 32

iii

List of Figures

1 Example of serial and parallel robots . . . . . . . . . . . . . . . . . 22 Robot schematic representation . . . . . . . . . . . . . . . . . . . . 23 References adopted for the robot . . . . . . . . . . . . . . . . . . . 34 Graphic simulation of the SimMechanics model . . . . . . . . . . . 35 Work cycle to cut the extruded soap . . . . . . . . . . . . . . . . . 5

1.1 Comparison between methods . . . . . . . . . . . . . . . . . . . . . 121.2 Movements in the joints space . . . . . . . . . . . . . . . . . . . . . 131.3 Movements in the work space . . . . . . . . . . . . . . . . . . . . . 13

2.1 Comparison between methods . . . . . . . . . . . . . . . . . . . . . 172.2 Movements in the work space . . . . . . . . . . . . . . . . . . . . . 182.3 Movements in the joints space . . . . . . . . . . . . . . . . . . . . . 18

3.1 Work area in the x-y plane . . . . . . . . . . . . . . . . . . . . . . . 203.2 Work space with fixed ϑ . . . . . . . . . . . . . . . . . . . . . . . . 20

4.1 Singularity points in the work space . . . . . . . . . . . . . . . . . . 21

6.1 (P1, P2) and (P3, P1) - Velocities in the joints space . . . . . . . . . 256.2 (P1, P2) and (P3, P1) - Accelerations in the joints space . . . . . . . 256.3 Work cycle - Position in the joints space . . . . . . . . . . . . . . . 276.4 Work cycle - Velocities in the joints space . . . . . . . . . . . . . . . 276.5 Work cycle - Accelerations in the joints space . . . . . . . . . . . . 286.6 Work cycle - Position in the work space . . . . . . . . . . . . . . . . 286.7 Work cycle - Robot dynamics . . . . . . . . . . . . . . . . . . . . . 296.8 Work space - Robot dynamics with prismatic joint force . . . . . . . 296.9 Work cycle - Point P1 . . . . . . . . . . . . . . . . . . . . . . . . . 306.10 Work cycle - Point P2 . . . . . . . . . . . . . . . . . . . . . . . . . 306.11 Work cycle - Point Pi . . . . . . . . . . . . . . . . . . . . . . . . . . 306.12 Work cycle - Point P3 . . . . . . . . . . . . . . . . . . . . . . . . . 316.13 Work cycle - Point P1 . . . . . . . . . . . . . . . . . . . . . . . . . 316.14 Work cycle - scheme of the Simulink/SimMechanics model . . . . . 31

A.1 Function three-steps motion law . . . . . . . . . . . . . . . . . . . . 32A.2 Function cycloidal motion law . . . . . . . . . . . . . . . . . . . . . 33

v

List of Tables

1 Characteristics of the robot links . . . . . . . . . . . . . . . . . . . 32 Points coordinates for the work cycle . . . . . . . . . . . . . . . . . 53 Limits of the drivers . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.1 Initial and final joints space coordinates . . . . . . . . . . . . . . . 121.2 Initial and final work space coordinates . . . . . . . . . . . . . . . . 12

2.1 Initial and final work space coordinates . . . . . . . . . . . . . . . . 172.2 Initial and final joints space coordinates . . . . . . . . . . . . . . . 17

3.1 Joints limit switches . . . . . . . . . . . . . . . . . . . . . . . . . . 19

vii

Introduction

Serial and Parallel Robots1

In the automation technology field some basic characteristics separate robots fromgeneric automatic machines. Among all versatility, autonomy or ability to takedecisions, and reprogramming should be always part of a robot machine. Generallyin the industry sector the reference are serial robots. They are mechanical systemsdistinguishably because similar to the human arm. The serial robots are madeof a certain number of parts and connected with joints, in order to realize anopen kinematics chain. The first part is fixed on the ground, while the last as ahuman hand is able to manipulate objects and is called tool centre point (TCP)or end-effector. The complete control system is a computer for elaboration andthe electronics to communicate with motors and transducers. Different is the caseof parallel robots, which are a less spread category but recently in strong develop.They are considered complementary of the serial robots. In this case the kinematicchain is close. They should be preferred for load operations because of the rigidbody. On the other hand, their usually have lower mobility. This means that thespace that can be reach from the end-effector is smaller, compromising the numberof task they can perform. If the motors are left on the ground the structure resultslighter, with directly increase speed and acceleration. Even from a mathematicalpoint of view serial and parallel robots show complementary characteristics.

Purpose of the Project

In this semester project a serial robot allocated in a production line has been exten-sively analyzed. The study covers the manipulator direct and inverse kinematics,the inverse dynamics and all the main features. The equations obtained are imple-mented in Matlab and a model of the robot realized in Simulink/SimMechanics. Inthis way simulations can be performed and the behaviour predicted. As shown inFigure 2, the manipulator has the structure of an anthropomorphous robot, wherethe entire body is considered as a chain of links connected with ideal constraints.The sketch in Figure 3a refers to the robot 3 degree of freedom. The rotationaland the prismatic joints are responsible for the in-plane movements ϑ and ρ, whilethe rotational joint titled of 30◦ creates the out-of-plane rotation ϕ. Along thekinematic chain, for all the joints reference systems are defined. In particular, thelast rotational joint requires a two-step definition. Moving from coordinates system3 to system 4, a rotation of 30◦ around z3-axis is actually made. In this way the

1This section has been extracted from Robotica Industriale. Giovanni Legnani. Casa EditriceAmbrosiana, (2003)

1

2 Introduction

(a) Serial robot SCARA (b) Parallel Robot DELTA

Figure 1: Example of serial and parallel robots

x4-axis and the joint rotational axis are coincident, reducing the complexity of theequations. The tern in 0 will be also called as absolute reference system. Table 1provides all the information about link properties. In particular, in Figure 3b thelink l0 is exactly 45◦ when l2 is at the initial position of 0◦. The robot is supposedmade in aluminium 6010. As first step of the study, effects of the electrical motorsare neglected. In addition, to simplify the problem the link l0 is also neglected,and the relative coordinate ρ translated in a rotational dof α of the first joint.

Figure 2: Robot schematic representation

Introduction 3

ρ

ϑ

φ

x1

y1

z1

y2

x2

z2

y3

z3 = z4

β x3

x4

y4

x5

y5

z5

x0

y0

z0

(a) Joints system of coordinates

l1

l2 l3

l4

l0

0

1 2

3-4

5

(b) Definition of links length

Figure 3: References adopted for the robot

Figure 4: Graphic simulation of the SimMechanics model

Link Length Ext. Diameter Int. Diameter Weight− [m] [m] [m] [kg]

1 1,00 - - -2 1,00 0,250 0,230 20,363 0,25 0,125 0,105 4,884 0,25 0,125 0,105 4,880 0,71 - - -

Table 1: Characteristics of the robot links

4 Introduction

Here next are listed matrices of the inertial moments [kgm2] respect to the linkscentre of mass gi. To obtain the matrices the following equations has been applied

Jx2(g2) =1

2m2(r

22e + r22i) (1)

Jy2(g2) =1

12m2[3(r22e + r22i) + h22] +m2d

22 (2)

where di is the distance between the centre of gravity of the i-th link and the i-threference system, providing the momentum of transport.

J2(g2) =

0.293660 0 00 1.843290 00 0 1.843290

(3)

J3(g3) =

0.016248 0 00 0.109735 00 0 0.109735

(4)

J4(g4) =

0.016248 0 00 0.109735 00 0 0.109735

(5)

The matrices of the inertial moments [kgm2] respect to the reference systems arebasically the same except that there is not momentum of transport.

J2(1) =

0.293660 0 00 6.932670 00 0 6.932670

(6)

J3(2) =

0.016248 0 00 0.414567 00 0 0.414567

(7)

J4(4) =

0.016248 0 00 0.414567 00 0 0.414567

(8)

Case of Study

After be able to correctly describe the manipulator, the next step is to assign theinstructions for the task the robot has been chosen. The robot is integrated in theproduction line of soap. In particular the task assigned is a cut operation for theextruded product. The robot will move with an orthogonal cut, considering thedirection of progress of the line. In order to follow this request, the relative speedbetween the cutting utensil and the material must be zero. The figure shows thegeometry trajectory required for the TCP where the motion laws are the following

• Motion law with minimum time from waiting position P1 to position P2

• Vertical cut operation reaching point P3 with only straight lines

Introduction 5

• Return to P1 with minimum time

It is possible to identify an intermediate point Pi at z = 0 where the TCP changesthe direction of motion, and the couple (P2, P3) is specular respect to this point.The speed of progress for the extruded soap ve is 0, 1m

s. The work cycle will be

performed respecting limits of engine speed, acceleration and torque. As in Table3, maximum velocities and accelerations raise from the first to the last electricdrive because masses and inertias are lower toward the kinematic chain end.

P1

P2

Pi

P3

ve y

z x

Figure 5: Work cycle to cut the extruded soap

Position x y z− [m] [m] [m]

P1 0,5 0,50 0,0P2 1,0 0,6 0,1Pi 1,0 0,0 0,0P3 1,0 0,6 -0,1

Table 2: Points coordinates for the work cycle

dof vmax +amax −amax− [m

s] [m

s2] [m

s2]

ρ 0,5 2,0 2,0ϑ 4,0 20 20ϕ 5,0 30 30

Table 3: Limits of the drivers

Chapter 1

Direct Kinematics

The direct kinematics problem means to completely describe the TCP kinemat-ics (position, velocity and acceleration) through only information from the joints.In other words, to translate the assignments for the drivers into the consequentmovement for the hand. Because this is a serial robot, it is possible to obtain theanalytic expression. The linear movement ρ of the prismatic joint is converted inthe angular dof α of the free rotational joint.

α = a sin(

2( l0 − ρ

l1

)2− 1)

(1.1)

Therefore, the coordinates in the joints space are summarized in the vector

Q = [α, ϑ, ϕ] (1.2)

while the vector for the coordinates in the work space is

S = [x, y, z] (1.3)

In the convention adopted α and ϑ are anticlockwise rotations around the z1 and z2axes, while ϕ is the anticlockwise rotation around the x4 axis. Next in this chapterthe position is found using position matrices, because it reduces the complexity todescribe the movement out-of-plane. Velocity and acceleration instead are obtainedwith both Jacobian method and matrices resolution.

1.1 PositionThe position of points in the space can be completely described using 3 coordinates.Nevertheless, to easily solve the geometric problem the homogeneous coordinatesare introduced.

P = [x, y, z, 1]T (1.4)

With this formulation it is possible to convert point coordinates from a tern 0 toa tern 1 thanks to the M01 4x4 matrix built in blocks

P0(0) = M01P1(1) =

R01 T(0)

0 0 0 1

P1(1) (1.5)

6

1.1. Position 7

where R01 is the rotation matrix and T(0) is the translation matrix. They repre-sent the angular and relative positions of the two terns evaluated. Extending thisprocess among all the coordinate systems from the base to the hand, the transfor-mation matrices are defined.

M01 =

1 0 0 00 1 0 l10 0 1 00 0 0 1

(1.6)

M12 =

cosα − sinα 0 l2 cosαsinα cosα 0 l2 sinα

0 0 1 00 0 0 1

(1.7)

M23 =

cosϑ − sinϑ 0 l3 cosϑsinϑ cosϑ 0 l3 sinϑ

0 0 1 00 0 0 1

(1.8)

M34 =

cos β − sin β 0 0sin β cos β 0 0

0 0 1 00 0 0 1

(1.9)

M45 =

1 0 0 l4 cos β0 cosϕ − sinϕ −l4 sin β cosϕ0 sinϕ cosϕ −l4 sin β sinϕ0 0 0 1

(1.10)

It is possible to have multiple change in the following way

Mik = MijMjk (1.11)

Therefore, through the serial kinematic chain it is possible to obtain the transfor-mation matrix from the end to the base.

M05 =5∏i=1

Mi−1,i (1.12)

The T translation vector in M05 is the projection of the reference system 5 (centreof the TCP) along the directions of the absolute reference system 0. In the nextstep this matrix will be analytically derived.

M02 = M01M12 =

cosα − sinα 0 l2 cosαsinα cosα 0 l1 + l2 sinα

0 0 1 00 0 0 1

(1.13)

8 Chapter 1. Direct Kinematics

M03 = M02M23 =

CαCϑ − SαSϑ −CαSϑ − SαCϑ 0 l2Cα + l3(CαCϑ − SαSϑ)SαCϑ + CαSθ −SαSϑ + CαCϑ 0 l1 + l2Sα + l3(SαCϑ + CαSϑ)

0 0 1 00 0 0 1

(1.14)

Taking use of the Werner formulas

M03 =

Cα+ϑ −Sα+ϑ 0 l2Cα + l3Cα+ϑSα+ϑ Cα+ϑ 0 l1 + l2Sα + l3Sα+ϑ

0 0 1 00 0 0 1

(1.15)

M04 = M03M34 =

Cα+ϑCβ − Sα+ϑSβ −Cα+ϑSβ − Sα+ϑCβ 0 l2Cα + l3Cα+ϑSα+ϑCβ + Cα+ϑSβ −Sα+ϑSβ + Cα+ϑCβ 0 l1 + l2Sα + l3Sα+ϑ

0 0 1 00 0 0 1

(1.16)

Again using the Werner formulas

M04 =

Cα+ϑ+β −Sα+ϑ+β 0 l2Cα + l3Cα+ϑSα+ϑ+β Cα+ϑ+β 0 l1 + l2Sα + l3Sα+ϑ

0 0 1 00 0 0 1

(1.17)

M05 = M04M45 =

Cα+ϑ+β −Sα+ϑ+βCϕ Sα+ϑ+βSϕ l2Cα + l3Cα+ϑ + l4CβCα+ϑ+β + l4SβCϕSα+ϑ+βSα+ϑ+β Cα+ϑ+βCϕ −Cα+ϑ+βSϕ l1 + l2Sα + l3Sα+ϑ + l4CβSα+ϑ+β − l4SβCϕCα+ϑ+β

0 Sϕ Cϕ −l4SβSϕ0 0 0 1

(1.18)

Rearranging the equations, the matrix became the following

M05 =

Cα+ϑ+β −Sα+ϑ+βCϕ Sα+ϑ+βSϕ l2Cα + l3Cα+ϑ + l4

2

[Cα+ϑ+2β

(1− Cϕ

)+ Cα+ϑ

(1 + Cϕ

)]Sα+ϑ+β Cα+ϑ+βCϕ −Cα+ϑ+βSϕ l1 + l2Sα + l3Sα+ϑ + l4

2

[Sα+ϑ+2β

(1− Cϕ

)+ Sα+ϑ − S−α−ϑCϕ

]0 Sϕ Cϕ −l4SβSϕ0 0 0 1

(1.19)

The last column is actually the system of equations which describe the work spaceas function of joints space.

S = F (Q) (1.20)x = l2Cα + l3Cα+ϑ + l4

2

[Cα+ϑ+2β

(1− Cϕ

)+ Cα+ϑ

(1 + Cϕ

)]y = l1 + l2Sα + l3Sα+ϑ + l4

2

[Sα+ϑ+2β

(1− Cϕ

)+ Sα+ϑ − S−α−ϑCϕ

]z = −l4SβSϕ

(1.21)

1.2. Velocity and Acceleration 9

1.2 Velocity and Acceleration

The first method here presented requires to write a Jacobian and its derivative.Especially for accelerations it can be a long process and far from be without anymistake. Instead a matrix-based script is well suited to be implemented in Matlab,where only the initial matrices are defined and left to the software the task tocalculate all the rest.

1.2.1 Definition of the Jacobian

One of the possible way to proceed is to calculate end-effector speed and accelera-tion deriving the expression of S.

S = J(Q)Q (1.22)

S = J(Q)Q+ J(Q)Q (1.23)

Therefore, it is necessary to define Jacobian matrix and its derivative.

J(Q) =

dxdα

dxdϑ

dxdϕ

dydα

dydϑ

dydϕ

dzdα

dzdϑ

dzdϕ

(1.24)

Jij =dJijdα

α +dJijdϑ

ϑ+dJijdϕ

ϕ (1.25)

The analytic equations are not reported because of the huge space required. Any-way, they have been written as Matlab functions.

1.2.2 Resolution with Matrices

Always working with homogeneous coordinates W is a 4x4 matrix built in blocks

P(i) = W(i)P(i) =

ω V0

0 0 0 0

P(i) (1.26)

where ω is the angular speeds matrix and V0 the speed vector of a point movingin-built with P , and passing through the reference system origin. In vector P(i) thefirst three positions are the velocity components respect the i-th reference system.The last element is zero. Because in this case there is only one dof between twojoints, the W matrices are simple.

W01(0) =

0 0 0 00 0 0 00 0 0 00 0 0 0

W12(1) =

0 −α 0 0α 0 0 00 0 0 00 0 0 0

(1.27)


W23(2) =

0 −ϑ 0 0

ϑ 0 0 00 0 0 00 0 0 0

W34(3) =

0 0 0 00 0 0 00 0 0 00 0 0 0

(1.28)

W45(4) =

0 0 0 00 0 −ϕ 00 ϕ 0 00 0 0 0

(1.29)

As for the position, even in this case the matrices will be reported to the absolutereference system.

W(i) = MijW(j)Mji (1.30)

where the matrix M has the following structure

Mji = M−1ij =

−RTij −RT

ijTij

0 0 0 1

(1.31)

To obtain the matrix of speeds for the TCP in the reference system 0, the last stepis to sum all the matrices calculated in the absolute reference. The same procedureis suggested from the theorem of velocities composition or the Theorem of Rivals

Wik(r) = Wij(r) +Wjk(r) (1.32)

For the case under study the formulation is the next

W05(0) =∑

5i=1Wi−1,i(0) (1.33)

Therefore, if P(0) is the position vector of the end effector respect to the tern 0,the velocity vector of the same can be calculated in this way

P(0) = W05(0)P(0) (1.34)

With the same approach is possible to proceed for the accelerations. Now H is a4x4 matrix built in blocks

P(i) = H(i)P(i) =

ω + ω2 A0

0 0 0 0

P(i) (1.35)

where ω and ω2 are the tangential and the normal accelerations.

H01(0) =

0 0 0 00 0 0 00 0 0 00 0 0 0

H12(1) =

−α2 −α 0 0α −α2 0 00 0 0 00 0 0 0

(1.36)

1.2. Velocity and Acceleration 11

H23(2) =

−ϑ2 −ϑ 0 0

ϑ −ϑ2 0 00 0 0 00 0 0 0

H34(3) =

0 0 0 00 0 0 00 0 0 00 0 0 0

(1.37)

H45(4) =

0 0 0 00 −ϕ2 −ϕ 00 ϕ −ϕ2 00 0 0 0

(1.38)

It could be possible to have the same result using only W matrices already defined

H(k) = W(k) +W 2k (1.39)

Obviously even the H matrices will be redefined in the absolute coordinate system,exactly as already seen for the speed.

H(i) = MijH(j)Mji (1.40)

To obtain the matrix of acceleration for the TCP in the reference system 0, thelast step is to sum the matrices calculated in the absolute reference with the sameprocedure suggested from the theorem of accelerations composition or the Theoremof Coriolis

Hik(r) = Hij(r) +Hjk(r) + 2Wij(r)Wjk(r) (1.41)

For the case under study the formulation is the next

H05(0) =∑

5i=1Hi−1,i(0) +

∑5j=2

∑j−1k=12Wk−1,k(0)Wj−1,j(0) (1.42)

Therefore, if P(0) is the position vector of the end effector respect to the tern 0,the acceleration vector of the same can be calculated in this way

P(0) = H05(0)P(0) (1.43)


Position ρ α ϑ ϕ− [m] [◦] [◦] [◦]

start 0,00 0 0 0end 0,15 50 40 40

Table 1.1: Initial and final joints space coordinates


Pstart 1,5 1,0 0,0Pend 0,605 2,258 -0,080

Table 1.2: Initial and final work space coordinates

1.3 Example of direct kinematics solution

In Table 1.1 and in Figure 1.2 the set of graphs shows initial and final jointsposition. The movement is described with a 3-steps motion law. They are inputsfor the direct kinematics problem. The solution is in Table 1.2 and in Figure 1.3. Inparticular, every acceleration has the tipical step shape. Because of the differrencein scale it is not possible to recognize the trapezoid for the speeds. Comparingresults from the two methods except for numerical approximations the differenceis always zero, as shown in Figure 1.1. Therefore for the direct kinematics boththe tecniques described represt a valid alternative.

CONFRONTO FRA PROCEDIMENTI - CINEMATICA DIRETTA

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

1

2

3

4

5

6x 10−16

t [s]

diff

[m]

xyz

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.2

0.4

0.6

0.8

1

1.2x 10−15

t [s]

diff

[m/s

]

xpypzp

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

1

2

3

4x 10−15

t [s]

diff

[m/s

2]

xppyppzpp

CONFRONTO FRA PROCEDIMENTI - CINEMATICA INVERSA

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

1000

2000

3000

4000

t [s]

diff

[rad]

alphathetaphi

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.5

1

1.5

2

t [s]

diff

[rad/

s]

alphapthetapphip

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.2

0.4

0.6

0.8

1

1.2

1.4

t [s]

diff

[rad/

s2]

alphappthetappphipp

''

Figure 1.1: Comparison between methods

1.3. Example of direct kinematics solution 13

ESEMPIO SOLUZIONE CINEMATICA DIRETTA

MOVIMENTO NELLO SPAZIO DEI GIUNTI

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.05

0.1

0.15

0.2

t [s]

rho

[m]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

10

20

30

40

50

t [s]

alph

a [°]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

10

20

30

40

t [s]

thet

a [°]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

10

20

30

40

t [s]

phi [

°]

MOVIMENTO NELLO SPAZIO DI LAVORO

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−5

0

5

10

t [s]

x[m]xp[m/s]xpp[m/s2]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−10

−5

0

5

10

15

t [s]

y[m]yp[m/s]ypp[m/s2]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

t [s]

z[m]zp[m/s]zpp[m/s2]

Figure 1.2: Movements in the joints space

ESEMPIO SOLUZIONE CINEMATICA DIRETTA


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.05

0.1

0.15

0.2

t [s]

rho

[m]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

10

20

30

40

50

t [s]

alph

a [°]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

10

20

30

40

t [s]

thet

a [°]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

10

20

30

40

t [s]

phi [

°]


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−5

0

5

10

t [s]

x[m]xp[m/s]xpp[m/s2]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−10

−5

0

5

10

15

t [s]

y[m]yp[m/s]ypp[m/s2]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−0.8

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

t [s]

z[m]zp[m/s]zpp[m/s2]

Figure 1.3: Movements in the work space

Chapter 2

Inverse Kinematics

The target of the inverse kinematics analysis is to calculate the motion law for theelectric drivers in order to perform the TCP expected movement. The nonlinearshape of the F function in the relation S = F (Q) brings some consequences. Ana-lytically derive the expression of Q from S could be difficult if the kinematic chainbecame complex, and the solution of the inverse problem more than one. Theprocedure to obtain the solution is not standard and it requires the analysis of thegeometry. Otherwise approximation-based numerical methods can be explored.

2.1 Analytic Solution

For the case under study the analytic expression of the inverse kinematic has beenfound. Exactly as for the direct kinematics, it is preferred to work with the dofα and only at the end convert it to ρ. This is possible because the equivalentexpression is simple and easily implementable. The first step is to find the dof ϕ,because only function of z.

z = −l4 sin β sinϕ (2.1)

From this relation one of the variables is already defined

ϕ1,2 =(− arcsin

( z

l4 sin β

), 180 +

( z

l4 sin β

))(2.2)

Therefore, only the last joint is responsible for rotations out-of-plane and so forvariations in z. The dof ϕ can move the end effector in two specular positions±z. To find the dof ϑ the idea is to combine coordinates in the work and in thejoints space. Following this approach, a distance d which ideally link the points1 and 5 can be expressed at the same time through variables in the only workspace and with variables in the only joints space. But because d would be writtenwithout α, the triangle generated is rotated of α degrees. In this way only ϕ isunknown.

d2 = x2+(y−l1)2 = [l2+l3Cϑ+l4CβCϑ+β+l4SβCϕSϑ+β]2+[l3Sϑ+l4CβSϑ+β−l4SβCϕCϑ+β]2

(2.3)

14

2.1. Analytic Solution 15

Passing across several simplifications the equation has the following shape

a sinϑ+ b cosϑ = c (2.4)

wherea = −l2l4(1− Cϕ)S2β

b = 2l2l3 + l2l4(1− Cϕ)C2β + l2l4(1 + Cϕ)

c = x2 + y2 + l21 − 2yl1 − l22 − l23 − (l4Cβ)2 − (l4SβCϕ)2 − 2l3l4(C2β + S2

βCϕ)

(2.5)The next step is to operate a change of variables, where t = tan ϑ

2

2at+ b(1− t2) = c(1 + t2) (2.6){sinϑ = 2t

1+t2

cosϑ = 1−t21+t2

(2.7)

From the resolution of the second degree equation, two values for t can be found

t =a±√a2 + b2 − c2b+ c

(2.8)

and from the direct relation the second dof in the joints space is

ϑ = 2 arctan (t) + 2kπ (2.9)

Now the only variable to define is α and it can be easily derived from one of theequations for the direct kinematics. For example, using x

x = l2Cα + l3Cα+ϑ + l4CβCα+ϑ+β + l4SβCϕSα+ϑ+β (2.10)

It it always possible to write this formulation

d sinα + e cosα = f (2.11)

where d = −l3Sϑ − l4CβSϑ+β + l4SβCϕCϑ+β

e = l2 + l3Cϑ + l4CβCϑ+β + l4SβCϕSϑ+β

f = x

(2.12)

The same change of variables is now t = tan α2{

sinα = 2t1+t2

cosα = 1−t21+t2

(2.13)

From the resolution of the second degree equation, two values for t can be found

t =d±

√d2 + e2 − f 2

e+ f(2.14)

16 Chapter 2. Inverse Kinematics

and the last dof in the joints space is

α = 2 arctan (t) + 2kπ (2.15)

The inverse kinematics problem for the position is now completely defined and itleads to more possible combinations. Each one the results ϑ1,2 carry the 2 solutionsα1,2 and so 4 couples of values (ϑ,α). Therefore, it is possible to reach the sameposition for the TCP through 2 different but symmetric configurations. The others2 solutions move the end effector in symmetric position, where the symmetry axisis directed as x and it starts at y = 1m. This because of the positive angles fromthe x-axis and the quote of 1m for the origin of the triangle sketched. Finally,the 2 solutions ϕ1,2 raise to 8 the possible combinations. In the choice of the rightcouple it is necessary to check the elbow links creates and that the final positionis the one expected.

Thanks to the Jacobian already defined for the direct kinematics, velocity andacceleration can be easily derived as

Q = J−1S (2.16)

Q = J−1(S − JQ) (2.17)

2.2 Iterative MethodAmong the techniques available in literature, the Newton-Raphson algorithm hasbeen implemented in this study. It is based on a system linearization around onepoint Q which should be closed to the solution. The final approximation desired isset. Because the direct problem is already solved, the relation S = F (Q) is known.This is approximated with the first terms of the Taylor Series.

F (Q) ≈ F (Q0) + J(Q0)(Q−Q0) (2.18)

Defining Sf as the TCP position, a sequence of values should converges at the rootSf = F (Q) searched.

Qn+1 = Qn + J−1n (Sf − F (Qn)) (2.19)

It is an iterative process under the condition

‖Sf − F (Qn)‖ ≤ ε (2.20)

2.3. Example of Inverse kinematics solution 17


Pstart 1,4 1,2 0,0Pend 1,0 1,6 0,1

Table 2.1: Initial and final work space coordinates

Position ρ α ϑ ϕ− [m] [◦] [◦] [◦]

start 0,00 -5,39 41,41 0end 0,15 7,07 76,81 -53,13

Table 2.2: Initial and final joints space coordinates

2.3 Example of Inverse kinematics solutionIn Table 2.1 and in Figure 2.2 the set of graphs shows initial and final TCP position.Even in this case the movement is described with a 3-steps motion law. Theyare inputs for the inverse kinematics problem. The solution in Table 2.2 andin Figure 2.3 is obtained with both the analytic set of equations and using anumerical method. As shown in Figure 2.1, approaching a singularity the iterativealgorithm became highly unstable and the difference from the analytic approachunrestrained. In addition, the solution is influenced from initial conditions. Itguarantee satisfying results only for initial angles close to the result.

CONFRONTO FRA PROCEDIMENTI - CINEMATICA DIRETTA

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

1

2

3

4

5

6x 10−16

t [s]

diff

[m]

xyz

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.2

0.4

0.6

0.8

1

1.2x 10−15

t [s]

diff

[m/s

]

xpypzp

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

1

2

3

4x 10−15

t [s]

diff

[m/s

2]

xppyppzpp

CONFRONTO FRA PROCEDIMENTI - CINEMATICA INVERSA

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

1000

2000

3000

4000

t [s]

diff

[rad]

alphathetaphi

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.5

1

1.5

2

t [s]

diff

[rad/

s]

alphapthetapphip

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.2

0.4

0.6

0.8

1

1.2

1.4

t [s]

diff

[rad/

s2]

alphappthetappphipp

'' Figure 2.1: Comparison between methods

18 Chapter 2. Inverse Kinematics

ESEMPIO SOLUZIONE CINEMATICA INVERSA


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 11

1.1

1.2

1.3

1.4

t [s]

x[m]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 11.1

1.2

1.3

1.4

1.5

1.6

t [s]

y[m]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.02

0.04

0.06

0.08

0.1

t [s]

z[m]

'''


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−0.6

−0.4

−0.2

0

0.2

t [s]

rho[m]rhop[m/s]rhopp[m/s2]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−10

−5

0

5

10

t [s]

alpha[°]alphap[rad/s]alphapp[rad/s2]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20

0

20

40

60

80

t [s]

theta[°]thetap[rad/s]thetapp[rad/s2]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−60

−40

−20

0

20

t [s]

phi[°]phip[rad/s]phipp[rad/s2]

'

Figure 2.2: Movements in the work space

ESEMPIO SOLUZIONE CINEMATICA INVERSA


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 11

1.1

1.2

1.3

1.4

t [s]

x[m]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 11.1

1.2

1.3

1.4

1.5

1.6

t [s]

y[m]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.02

0.04

0.06

0.08

0.1

t [s]

z[m]

'''


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−0.6

−0.4

−0.2

0

0.2

t [s]

rho[m]rhop[m/s]rhopp[m/s2]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−10

−5

0

5

10

t [s]

alpha[°]alphap[rad/s]alphapp[rad/s2]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20

0

20

40

60

80

t [s]

theta[°]thetap[rad/s]thetapp[rad/s2]

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−60

−40

−20

0

20

t [s]

phi[°]phip[rad/s]phipp[rad/s2]

' Figure 2.3: Movements in the joints space

Chapter 3

Work Space

Whichever is the trajectory assigned, the robot must not be able to hit itself.Because of that limit switches for all the joints are set. They are reported in Table3.1. The limit switches for the prismatic joint is the maximum possible extensionof link l0, starting from the initial configuration of 45◦

ρ = (−l√

2

2)÷ (2l − l

√2) (3.1)

With all the information available the complete work space can be sketched andsome conclusions summarized.

• The end efector cannot reach points outside the circumference of radius l2 +l3 + l4 and center (x0, y0 + l1)

• Because they are singularity, points on circumference border are reached withonly one link configuration

• The TCP is able of positioning in points inside the circumference with twolink configurations

• Points where joints are close to the limit switches are reached with only onelink configuration

dof Min Max− [◦] [◦]

α -45 +90ϑ -135 +135ϕ -180 +180

Table 3.1: Joints limit switches

19

20 Chapter 3. Work Space

The picture in Figure 3.1 shows the work space in x-y plane: the thin line refersto link l2 while the thick line to the robot extremity.

4.''AREA'DI'LAVORO'Di seguito sono riportati i valori dei fine corsa decisi per ogni link. Con questa scelta si è certi che, qualsiasi sia il comando di posizione imposto al robot, questo non urti se stesso. link 2 : α = (−45°)÷ (+90°)link 3 : ϑ = (−135°)÷ (+135°)link 4 : ϕ = (−180°)÷ (+180°)

⎧⎨⎪

⎩⎪

Si ricavano i fine corsa anche per il giunto prismatico:

ρ = (−l 22)÷ (2l − l 2)

!!

Questa quantità è da intendersi come differenza fra l’estensione finale del braccio l0 e quando invece è nella condizione a 45° (dove quindi ρ=0). Tenendo conto dei limiti imposti ai giunti, è possibile disegnare l’area di lavoro del robot e trarre alcune conclusioni:

- I punti al di fuori dell’arco di cfr di raggio l2+l3+l4 e centro (x0, y0+l1) non sono raggiungibili

- I punti che giacciono su tale cfr sono raggiungibili in una sola configurazione (singolarità)

- I punti interni alla cfr sono raggiungibili per due possibili configurazioni dei links - In prossimità dei fine corsa le soluzioni possibili diventano una sola

−0.5 0 0.5 1 1.5−0.5

0

0.5

1

1.5

2

2.5

X: 0Y: 0

x [m]

y [m

]

Il grafico mostra gli estremi dell’area di lavoro nel piano x-y: la linea più sottile è l’estremità del link 2 mentre quella più spessa la posizione della pinza.

Figure 3.1: Work area in the x-y plane

The next 3-dimensions graphic is obtained fixing ϑ and with a sweep of α and ϕ,it shows the ellipses in plane y-z generated from the oblique joint.

Questo grafico, ottenuto tenendo fisso a zero l’angolo θ e variando α e φ, mette in luce le ellissi generate nel piano y-z dal giunto ad asse sghembo.

−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

−0.5

0

0.5

1

1.5

2

2.5

x [m]z [m]

y [m

]

!

Figure 3.2: Work space with fixed ϑ

Chapter 4

Singularity

The study of singularity configurations can be conducted through different ways.

• Analysis of the inverse kinematics solutions. The singularity is the conditionof coincidence of two or more solutions of the inverse kinematics or if oneof the functions is not defined. There is a singularity when changing α, thelinks l2, l3 are aligned and so ϑ = 0◦ and ϕ = 0◦, 180◦.

• Analysis of the J matrix. In case of singularity the Jacobian determinant isannulled. With a sweep dense enough all the singularity configurations arecovered.

As appears from the 3-dimensions graphics, the singularities are part of the workspace limits.

5.''CONFIGURAZIONI'SINGOLARI'L'individuazione delle configurazioni di singolarità può essere condotta seguendo differenti strade:

• Analisi della soluzione della cinematica inversa# Le configurazioni singolari possono essere viste come i casi in cui due o più soluzioni della cinematica inversa coincidono, o quando una funzione in esse contenuta non è definita. Condizione di singolarità per θ=0 e φ=(0, 180°) al variare di α (bracci allineati).

• Analisi della matrice J In corrispondenza di queste il determinante dello jacobiano si annulla: coprendo tutti i movimenti concessi al robot con un delta d’angolo sufficientemente piccolo è possibile includere tutte le configurazioni singolari. Di seguito è riportato il grafico 3D risultante, che coincide con quanto ipotizzato sopra. Le configurazioni singolari coincidono con parte degli estremi dell’area di lavoro.

−0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

−0.2−0.15−0.1−0.0500.050.10.15

−0.5

0

0.5

1

1.5

2

2.5

x [m]

z [m]

y [m

]

'

Figure 4.1: Singularity points in the work space

21

Chapter 5

Inverse Dynamics

Through the inverse dynamics the drivers action to realize the expected end-effectormovement is derived. The forces acting on the links and the actions in between(joints reactions) are analyzed. To solve the inverse dynamics some matrices areintroduced.

• Matrix of the actions Φ: it represents a system of forces and couples respectthe reference system i. Because it is an emisimetric matrix, ΦT

(i) = −Φ(i) andhas the following shape

Φ(i) =

0 −Cz Cy fxCz 0 −Cx fy−Cy Cx 0 fz−fx −fy −fz 0

(5.1)

• Inertial pseudo-tensor J: it describes the mass distribution of a rigid bodyaround the reference system i with the following shape

J(i) =

Ixx Iyx Izx mxgIxy Iyy Izy mygIxz Iyz Izz mzgmxg myg mzg m

(5.2)

Combining these matricies with M , W , H defined with the analysis of the directkinematics allows for a simple and systematic resolution of rigid bodies dynamics.Precisely, to calculate inertial forces and couples respect the reference system i

Φ(i) = H0k(i)Jk(i) − Jk(i)HT0k(i) (5.3)

whereH0k(i) is the matrix of absolute acceleration for the body k, calculated respectan inertial system 0 (in this study also reported as the absolute reference system)and projected on the tern i. In order to consider also the weight force, one matrixin the absolute reference system will be defined

Hg =

0 0 0 00 0 0 −9, 810 0 0 00 0 0 0

(5.4)

22

23

To derive the inverse dynamics the fisrt step is to define the inertial pseudo-tensorof every link k, starting from the TCP toward the base. All the H matrices arealready known in the absolute reference. All the elements should be reported tothe same reference system, and so

Jk(0) = M0iJk(i)MT0i (5.5)

Summarizing, the matrix equation which covers all the possible forces acting on apart

Φ(i) = −(H0k(i)Jk(i) − Jk(i)HT0k(i)) + (Hg(i)Jk(i) − Jk(i)H

Tg (i)

) + Φ∧(i) (5.6)

For every joint j the constraint reaction is derived with the use of dynamics equilib-rium and considering the transmitted forces from joint i+ 1, inertial terms, weightand other external components

Φ∗i = Φ∗i+1 + Φi (5.7)

Finally, to obtain the motor actions the Principle of Virtual Works suggests thatthe virtual work of the i− th motor equals the virtual work of actions at the i− thjoint, which it depends from the sum of the previous links.

φiδqi + Φ∗i · Liδqi = 0 (5.8)

From this consideration the force or couple the i− th joint exercises is

φi = −Φ∗i · Li (5.9)

Chapter 6

Work Cycle

The final goal of this study is to assign the cycle instructions described in theintroduction to the robot. Among the specifics, it is required that in the extrudedirection the relative speed between cutting edge and piece is zero. From anexternal observer the TCP describes a V trajectory, while from an in-built pointthe movement is only vertically.

6.1 Motion between (P1 : P2) and (P3 : P1)

The movements (P1 : P2) and (P3 : P1) are without any trajectory constraintbut only for initial and final points. They will be performed with the minimumtime, compatibly with the drivers limit. The first step is to calculate the jointcoordinates Q1, Q2 and Q3 solving the inverse kinematics. With the definition ofthe variations ∆Q2,1 = Q2−Q1 and ∆Q1,3 = Q1−Q3, the minimum time for everyjoint is achieved with the maximum acceleration. Depending from the space ∆qi tocover, the drive can effectively reach the maximum speed before the decelerationphase or not. The two possible case are described as

if ‖∆qi‖ ≤vmaxi

2

(vmaxidmaxi

+vmaxiamaxi

)Tmini =

√2‖∆qi‖

amaxi + dmaxiamaxidmaxi

(6.1)

if ‖∆qi‖ >vmaxi

2

(vmaxidmaxi

+vmaxiamaxi

)Tmini =

‖∆qi‖vm

+1

2

vm(A+D)

AD(6.2)

Generally the time for every joint is different, and the maximum among them isthe minimum time of the problem. Therefore, for the other joints there will be adelay expressible as

λi =TminTi

(6.3)

where Tmin = 0.47s. Except for the drive with highest time, the others will enlargethe time scale in order to require Tmin to reach the end. In this way overload inthe actuators is avoided. The shape of the motion law does not change, only themaximum acceleration and speed. In this case the inertial forces decrease withbenefits for the drivers.

ai =amaxiλ2i

vi =vmaxiλ2i

di =dmaxiλ2i

(6.4)

24

6.1. Motion between (P1 : P2) and (P3 : P1) 25

The graphics below show velocity and acceleration for the 3 dof, where the seconddrive decide the minimum time.I grafici riportano l’andamento di velocità e accelerazione per i tre gdl. Si osserva come sia il secondo motore a determinare il tempo minimo. Quanto visto per questo primo tratto è identico in svolgimento e risultati anche per l’ultimo, fra P3 e P1.

0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.450

0.05

0.1

0.15

0.2

0.25

t [s]

rhop

[m/s

]

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.50

1

2

3

4

5

t [s]

thet

ap [r

ad/s

]

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.50

1

2

3

4

t [s]

phip

[rad

/s]

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

t [s]

rhop

p [m

/s2]

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5−20

−10

0

10

20

t [s]

thet

app

[rad/

s2]

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5−20

−10

0

10

20

t [s]

phip

p [ra

d/s2

]

Figure 6.1: (P1, P2) and (P3, P1) - Velocities in the joints space

I grafici riportano l’andamento di velocità e accelerazione per i tre gdl. Si osserva come sia il secondo motore a determinare il tempo minimo. Quanto visto per questo primo tratto è identico in svolgimento e risultati anche per l’ultimo, fra P3 e P1.

0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.450

0.05

0.1

0.15

0.2

0.25

t [s]

rhop

[m/s

]

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.50

1

2

3

4

5

t [s]

thet

ap [r

ad/s

]

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.50

1

2

3

4

t [s]

phip

[rad

/s]

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

t [s]

rhop

p [m

/s2]

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5−20

−10

0

10

20

t [s]

thet

app

[rad/

s2]

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5−20

−10

0

10

20

t [s]

phip

p [ra

d/s2

]

Figure 6.2: (P1, P2) and (P3, P1) - Accelerations in the joints space

26 Chapter 6. Work Cycle

6.2 Motion between (P2 : Pi) and (Pi : P3)

For these couples of points the speed in z direction will be equal to ve and thetrajectory always made of straight lines. Considering now the descent phase (theascent is symmetric), the distance can be expressed as

dx = ‖P2x − Pix‖ = 0, 0m (6.5)

dy = ‖P2y − Piy‖ = 0, 6m (6.6)

dz = ‖P2z − Piz‖ = 0, 1m (6.7)

d =√d2x + d2y + d2z = 0, 6083m (6.8)

The constrain in speed is translated in the time domain

t =dzve

= 1s (6.9)

To obtain a straight trajectory directional vectors are used

P (t) = P2 + uλ(t) (6.10)

P (t) = uλ(t) (6.11)

P (t) = uλ(t) (6.12)

where u has the same orientation of the trajectory searched

u =(Pi − P2)

‖Pi − P2‖(6.13)

and λ is a three-steps motion law. The goal here would be to move the cutting edgewith also constant vertical speed. This will be not completely true because of theinversion of motion in Pi. Anyway, short dt for both acceleration and decelerationleads to satisfying results. The alternative could be to change y coordinate for theintermediate point Pi in order to leave the motion inversion under the extrudedproduct. Because this would be possible the conveyor belt must be soft and slightlywarps under utensil pressure.

6.2. Motion between (P2 : Pi) and (Pi : P3) 27

The set of graphics shows the entire cycle of work, which needs 2, 95s to termi-nate. Limitations and constraints are respected. First the 3 dof ρ, ϑ, ϕ and theirderivatives are reported, while the next set are the x, y, z coordinates of the endeffector.I grafici riportano il ciclo di lavoro in α, θ e φ

0 0.5 1 1.5 2 2.5 3−0.08

−0.06

−0.04

−0.02

0

0.02

t [s]

rho

[m]

0 0.5 1 1.5 2 2.5 3−3

−2.5

−2

−1.5

−1

−0.5

t [s]

thet

a [ra

d]

0 0.5 1 1.5 2 2.5 3−1

−0.5

0

0.5

1

t [s]

phi [

rad]

I grafici riportano il ciclo di lavoro svolto dall’utensile in x, y e z.

0 0.5 1 1.5 2 2.5 30.5

0.6

0.7

0.8

0.9

1

1.1

1.2

t [s]

0 0.5 1 1.5 2 2.5 3−0.2

0

0.2

0.4

0.6

0.8

t [s]

0 0.5 1 1.5 2 2.5 3−0.1

−0.05

0

0.05

0.1

t [s]

x[m]

y[m]

z[m]

Figure 6.3: Work cycle - Position in the joints spaceInfine si riportano i grafici con gli andamenti di velocità e accelerazione dei giunti per l’intero ciclo di lavorazione, che ha una durata totale di 2.95s; è garantito il rispetto dei limiti imposti.

0 0.5 1 1.5 2 2.5 3−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

t [s]

rhop

[m/s

]

0 0.5 1 1.5 2 2.5 3−2

−1

0

1

2

3

4

5

t [s]

thet

ap [r

ad/s

]

0 0.5 1 1.5 2 2.5 30

1

2

3

4

t [s]

phip

[rad

/s]

0 0.5 1 1.5 2 2.5 3−1

−0.5

0

0.5

1

1.5

t [s]

rhop

p [m

/s2]

0 0.5 1 1.5 2 2.5 3−30

−20

−10

0

10

20

t [s]

thet

app

[rad/

s2]

0 0.5 1 1.5 2 2.5 3−20

−10

0

10

20

t [s]

phip

p [ra

d/s2

]

Figure 6.4: Work cycle - Velocities in the joints space


Infine si riportano i grafici con gli andamenti di velocità e accelerazione dei giunti per l’intero ciclo di lavorazione, che ha una durata totale di 2.95s; è garantito il rispetto dei limiti imposti.

0 0.5 1 1.5 2 2.5 3−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

t [s]

rhop

[m/s

]

0 0.5 1 1.5 2 2.5 3−2

−1

0

1

2

3

4

5

t [s]

thet

ap [r

ad/s

]

0 0.5 1 1.5 2 2.5 30

1

2

3

4

t [s]

phip

[rad

/s]

0 0.5 1 1.5 2 2.5 3−1

−0.5

0

0.5

1

1.5

t [s]

rhop

p [m

/s2]

0 0.5 1 1.5 2 2.5 3−30

−20

−10

0

10

20

t [s]

thet

app

[rad/

s2]

0 0.5 1 1.5 2 2.5 3−20

−10

0

10

20

t [s]

phip

p [ra

d/s2

]

Figure 6.5: Work cycle - Accelerations in the joints space

I grafici riportano il ciclo di lavoro in α, θ e φ

0 0.5 1 1.5 2 2.5 3−0.08

−0.06

−0.04

−0.02

0

0.02

t [s]

rho

[m]

0 0.5 1 1.5 2 2.5 3−3

−2.5

−2

−1.5

−1

−0.5

t [s]

thet

a [ra

d]

0 0.5 1 1.5 2 2.5 3−1

−0.5

0

0.5

1

t [s]

phi [

rad]

I grafici riportano il ciclo di lavoro svolto dall’utensile in x, y e z.

0 0.5 1 1.5 2 2.5 30.5

0.6

0.7

0.8

0.9

1

1.1

1.2

t [s]

0 0.5 1 1.5 2 2.5 3−0.2

0

0.2

0.4

0.6

0.8

t [s]

0 0.5 1 1.5 2 2.5 3−0.1

−0.05

0

0.05

0.1

t [s]

x[m]

y[m]

z[m]

Figure 6.6: Work cycle - Position in the work space


The last group are the drivers. The first set considers the first rotational jointand so the 3 actions C1, C2, C3, while the real force is at the prismatic joint andthe second set is F1, C2, C3.Infine sono riportate le azioni dei motori: prima considerando come motrice la coppia al primo giunto rotoidale (rappresenta solo uno step di passaggio), poi la forza che agisce al manicotto.

0 0.5 1 1.5 2 2.5 30

100

200

300

400

500

t [s]

C1

[Nm

]

0 0.5 1 1.5 2 2.5 3−60

−40

−20

0

20

40

t [s]

C2

[Nm

]

0 0.5 1 1.5 2 2.5 3−10

−5

0

5

10

t [s]

C3

[Nm

]

0 0.5 1 1.5 2 2.5 3−20

−10

0

10

20

30

t [s]

F1 [N

m]

0 0.5 1 1.5 2 2.5 3−60

−40

−20

0

20

40

t [s]

C2

[Nm

]

0 0.5 1 1.5 2 2.5 3−10

−5

0

5

10

t [s]

C3

[Nm

]

Figure 6.7: Work cycle - Robot dynamics

Infine sono riportate le azioni dei motori: prima considerando come motrice la coppia al primo giunto rotoidale (rappresenta solo uno step di passaggio), poi la forza che agisce al manicotto.

0 0.5 1 1.5 2 2.5 30

100

200

300

400

500

t [s]

C1

[Nm

]

0 0.5 1 1.5 2 2.5 3−60

−40

−20

0

20

40

t [s]

C2

[Nm

]

0 0.5 1 1.5 2 2.5 3−10

−5

0

5

10

t [s]

C3

[Nm

]

0 0.5 1 1.5 2 2.5 3−20

−10

0

10

20

30

t [s]

F1 [N

m]

0 0.5 1 1.5 2 2.5 3−60

−40

−20

0

20

40

t [s]

C2

[Nm

]

0 0.5 1 1.5 2 2.5 3−10

−5

0

5

10

t [s]

C3

[Nm

]

Figure 6.8: Work space - Robot dynamics with prismatic joint force


This sequence shows the entire work cycle simulated in Simulink/SimMechanics,without the prismatic joint and providing α, ϑ, ϕ as inputs. In Figure 6.14 is addedthe complete robot scheme, with one more actuator in order to eventually simulatethe external force.

Figure 6.9: Work cycle - Point P1


Figure 6.11: Work cycle - Point Pi




Figure 6.14: Work cycle - scheme of the Simulink/SimMechanics model

Appendix A

Laws of Motion

Choosing between laws of motion available in literature, they should at least guar-antee continuity in space and speed domains. Even the continuity in accelerationwould be good practise. The most common is the family of three-steps motion laws,with a symmetric shape and where one of the derivatives has constant sections.They are often chosen because simple and well suited to evaluate main behavioursin the mechanism.

Three-steps motion law

Among all the possible cases in this project the three-steps constant accelerationhas been implemented. Therefore, the speed draws a symmetric trapezoid as inFigure A.1.

Prof. Giovanni Legnani, Ing. Hermes Giberti Meccanica dei Robot

Figura 2: Legge tre tratti e cicloidale.

Per il debug della cinematica si confrontino le velocita ed accelerazioni calcolate analitica-mente con quelle calcolate numericamente.

Ad esempio, definite le quantita: s, v, a ovvero array di posizione, velocita ed accelerazionecalcolati “analiticamente”, tt array con tempi campionati e dT passo di campionamento, sipossono calcolano i corrispondenti valori numericamente attraverso il seguente listato Matlab:✞s_p = diff(s)/dT; % velocita ( calcolo per derivazione numerica)

% (si perde un punto)

s_pp=diff(v)/dT; % accelerazione (calcolo per derivazione numerica)


figure (100)

plot(tt ,v, tt(1:end -1),s_p) % i grafici devono essere sovrapposti

figure (101)

plot(tt ,a, tt(1:end -1),s_pp) % i grafici devono essere sovrapposti ✌✝ ✆

2 Esercitazione: 18 ottobre 2012

Figure A.1: Function three-steps motion law

32

33

Writing the kinematic relations under the assumption of ∆t1 = ∆t3 (always sym-metric shape), the maximum speed and acceleration are

vmax =∆s

T

1

(1− λ)(A.1)

amax =∆s

T 2

1

λ(1− λ)(A.2)

whereλ =

∆t1T

(A.3)

The variable λ is the degree of freedom to change the law shape. For example withhigher λ, because ∆t1 grows also vmax increases while the amax decreases.

Cycloidal motion law

A different possibility is to implement a motion law described through trigonomet-ric functions.

Prof. Giovanni Legnani, Ing. Hermes Giberti Meccanica dei Robot

Figura 2: Legge tre tratti e cicloidale.

Per il debug della cinematica si confrontino le velocita ed accelerazioni calcolate analitica-mente con quelle calcolate numericamente.

Ad esempio, definite le quantita: s, v, a ovvero array di posizione, velocita ed accelerazionecalcolati “analiticamente”, tt array con tempi campionati e dT passo di campionamento, sipossono calcolano i corrispondenti valori numericamente attraverso il seguente listato Matlab:✞s_p = diff(s)/dT; % velocita ( calcolo per derivazione numerica)


s_pp=diff(v)/dT; % accelerazione (calcolo per derivazione numerica)


figure (100)

plot(tt ,v, tt(1:end -1),s_p) % i grafici devono essere sovrapposti

figure (101)

plot(tt ,a, tt(1:end -1),s_pp) % i grafici devono essere sovrapposti ✌✝ ✆

2 Esercitazione: 18 ottobre 2012

Figure A.2: Function cycloidal motion law

x = ∆s( tT− 1

2πsin(

2πt

T

))(A.4)

x =∆s

T

(1− cos

(2π

t

T

))(A.5)

x =∆s

T 22π sin

(2π

t

T

)(A.6)

In this casevmax = 2

∆s

T(A.7)

amax = 2π∆s

T 2(A.8)

Documents

Industrial Robot Analysis for Cutting Cycle