Indira Gandhi National Open University

Final Year Course

ROBOTICS(BME-029)

By

K.Kiran Kumar

Assistant professor

Mechanical engineering department

B.S.Abdur Rahman University

Email:- kiranmek3@gmail.com / kiranmedesign@gmail.com

BLOCK 2Topics

KINEMATICS AND DYNAMICS OF ROBOTS

UNIT 4- Kinematic Transformations

UNIT 5- Kinematic analysis

UNIT 6- Dynamics

UNIT 4 Kinematic Transformations

Kinematics• It is defined as the relations between the motions

of the interconnecting bodies and the joints without considering forces causing these motions.

• The inclusion of the cause of forces and torques at the joints, is called Dynamics

Kinematic Structure of the Robot

• Links• Joints/pairs• Revolute Joint (R)• Prismatic Joint (P) • Helical joint (H)• Cylindrical joint (C )• Spherical joint (s )• Planar joint (E)

Kinematic chain is a series of links connected by joints.The kinematic chain in which each and every link is coupled to two other links, it is called closed .It is open if it contains exactly two links, namely, the end ones, that are coupled to only one link.

– Simple joints

• prismatic—sliding joint, e.g., square cylinder in square tube

• revolute—hinge joint

– Compound joints

• ball and socket = 3 revolute joints

• round cylinder in tube = 1 prismatic, 1 revolute

• Mobility

– Wheels

– multipedal (multi-legged with a sequence of actions)

contd….

Degrees of freedom

The number of independent co-ordinates required to Fully

describe the motion of configuration.

Most working robots today have 6 degrees of freedom.

Determining degrees of freedom for spatial systems is called Kutzbachcriterion and determining degrees of freedom for planar systems is called Grubler criterion.

Degrees of freedom (DOF)=ξ(r-p-1)+pΣi (ni )

ξ- dimension of working space(For planar: 3 and spatial: 6)

r- Number of links in the system

p- Number of kinematic pairs/joints in the system

ni - Relative degrees of freedom of each joint.

DOF of a Rigid Body

In a plane

In space

Degrees-of-Freedom

3D Space = 6 DOF

3 position

3 orientation

In robotics,

DOF = number of independently driven joints

As DOF positioning accuracy

computational complexity

cost

flexibility

power transmission is more difficult

Robot Mapping(Application of Rigid body co-ordinate transformation)

CeTa

P

Ho

n

Tool

R

0

Note: Drill not shown, (the Tool frame is actually located at the drill tip!)

Contd…

• Robot Reference Frames

– World frame

– Joint frame

– Tool frame

10

x

yz

x

z

y

W R

PT

Configuration of Rigid body Coordinate Transformation

• Homogeneous coordinates: embed 3D vectors into 4D by adding a “1”

• More generally, the transformation matrix T has the form:

FactorScalingTrans. Perspect.

Vector Trans. MatrixRot.T

a11 a12 a13 b1

a21 a22 a23 b2

a31 a32 a33 b3

c1 c2 c3 sf

Defining the Homogeneous Transformation Matrix

• It is a 4x4 Matrix that describes “3-Space” with information that relates Orientation and Position (pose) of a remote space to a local space

nx ox ax dx

ny oy ay dy

nz oz az dz

0 0 0 1

N vector projects the Xrem Axis to the Local Coordinate System

O vector projects the Yrem Axis to the Local Coordinate System

A vector projects the Zrem Axis to the Local Coordinate System

D vector is the position of the origin of the remote space in Local Coordinate dimensions

This 3x3 ‘Sub-Matrix’ is the information that relates orientation of Framerem to Frame Local

(This is called R the rotational Submatrix)

Defining the Homogeneous Transformation Matrix

nx ox ax dx

ny oy ay dy

nz oz az dz

0 0 0 1 Perspective or Projection Vector

Scaling Factor

This matrix is a transformation tool for space motion!

3D Rotation of Points

Rotation around the coordinate axes, counter-clockwise:

100

0cossin

0sincos

)(

cos0sin

010

sin0cos

)(

cossin0

sincos0

001

)(

z

y

x

R

R

R

p

p’

y

z

x

Detailed derivations(Concept behind the generation of the previous matrices)

Not needed for examination point of view

1 1 1 1 1 1 1

0 0 0 0 0 0 0

p p p

p p p

P x i y j z k

P x i y j z k

0 1 0 1 1 1 1 1 1 0

0 1 0 1 1 1 1 1 1 0

0 1 0 1 1 1 1 1 1 0

( )

( )

( )

p p p p

p p p p

p p p p

x P i x i y j z k i

y P j x i y j z k j

z P k x i y j z k k

Relating these two definition of the SAME Point:

Derivation continued…

0 1 0 1 1 0 1 1 0 1

0 1 0 1 1 0 1 1 0 1

0 1 0 1 1 0 1 1 0 1

( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

p p p p

p p p p

p p p p

x i i x j i y k i z

y i j x j j y k j z

z i k x j k y k k z

0 1 0 1 0 1 0 1

0 1 0 1 0 1 0 1

0 1 0 1 0 1 0 1

p p

p p

p p

x i i j i k i x

y i j j j k j y

z i k j k k k z

Rewriting it in Matrix Form:

Psst:This is a R matrix!

17

Review• Orientation Representation

(Euler Angles)

– Description of Yaw, Pitch, Roll

• A rotation of about the OX axis

( ) -- yaw

• A rotation of about the OY axis

( ) -- pitch

• A rotation of about the OZ axis

( ) -- rollX

Y

Z

,xR

,yR

,zR

yaw

pitch

roll

Using the same Inner Product approach

• T rotz =

• Trotx =

• Troty =

1 0 0 0

0 0

0 0

0 0 0 1

Cos Sin

Sin Cos

0 0

0 1 0 0

0 0

0 0 0 1

Cos Sin

Sin Cos

0 1

0 1

0 1

0 0

0 0

0 0 1 0

0 0 0 11 1

p p

p p

p p

Cos Sinx x

Sin Cosy y

z z

p

p’

y

z

x

Example 1

Frame xyz is first offset from the base frame by

p = *200 100 0+, followed by a 90˚ rotation applied about the frame (or body) z axis?

1000

0100

100001

200010

1000

0100

0001

0010

1000

0100

100010

200001

H

H = H(p) H(z, 90˚)

Example 2

A point is attached to a rotating frame, the frame rotates 60 degree about the OZ axis of the reference frame. Find the coordinates of the point relative to the reference frame after the rotation.

20

)2,3,4(uvwa

2

964.4

598.0

2

3

4

100

05.0866.0

0866.05.0

)60,( uvwxyz azRota

21

Denavit-Hartenberg (DH) Convention• Number the joints from 1 to n starting with the base and ending with the

end-effector.

• Establish the base coordinate system. Establish a right-handed orthonormal coordinate system at the supporting base with axis lying along the axis of motion of joint 1.

• Establish joint axis. Align the Zi with the axis of motion (rotary or sliding) of joint i+1.

• Establish the origin of the ith coordinate system. Locate the origin of the ith coordinate at the intersection of the Zi & Zi-1 or at the intersection of common normal between the Zi & Zi-1 axes and the Zi axis.

• Establish Xi axis. Establish or along the common normal between the Zi-1 & Zi axes when they are parallel.

• Establish Yi axis. Assign to complete the right-handed coordinate system.

• Find the link and joint parameters

),,( 000 ZYX

iiiii ZZZZX 11 /)(

iiiii XZXZY /)(

0Z

Robot Kinematic Modeling

DH Notation parameters

• Not all ith variables relate to i and i-1

• Link offset (di):Distance from xi-1 to xi along zi

• Joint angle (i): angle between xi-1 and xi about zi

• Link length (ai): distance from zi to zi+1 along xi

• Link Twist (i): angle between zi and zi+1 about xi

Robot Kinematic ModelingDefine the Link Transformation Matrices

The position and orientation of the i-th coordinate frame can be expressed in the (i – 1)-th coordinate frame by the following homogeneous transformation matrix:

1 0 0 0

r α cos sinα 0

sinθd θcossinα- θcosα cos sinθ

θ cosd sinθsinα sinθ α cos- θ cos

T )(qA 1-

iii

iiiiiii

iiiiiii

i

i

ii

Ai = Rot(z, ) Trans(0, 0, ri) Trans(di, 0, 0) Rot(x, )

Robot Kinematic ModelingCompute the Forward Transformation Matrix

The position and orientation of the end-effector coordinate frame is expressed in the base coordinate frame by the forward transformation matrix:

0TN(q1, q2,…, qN) = 0TN = A1A2…AN =

1 0 0 0

p a s n

p a s n

p a s n

zzzz

yyyy

xxxx

Problem 1

TTTTTB

W

2

3

1

2

0

1

0

3

Consider a 3-link manipulator

We can derive kinematic equations:

i i-1 ai-1 di i

1 0 0 0 1

2 0 L1 0 2

3 0 L2 0 3

1

X

3

Y

1

Y2

X2

Y

3

X

0

X

0

Y

D-H transformation

Algebraic Solution

1000

0100

00

0

1000

0100

00

0

1000

0100

00

00

1000

0

33

233

2

3

22

122

1

2

11

11

1000101

1000101

011

0

1

cs

Lsc

Tcs

Lsc

T

cs

sc

dccscss

dsscccs

asc

T

Algebraic Solution

The kinematics of the example seen before are:

1000

0100

0

0

12211123123

12211123123

0

3

slslcs

clclsc

TTB

W

Assume goal point is the specification of wrist frame, specified by 3 numbers:

1000

0100

0

0

ycs

xsc

TB

W

Algebraic SolutionBy comparison, we get

the four equations:

12211

12211

123123

slsly

clclx

sscc

Summing the square

of the last 2 equations: 221

2

2

2

1

22 2 cllllyx

From here we get an

expression for c221

2

2

2

1

22

22 ll

llyxc

).,(2tan1 222

2

22 csAcs And finally:

30

Problem 2

Joint i i ai di i

1 0 a0 0 0

2 -90 a1 0 1

3 0 0 d2 2

a0 a1

Z0

X0

Y0

Z3

X2

Y1

X1

Y2

d2

Z1

X33O

2O1O0OJoint 1

Joint 2

Joint 3

31

Contd..Joint i i ai di i

1 0 a0 0 0

2 -90 a1 0 1

3 0 0 d2 2

1000

0100

0cosθsinθ

0sinθcosθ

00

00

0

00

00

1sin

cos

a

a

T

1000

000

sinθ

cosθ

1

1

11

sincos0

cossin0

111

111

2

a

a

T

))()(( 210

3213

0 TTTT

1000

0sinθcosθ 22

2

2

223

100

00cossin

0

dT

1000

01

iii

iiiiiii

iiiiiii

i

idCS

SaCSCCS

CaSSSCC

T

Problem: 3SCARA Robot

8/30/2014 Page 33

UNIT 5Kinematic analysis

POSITION ANALYSISFKS vs. IKS

• In FKS(Forward kinematics) we built a tool for finding end frame geometry from Given Joint data:

• In IKS (Inverse kinematics) we need Joint models from given End Point Geometry:

Joint

Space

Cartesian

Space

Joint

SpaceCartesian

Space

Forward vs. Inverse Kinematics

• Forward Kinematics

– Compute configuration end effector position given individual DOF values(i.e. link angles and lengths).

• Inverse Kinematics

– Compute individual DOF values that result in specified end effector position.

Forward and inverse kinematics :

• Robot kinematic calculations deal with the relationship between joint positions and an external fixed Cartesian coordinate frame.

• Dynamics, force, momentum etc. are not considered.

Forward

Kinematic

Equations

Inverse

Kinematic

Equations

Joint space

(J1,J2 ..)

Cartesianspace

(x,y,z,O,A,T)

Forward Kinematic Problem

q2

q1

q3

x

y

z

Given: q1, q2, q3….(n joint positions)

Find: End-Effector position PE

and orientation RE

(m end-effector parameters)

Forward Kinematic Problem1. Assign Cartesian Coordinate frames to each link

(including the base & end-effector N)

2. Identify the joint variables and link kinematicparameters

3. Define the link transformation matrices. i-1Ti = Ai

4. Compute the forward transformation0TN(q1, q2,…, qN) = A1A2A3…AN =

1 0 0 0

p a s n

p a s n

p a s n

zzzz

yyyy

xxxx

Direct Kinematics• Position of tip in (x,y) coordinates

Inverse Kinematics (IK)

• Given end effector position, compute required joint angles.

• In simple case, analytic solution exists

– Use trignometry, geometry, and algebra to solve

Inverse Kinematic Problem

Given: Position & Orientation Find:of END-EFFECTOR

joint coordinates

0TN q1, q2, q3,…, qN

Need to solve at most six independent equations in N unknowns.

Direct versus Inverse Kinematics

• Direct Kinematics x = L1*cos(t1) + L2*cos(t1+t2)

y = L1* sin(t1) + L2*sin(t1+t2)

Given the joint angles t1 and t2 we can compute the position of the tip (x,y)

• Inverse Kinematics Given x and y we can compute t1 and t2

t2 = acos[(x^2 + y^2 - L1^2 - L2^2)/(2*L1*L2)]

This gives us two values for t2, now one can compute the two corresponding values of t1.

2

1

(x , y)

l2

l1

Inverse Kinematics of a Two Link Manipulator

Given: l1, l2 , x , y

Find: 1, 2

Redundancy:A unique solution to this problem

does not exist. Notice, that using the “givens” two solutions are possible. Sometimes no solution is possible.

(x , y)

21

2

2

2

1

22

2

221

2

2

2

1

21121121

2

2

2

1

21121

2

21

2

2

2

1

2

121121

2

21

2

2

2

1

2

1

2222

2

yxarccosθ

c2

)(sins)(cc2

)(sins2)(sins)(cc2)(cc

yx)2((1)

ll

ll

llll

llll

llllllll

Solution:

l1

l22

1

(x , y)

21

21211

21211

1221

11

θθθ(3)

sinsy(2)

ccx(1)

)θcos(θc

cosθc

ll

ll

Only Unknown

))(sin(cos))(sin(cos)sin(

))(sin(sin))(cos(cos)cos(

:

abbaba

bababa

Note

))(sin(cos))(sin(cos)sin(

))(sin(sin))(cos(cos)cos(

:

abbaba

bababa

Note

)c(s)s(c

cscss

sinsy

)()c(c

ccc

ccx

2211221

12221211

21211

2212211

21221211

21211

lll

lll

ll

slsll

sslll

ll

We know what 2 is from the previous slide. We need to solve for 1 . Now we have two equations and two unknowns (sin 1 and cos 1 )

22

222211

2212

22

1122221

221122221

221

221

2211

yx

x)c(ys

)c2(sx)c(

1

)c(s)s()c(

)(xy

)c(

)(xc

slll

llllslll

lllll

sls

ll

sls

Substituting for c1 and simplifying many times

Notice this is the law of cosines and can be replaced by x2+ y2

22

222211

yx

x)c(yarcsinθ

slll

Solution To Inverse Kinematics Problems

0TN = 0T11T2

2T3…N-1TN = A1A2A3…AN

Find: q = q1, q2, q3, … , qN (joint coordinates)

1

p a o n

p a o n

p a o n

Tzzzz

yyyy

xxxx

N

0

i i i i i i i

i i i i i i i

i i i

cθ -c sθ s sθ d cθ

sθ c cθ -s cθ d sθAi

s c r

1

Given:

Solution To Inverse Kinematics

General Approach: Isolate one joint variable at a time

A1-1 0TN = A2A3…AN = 1TN

function of q1 function of q2, … , qN

• Look for constant elements in 1TN

• Equate LHS(i,j) = RHS(i,j)• Solve for q1

Solution To Inverse KinematicsA2

-1A1-10TN = A3…AN = 2TN

function of q1, q2

function of q3, … , qN

only one unknown q2 since q1 has been solved for

• Look for constant elements of 2TN

• Equate LHS(i,j) = RHS(i,j)• Solve for q2

• Maybe can find equation involving q1 only

Note: – There is no algorithmic approach that is

100% effective– Geometric intuition is required

8/30/2014 Page 48

Comparison of Inverse kinematics

Limitations:– The Solution set

can contain Redundancies:

Multiple solutions– equations to solve

are in general nonlinear, and thus it is not always possible to find a closed form solution

Advanatges:• Allow “Off-Line

Programming” solutions

• The IKS allows the engineer to equate Workspace capabilities with Programming realities to assure that execution is feasible.

• The IKS Aids in Workplace Design and Operational Simulations.

Jacobian Matrix

6

5

4

3

2

1

z

y

x

z

y

x

6

5

4

3

2

1

z

y

x

49

Joint Space Task Space

Forward

Inverse

Kinematics

Jaconian Matrix: Relationship between joint space velocity with task space velocity

Jacobian Matrix

Jacobian Matrix

z

y

x

z

y

x

50

1

2

1

6

)(

nn

n

q

q

q

dq

qdh

nn

n

n

n

q

h

q

h

q

h

q

h

q

h

q

hq

h

q

h

q

h

dq

qdhJ

6

6

2

6

1

6

2

2

2

1

2

1

2

1

1

1

6

)(

Jacobian is a function of q, it is not a constant!

51

Problem• 2-DOF planar robot arm

– Given l1, l2 , Find: Jacobian2

1

(x , y)

l2

l1

),(

),(

)sin(sin

)cos(cos

212

211

21211

21211

h

h

ll

ll

y

x

)cos()cos(cos

)sin()sin(sin

21221211

21221211

2

2

1

2

2

1

1

1

lll

lll

hh

hh

J

2

1

Jy

xY

52

Problem• Find the singularity configuration of the 2-DOF planar

robot arm

)cos()cos(cos

)sin()sin(sin

21221211

21221211

lll

lllJ

2

1

Jy

xY

2

1

(x , y)

l2

l1

x

Y

=0

V

determinant(J)=0 Not full rank

02

Det(J)=0

Jacobian Matrix• Inverse Jacobian

• Singularity• Occurs when two or more of the axes of the robot form a

straight line, i.e., collinear

–Avoid it

53

666261

262221

161211

JJJ

JJJ

JJJ

qJY

6

5

4

3

2

1

q

q

q

q

q

q

YJq 1

5q

1q

UNIT 6Dynamics

(To be considered for real-time applications) A dynamic model can be used to develop suitable control strategies.

A sophisticated controller requires the use of realistic dynamic model to achieve optimal performance under high speed operations.

Some control schemes rely directly on the dynamic model to compute actuator torques and forces required to follow a desired trajectory.

The dynamic analysis of a robot reveals all the joint reaction forces and moments needed for the design and sizing of the links, bearings and actuators.

Dynamics - the study of motion with regard to forces (the study of the relationship

between forces/torques and motion). Composed of kinematics and kinetics.

a) Forward Dynamics (simulation) - given the actuator forces and torques, it will compute the motion.

b) Inverse Dynamics (control) - given the desired motion, calculate the actuator forces and torques.

We will examine two approaches to this problem

• Euler – Lagrange Approach:– Develops a “Lagrangian Function” which relates Kinetic and Potential

Energy of the manipulator, as it is moving, thus dealing with the manipulator “As a Whole” in building force/torque equations

• Newton – Euler Approach:– This approach works to separate the effects of each link on machine

torques by writing down its motion in a separable linear and angular sense.

– However, due to the highly coupled motions in a robot, it requires a forward recursion through the entire manipulator for building velocity and acceleration models of a link followed by a backward recursion for force and torque on each link ‘in turn’

Euler – Lagrange approach

• Employs a Denavit-Hartenberg structural analysis to define “Generalized Coordinates” for the structural models of the machine.

• It provides a closed form interpretation of the various components in the dynamic model:

– Due to Inertia

– Due to Gravitational Effects

– Due to Friction (joint/link/driver)

– Due to Coriolis Forces relating motion of one link to coupling effects of other links’ motions

– Due to Centrifugal Forces that cause the link to have a tendency to ‘fly away’ due to coupling to neighboring links and its own motion

Newton-Euler Approach

• A ‘computationally more efficient’ approach to force/torque determination

• It starts at the “Base Space” and moves forward toward the “End Space” – computing trajectory, velocity and acceleration demands then

• Using this ‘forward velocity’ information the control computes forces and moments starting at the “End Space” and moving back to the “Base Space”

Manipulator DynamicsBy Euler-Lagrangian Approach

• Lagrange-Euler Formulation

– Lagrange function is defined

• K: Total kinetic energy of robot

• P: Total potential energy of robot

• : Joint variable of i-th joint

• : first time derivative of

• : Generalized force (torque) at i-th joint

iq

iq

58

PKL

i

ii q

L

q

L

dt

d

)(

iqi

Manipulator Dynamics• Kinetic energy

– Single particle:

– Rigid body in 3-D space with linear velocity (V) , and angular velocity ( ) about the center of mass

– I : Inertia Tensor: • Diagonal terms: moments of inertia

• Off-diagonal terms: products of inertia

2

2

1mvk

TT IVmVk2

1

2

1

59

dmyxyzdmxzdm

yzdmdmzxxydm

xzdmxydmdmzy

I

)(

)(

)(

22

22

22

dmzyIxx )( 22

dmxyIxy )(

Velocity of a link

60

i

irix

iy

iz

0x

0z

0yir0

1

i

i

i

i

iz

y

x

r

i

i

i

i

i

i

ii rTTTrTr )( 1

2

1

1

000

A point fixed in link i and expressed w.r.t. the i-th frame

Same point w.r.t the base frame

Velocity of a link

i

i

i

j

j

j

ii

i

ii

i

i

i

i

i

i

i

i

i

i

i

i

i

i

i

ii

i

rqq

TrTrTTT

rTTTrTTT

rTTTdt

dr

dt

dVV

)(

)(

1

001

2

1

1

0

1

2

1

1

01

2

1

1

0

1

2

1

1

000

i

ir

61

Velocity of point expressed w.r.t. i-th frame is zero

Velocity of a link

1000

01

iii

iiiiiii

iiiiiii

i

idCS

SaCSCCS

CaSSSCC

T

1000

00001 iiiiiii

iiiiiii

i

i

iCaSSSCC

SaCSCCS

q

T

1000

0

0000

0000

0001

0010

11

iii

iiiiiii

iiiiiii

i

ii

i

i

i

dCS

SaCSCCS

CaSSSCC

TQq

T

iiq

62

• Rotary joints,

0000

0000

0001

0010

iQ

Velocity of a link

1000

01

iii

iiiiiii

iiiiiii

i

idCS

SaCSCCS

CaSSSCC

T

0000

1000

0000

0000

1

i

i

i

q

T

i

ii

i

i

i TQq

T1

1

ii dq

63

• Prismatic joint,

0000

1000

0000

0000

iQ

Velocity of a link

ijfor

ijforTTQTTT

q

Ti

i

j

jj

j

j

j

i

0

11

1

2

2

1

1

00

ijfor

ijforTQT

q

TU

i

jj

j

j

i

ij0

1

1

00

i

j

i

ijij

i

i

i

j

j

j

ii

i

i

i

ii

i rqUrqq

TrTTT

dt

dr

dt

dVV

11

01

2

1

1

000 )()()(

64

The effect of the motion of joint j on all the points on link i

Kinetic energy of link i

i

p

rp

T

ir

Ti

i

i

iip

i

r

ii qqUdmrrUTrdKK1 1

)(2

1

iiiiiii

ii

zzyyxx

yzxz

iiyz

zzyyxx

xy

iixzxy

zzyyxx

iii

iiiiii

iiiiii

iiiiii

Ti

i

i

ii

mzmymxm

zmIII

II

ymIIII

I

xmIIIII

dmdmzdmydmx

dmzdmzdmzydmzx

dmydmzydmydmyx

dmxdmzxdmyxdmx

dmrrI

2

2

2

2

2

2

1

i

i

i

i

iz

y

x

r

dmxm

x i

i

i

1

65

Center of mass

Pseudo-inertia matrix of link i

Manipulator Dynamics

n

i

i

p

i

r

rp

T

iriip

i

p

rp

T

ir

Ti

i

i

iip

i

r

n

i

n

i

i

qqUIUTr

qqUdmrrUTrKK

1 1 1

1 111

)(2

1

)(2

1

iI

iq iq

66

• Total kinetic energy of a robot arm

: Pseudo-inertia matrix of link i, dependent on the mass distribution of link i and are expressed w.r.t. the i-th frame,

Need to be computed once for evaluating the kinetic energy

Scalar quantity, function of and , ni ,2,1

Manipulator Dynamics

)( 00

i

i

i

i

i

ii rTgmrgmP

n

i

n

i

i

i

i

ii rTgmPP1 1

0 ])([

)0,,,( zyx gggg

iq

67

• Potential energy of link i

: gravity row vector expressed in base frame2sec/8.9 mg

ir0

i

ir : Center of mass w.r.t. i-th frame

: Center of mass w.r.t. base frame

• Potential energy of a robot arm

g

Function of

Manipulator Dynamics

j

jji

n

ij

j

n

ij

j

k

j

m

mk

T

jij

m

jkn

ij

j

k

k

T

jijjk

ii

i

rUgm

qqUIq

UTrqUIUTr

q

L

q

L

dt

d

1 11

)()(

)(

68

• Lagrangian function

n

i

i

j

i

k

n

i

i

i

i

ikj

T

ikiij rTgmqqUIUTrPKL1 1 1 1

0 )()(2

1

Manipulator Dynamics

ijfor

ijforTQT

q

TU

i

jj

j

j

i

ij0

1

1

00

kiorji

kjiTQTQT

jkiTQTQT

Uq

Ui

jj

j

kk

k

i

kk

k

jj

j

ijk

k

ij

0

1

1

1

1

0

1

1

1

1

0

69

The interaction effects of the motion of joint j and joint k on all the points on link i

The effect of the motion of joint j on all the points on link i

Manipulator Dynamics

im

n

k

n

m

kikm

n

k

kiki CqqhqD

1 11

n

kij

T

jijjkik UIUTrD),max(

)(

n

mkij

T

jijjkmikm UIUTrh),,max(

)(

j

jji

n

ij

ji rUgmC

70

• Dynamics Model

Manipulator Dynamics

)(),()( qCqqhqqD

nnn

n

DD

DD

D

1

111

nh

h

qqh 1

),(

nC

C

qC 1

)(

71

• Dynamics Model of n-link Arm

The Acceleration-related Inertia matrix term, Symmetric

The Coriolis and Centrifugal terms

The Gravity terms

n

1

Driving torque applied on each link

Example

X0

Y0

X1Y1

1

L

m

1

0

01

1

l

r

1

1

11

11

1

1

1

0

1

0

1000

0100

00

00

rCS

SC

rTr

72

Example: One joint arm with point mass (m) concentrated at the end of the arm, link length is l , find the dynamic model of the robot using

L-E method.

Set up coordinate frame as in the figure

]0,0,8.9,0[ g

Contd…

X0

Y0

X1Y1

1

L

m

1

1

11

11

1

1

1

0

1

0

1000

0100

00

00

rCS

SC

rTr

0

0

1

1

1

1

1

01

1

1

1

0

1

01

Cl

Sl

rTQrTdt

drV

73

Contd…

dmClSlCl

Sl

TrK2

111

1

1

)00

0

0(

2

1

dmVVTrdKT)(

2

111

74

mClCSl

CSlSl

Tr2

1

2

1

2

11

2

11

22

1

2

)

0000

0000

00)(

00)(

(2

1

Kinetic energy

Contd…

)( 1

1

0 rTmgP

1

0

0

1000

0100

00

00

008.9011

11 l

CS

SC

m

18.9 Slm

75

• Potential energy

1

2

1

2 8.92

1 SlmmlPKL

11

2

11

2

11

1

8.98.9)(

)(

ClmmlClmmldt

d

LL

dt

d

• Lagrange function

• Equation of Motion

Concept behind Newton-Euler approach

• This method is jointly based on:– Newton’s 2nd Law of Motion Equation:

and considering a ‘rigid’ link

– Euler’s Angular Force/ Moment Equation:

i CF m

i imoment CM i i CM iN I I

Again we will Find A “Torque” Model

• Each Link Individually

• We will move from Base to End to find Velocities and Accelerations

• We will move from End to Base to compute force (f) and Moments (n)

• Finally we will find that the Torque is:

1 1(1 )i i iqT T

n z f z bi i i i i i

Gravity is implicitly included in the model by considering acc0

= g where g is (0, -g0, 0) or (0, 0, -g0)

i is the joint type

parameter (is 1 if revolute; 0 if prismatic)

like in Jacobian!

We will Build Velocity Equations

• Consider that i is the joint type

parameter (is 1 if revolute; 0 if prismatic)

• Angular velocity of a Frame k relative to the Base:

• NOTE: if joint k is prismatic, the angular velocity of frame k is the same as angular velocity of frame k-1!

1 1k k k k kq z

Angular Acceleration of a “Frame”

• Taking the Time Derivative of the angular velocity model of Frame k:

1 1 1 1k k k k k k k kq z q z

Same as (dw/dt) the

angular acceleration in dynamics

Linear Velocity of Frame k:

• Defining sk = dk – dk-1 as a link vector, Then the

linear velocity of link K is:

• Leading to a Linear Acceleration Model of:

1 11k k k k k k kv v s q z

1

1 11 2

k k k k k k k

k k k k k k

s s

q z q z

Normal component of acceleration (centrifugal

acceleration)

This completes the Forward Newton-Euler Equations:

• To evaluate Link velocities & accelerations, start with the BASE (Frame0)

• Its Set V & A set (for a fixed or inertial base) is:

0

0

0

0

0

0

0v

g

Now we define the Backward (Force/Moment) Equations

• Work Recursively from the End

• We define a term rk which is the vector from the end of a link to its center of mass:

k k k

k

r c d

c

is location of center of mass of Link k

Defining f and n Models

1k k k k k k k k kf f m r r

1 1k k k k k k k k k

k k k

n n s r f r f D

D

Inertial Tensor of Link k – in base space

The term in the brackets represents the linear acceleration of the center of mass of Link k

Combine them into Torque Models:

1 11T T

k k k k k k k k kn z f z b q

The N-E Algorithm:

• Step 1: set T00 = I; fn+1 = -ftool; nn+1 = -ntool; v0 = 0;

vdot0 = -g; 0 = 0; dot

0 = 0

• Step 2: Compute –• Zk-1’s

• Angular Velocity & Angular Acceleration of Link k

• Compute sk

• Compute Linear velocity and Linear acceleration of Link k

• Step 3: set k = k+1, if k<=n back to step 2 else set k = n and continue

The N-E Algorithm cont.:

• Step 4: Compute –

• rk (related to center of mass of Link k)

• fk (force on link k)

•

• Nk (moment on link k)

• tk

• Step 5: Set k = k-1. If k>=1 go to step 4

0 0

Tk k

k kD R D R

Problem

• This 1-axis ‘robot’ is called an Inverted Pendulum

• It rotates about z0

“in the plane”

Z0

X0

Y0

Z1

X1

Y1

m1

Writing some info about the device:

1

1

2

1 11

1 1 1 1

1 1 1 11

0 1

2

0

0

1

0 0 0

0 1 012

0 0 1

0

0

0 0 1 0

0 0 0 1

a

c

m aD

C S a C

S C a ST A

“Link” is a thin cylindrical rod

Continuing and computing:1 1 1

1 0

11 1 1 1

1 1 1 1

1 1

1 1

021 0 0 0

0 00 1 0 00 0 1 0 00 0 1 00 0 0 1 1

2

2

0

c H T c

aC S a C

S C a S

a C

a S

Inertial Tensor computation:

1 1

1 0 1 0

1 1 1 12

1 11 1 1 1

2

1 1 1221 1

1 1 1

0 0 0 0 0

0 0 1 0 012

0 0 1 0 0 1 0 0 1

0

012

0 0 1

T

D R D R

C S C Sm a

S C S C

S S Cm a

S C C

(Angular Velocity & Acceleration)

1 1 1

1 1 1

1 1

1 11

1 0

1 1

1 1

0 0

0 0 0

1 1

0 0

0 0 0

1 1

1 0 0 0

( ) 0 1 0 00

0 0 1 01

0

o

q q

q q

a C

a Ss H O O

a C

a S

Starting: Base (i=0)

Ang. vel = Ang. acc = Lin. vel = 0

Lin. Acc = -g (0, -g0, 0)T

1 = 1

Linear Velocity

1 0 1 1 1 1 0

1

1 1 1 1

1

1 1 1

1

0 0

0 0 1 1 0

1 0 1

0

v v s q z

C

q a S q

S

a q C

Linear Acceleration 1 1 1 1 1 1

1 1

1 1 1 1 1 1 1

1 1

2

1 1 1 1 1 1

1 1

2

1 1 1 1 1 1

0

0

0 1 0

0 0

0 0

g s s

S S

g a q C q a q C

S C

g a q C a q S

S C

g a q C a q S

Note:

g = (0, -g0, 0)T

Thus Forward Activities are completed

• Compute r1 to

begin Backward Formations:

1 1 1

1 1

1 1

1 11 1

1

11

2

200

20

r c O

a C

a Ca S

a S

Ca

S

Finding f1

Consider: ftool = 0

1 1 1 1 1 1 1

1 1

1 1 1 11 1 1 1 1

1

1 11 1 1

{ }

0 0 0

{ 0 0 0 }2 2

1 0 1 1 0

{2

0

f m r r

C Ca q a q

m S q S

Sa q

m C

12

1 11

1 12

1 1 1 11 1 1 1

1 1 1 22 1 1 1 1

1 1 1 1 1 1 1 1

0

0 }2

1 0

{ }2 2

0 0

2 20 0 0

Sa q

C

S Ca q a q

m C S

S C Sa q a q

m g a q C a q S C

1

1

0

C

S

Collapsing the terms

1 12

1 1 1 11 1 1 1

2 2

1 1 1 1 1 1 1 1 1 1

1 0

2 20 0

02 2

T

S Ca q a q

f m g C S

a q S q C a q C q Sm g

Computing n1:

1 1 1 1 1 1 1 1 1

1 1 2 2 2

1 1 1 1 1 1 11 1 1 1

2 211 1 1 1 1 1 1 1 1 11

1 1 0

0 0 0

0 0 02 12 12

0 0 1 1 1

02 2 2

0

n s r f D D

C Ca m a q m a q

a S S f

Ca q S q C a q C q Sa

S m g

2

1 1 1

2 2 21 1 1 1 1 1 1 1 1 1 1 1 11 1

1 0 1

0

012

1

0 0

0 02 2 2 12

1 1

T

m a q

a q C q S a q S q C m a qa mC g S

This X-product goes to Zero!

The Link Force Vector

Simplifying:

21 0 1 1 11 1 1

0

03 2

1

m g m a Cn a q

Writing Torque Model

1 1 1 0 1 1 0 1 1

21 0 1 1 11 1 1 1

2

1 1 0 1 1 11 1 1 1

1

0 0

0 0 0 1 1 03 2

1 1

3 2

T T

T

n z f z b q

m g m a Ca q f b q

m a g m a Cq b q