IEEE Robotics Talk

Kinematics & Dynamics of Robots (The Articulated & the Wheeled )

Ghulam Mustafa @ Santa Clara University

04/22/2015

(Part 1: Articulated Rigid Bodies)

Some Definitions Spatial Motion of Rigid Bodies Kinematic Representation Denavit – Hartenberg Representation Two Worlds (Forward/Inverse Kinematics) Joint Velocities & Accelerations The Jacobian Computations and Algorithms

Content of this Talk : Part 1 – Articulated Rigid Bodies

Instantaneously Coincident Coordinates Sheth - Uicker Representation Kinematics of Some Wheeled Robots Time Derivatives in ICC The Wheel Jacobian Computations and Algorithms Velocity – Torque Duality Robot Dynamics : Equations of Motion Newton—Euler Recursive Algorithm Robot Control

Content of this Talk : Part 2 – Wheeled Mobile Robots

In theory, there is no difference between theory and practice. In practice there is.

It's tough to make predictions, especially about the future.

Kinematics, Dynamics and Rigid Bodies & Equations of Motion Kinematics … is the branch of mechanics which describes the motion of points, bodies and systems of bodies without consideration of the causes of motion It is a study of trajectories of points, lines and other geometric objects and their differentials, velocity and acceleration

Dynamics … is a branch of mechanics concerned with the study of forces and torques and their effect on motion.

Rigid Body … is an abstraction of a solid in which deformation does not occur. The distance between points of a rigid body remains constant regardless of external forces exerted on it.

Equations of Motion … are relationships that relate the dynamics to the kinematics under that rigid body abstraction

Newton-Euler-Lagrange [and sometimes Hamilton]

Isaac Newton 1642-1726

Joseph-Louis Lagrange

1736-1813

Leonhard Euler 1707-1783

William Hamilton 1805-1865

Spatial Motion of a Rigid Body

Any spatial motion, no matter how complex can be decomposed and reconstructed as

General Motion

Pure Translation

Pure Rotation

Newtonian Formulism … point masses on curvilinear trajectories

Eulerian Formulism … rigid bodies rotating about a point

Parallel Universes : The Newtonian – point masses move on curves – rotation makes no sense.

The Eulerian – rigid bodies rotate about a point. Both are mathematical abstractions, there are no

point masses nor rigid bodies. Real bodies have dimensions and deform.

Translation

Rotation

Rigid Body … Location = Position and Orientation

A

Position

Orientation

B

A p

Location Position Orientation B

AR

z

y

x

pB

A B

A

B

A

B

A

B

A ZYXR Position Vector Rotation Matrix

10

B

A

B

A

A

B

PRT ITTTTpTppTp A

AC

A

B

A

C

CB

C

BBA

B

A B

Base

Articulated Robot … The Forward Kinematics (FK)

Base

0

1

2

3

T0

0

TTTT 2

3

1

2

0

1

0

3 ..

T0

1

T0

2

T0

3

Forward Kinematics

Base End Effector

Robots Two Worlds | The Inner versus the Outer

Joint Space

q1

q2

q3

.

.

.

.qn

Work Space

x

y

z

a

b

g

Camera

Kuka

The camera sends the outer world information of position and orientation of objects in work space. The robot uses this information to assume a pose.

Work Space Joint Space

Inverse Kinematics

Forward Kinematics

Forward Kinematics | FK Problem Statement Given: Link orientations (the configuration) in the joint space (qi). Determine: End Effector (aka tool) position and orientation (the pose).

TTTTqqqT m

mmm

B

W

11

2

0

1

0

21 ....),...,,(

Rotation Matrix (Orientation) Position Vector

Joint Space (m dimensional)

Work Space (n dimensional)

Mathematically nmnm xqf ,:

q x

(.)f

)(qfx

FK via DH Representation

A methodology for computing the position and orientation of the End Effector with respect to the base frame of the manipulator.

This will be done by:

Defining the geometry of each link and the joints. Attaching frames to the joints. Establishing relationships between the joint axes. Writing transform equations to represent vectors in their own frame and mapping them to frames of other links.

z0 z1

z2

x0

x1

x2

Joint 0

Joint 3 Wire Frame

zi

Joint i-1

Joint i

Link i-1

zi-1

Link & Joints

Link Parameters

Link Twist

Link Length – [ai-1] defined along the common

perpendicular. Unique except for parallel axes.

Link Length & Link Twist are Fixed (link design parameters)

Link Twist – [ai-1] – right-hand rotation about [ai-1].

Joint Parameters

Link Twist

Link Offset – [di] difference in height between ai and ai-1 along the i axis.

di - Link Offset is a Variable for Primatics Joints (Joint Angle is Fixed) qi - Joint Angle is a Variable for Revolute Joints (Link Offset is Fixed)

ai

ai-1

Axis i+1

Joint Angle – [qi] – angle spanned by ai and ai-1 right-hand rotation

about [ai-1].

Forward Kinematics

ai-1

Xi-1

Oi-1

Zi-1

{i-1}

{i-1} {R} {Q} {P} {i}

)().().().(... 11

11

izizixix

P

i

Q

P

R

Q

i

R

i

i dDRaDRTTTTT a

Xi

{i}

Oi

Zi

{Q}

{R}

Forward Kinematics

)().().().(),,,( 1111

1

izizixixiiii

i

i dDRaDRdaT aa

1000

100

0010

0001

)(

i

iz

d

dD

1000

0100

00)cos()sin(

00)sin()cos(

)(

ii

ii

izR

1000

0100

0010

001

)(

1

1

i

ix

a

aD

1000

0)cos()sin(0

0)sin()cos(0

0001

)(

11

11

ii

ii

izR

aa

aa

1000

)()()()()()(

)()()()()()(

0)()(

1111

1111

1

1

iiiiiii

iiiiiii

iii

i

idccscss

dsscccs

asc

Taaaa

aaaa

restart; with(LinearAlgebra):

> m:=3:

> alias(seq(c[i]=cos(Theta[i]), i=1..m),

> seq(s[i]=sin(Theta[i]), i=1..m)):

> A := (a,alpha,d,theta) -> Matrix(4, 4, [

> [cos(theta),-sin(theta),0,a],

> [sin(theta)*cos(alpha), cos(theta)*cos(alpha),

> -sin(alpha), -a*sin(theta)*d],

> [sin(theta)*sin(alpha), cos(theta)*sin(alpha),

cos(alpha), cos(alpha)*d],

> [0, 0, 0, 1]]):

> T(a, alpha, d, theta):

> a := Vector(m,{(2)=l1,(3)=l2}):

> alpha := Vector(m):

> theta:=Vector(m,i->Theta[i]):theta[3]:=0:

> d:=Vector(m):

> for i to m do

T[i] := A(a[i], alpha[i], d[i], theta[i])

> end do:

Zi-1 Zi

Zi+1

Oi-1

Oi

Oi+1

Xi-1

Xi Xi+1

Yi-1 Yi

Yi+1

Forward Kinematics Stanford

:= T

( )c1 c2 c3 c1 s2 s3 a3 ( ) c1 c2 s3 c1 s2 c3 d4 c1 c2 a2 s1 d3

( )s1 c2 c3 s1 s2 s3 a3 ( ) s1 c2 s3 s1 s2 c3 d4 s1 c2 a2 c1 d3

( ) s2 c3 c2 s3 a3 ( )s2 s3 c2 c3 d4 s2 a2

:= DH

0 0 0 1

20 0 2

0 a2 d3 3

2a3 d4 4

20 0 5

20 0 6

DH Parameters

Tip Coordinates

Tip :=

( )( )( )c1 c2 c3 c1 s2 s3 c4 s1 s4 c5 ( ) c1 c2 s3 c1 s2 c3 s5 c6 ( )( )c1 c2 c3 c1 s2 s3 s4 s1 c4 s6[ ,

( )( )( )c1 c2 c3 c1 s2 s3 c4 s1 s4 c5 ( ) c1 c2 s3 c1 s2 c3 s5 s6 ( )( )c1 c2 c3 c1 s2 s3 s4 s1 c4 c6 ,

( )( )c1 c2 c3 c1 s2 s3 c4 s1 s4 s5 ( ) c1 c2 s3 c1 s2 c3 c5 ,

( )c1 c2 c3 c1 s2 s3 a3 ( ) c1 c2 s3 c1 s2 c3 d4 c1 c2 a2 s1 d3 ]

( )( )( )s1 c2 c3 s1 s2 s3 c4 c1 s4 c5 ( ) s1 c2 s3 s1 s2 c3 s5 c6 ( )( )s1 c2 c3 s1 s2 s3 s4 c1 c4 s6[ ,

( )( )( )s1 c2 c3 s1 s2 s3 c4 c1 s4 c5 ( ) s1 c2 s3 s1 s2 c3 s5 s6 ( )( )s1 c2 c3 s1 s2 s3 s4 c1 c4 c6 ,

( )( )s1 c2 c3 s1 s2 s3 c4 c1 s4 s5 ( ) s1 c2 s3 s1 s2 c3 c5 ,

( )s1 c2 c3 s1 s2 s3 a3 ( ) s1 c2 s3 s1 s2 c3 d4 s1 c2 a2 c1 d3 ]

( )( ) s2 c3 c2 s3 c4 c5 ( )s2 s3 c2 c3 s5 c6 ( ) s2 c3 c2 s3 s4 s6[ ,

( )( ) s2 c3 c2 s3 c4 c5 ( )s2 s3 c2 c3 s5 s6 ( ) s2 c3 c2 s3 s4 c6 ( ) s2 c3 c2 s3 c4 s5 ( )s2 s3 c2 c3 c5, ,

( ) s2 c3 c2 s3 a3 ( )s2 s3 c2 c3 d4 s2 a2 ]

[ , , , ]0 0 0 1

FK from Base to Tip

The Jacobian

Base

0

1

2

3

v

11qJ

22qJ

33qJ

qJqJqJqJv 332211

Linear combination of Joint velocities

Linear Velocity Angular Velocity

Tzyxzyx vvvv

v

J

JJ

vLinear velocity contribution

Angular velocity contribution

The Jacobian – via Direct Computation

10

00

0 nn

n

PRT

x

y

x

xy

xz

yz

S

0

0

0

> restart; with(LinearAlgebra):

> m:=3:alias(seq(c[i]=cos(Theta[i]), i=1..m),

> seq(s[i]=sin(Theta[i]), i=1..m)):

> A := (a,alpha,d,theta) -> Matrix(4, 4, [

> [cos(theta),-sin(theta),0,a],

> [sin(theta)*cos(alpha), cos(theta)*cos(alpha),

> -sin(alpha), -a*sin(theta)*d],

> [sin(theta)*sin(alpha), cos(theta)*sin(alpha),

cos(alpha), cos(alpha)*d],

> [0, 0, 0, 1]]):

> A(a, alpha, d, theta):

> a := Vector(m,{(2)=l1,(3)=l2}):

> alpha := Vector(m):

> theta:=Vector(m,i->Theta[i]):theta[3]:=0:

> d:=Vector(m):

> for i to m do

T[i] := A(a[i], alpha[i], d[i], theta[i])

> end do:

> Tip := `.`(seq(T[i], i=1..m)):

> collect(Tip[1,1], [c[1], c[2], s[1]]):

> T0||m := Column(Tip, 4):

> P0||m := SubVector(%, [1..3]):

> R := SubMatrix(Tip, [1..3], [1..3]):

> thetas := [seq(Theta[i], i=1..m)]:

> with(VectorCalculus, Jacobian):

> Jv:=Jacobian(P0||m, thetas):

> Rtranspose := Transpose(R):

> for i from 1 to m-1 do

R||i:=map(diff,R,theta[i]):

R||i.Rtranspose:

omega := simplify(%):

Jomega[i] := Vector[row]

([omega[3,2], -omega[3,1],

omega[2,1]]):

print(evaln(Jomega[i])=Jomega[i])

> end do:

> Jomega := Transpose(<seq(Jomega[i], i=1..m-1)>);

> Jv:=SubMatrix(Jv, [1..3], [1..2]);

> J := <Jv , Jomega>;

v

Position Orientation

qJqq

Pq

q

Pq

q

Pv v

nnn

3

3

0

2

2

0

1

1

0

vJ

J

J

SRRRR

RRRRIRR

T

nn

T

nn

T

nn

T

nn

T

nn

0000

000000

..

0...

# Compute r0j

> for j to m do

> r0 || j := T || j[[1 .. 3], 4]

> end do;

# Compute rjm

> for j to m do

> r || j || m := r0 || m-r0 || j

> end do;

# Compute zj

> z := Vector([0, 0, 1]);

> for j to m do

> z || j := T || j[[1 .. 3], [1 .. 3]].z

> end do;

# Compute Jvj and Jwj for each joint

> for j to m-1 do

> if joints(j) = R then

> Jv || j := `&x`(z || j, r || j || m);

> Jw || j := z || j

> elif joints(j) = P then

> Jv || j := z || j;

> Jw || j := Vector([0, 0, 0])

> end if;

> J || j := collect(Vector([Jv || j, Jw || j]), {c, s})

> end do;

# Assemble J from Jv and Jw

> Jdummy := Matrix([J || 1]);

> for j from 2 to m-1 do

> J := Matrix([Jdummy, J || j]);

> Jdummy := J

> end do;

The Jacobian – via Propagation

JJv ,

J

0

j

mjr0

mr0

jmr

jmjm rrr 00

0

0

0

0

j

j

j

j

J

zJ

zJ

rzJ

jv

j

jmjv

Revolute Joint

Prismatic Joint

j

j

jj zRz 00

The Jacobian – Stanford Arm

J :=

( ) s1 c2 c3 s1 s2 s3 a3 ( )s1 c2 s3 s1 s2 c3 d4 s1 c2 a2 c1 d3[ ,

( ) c1 c2 s3 c1 s2 c3 a3 ( )c1 s2 s3 c1 c2 c3 d4 c1 s2 a2 ( ) c1 c2 s3 c1 s2 c3 a3 ( )c1 s2 s3 c1 c2 c3 d4 0 0 0, , , , ]

( )c1 c2 c3 c1 s2 s3 a3 ( ) c1 c2 s3 c1 s2 c3 d4 c1 c2 a2 s1 d3[ ,

( ) s1 c2 s3 s1 s2 c3 a3 ( ) s1 c2 c3 s1 s2 s3 d4 s1 s2 a2 ( ) s1 c2 s3 s1 s2 c3 a3 ( ) s1 c2 c3 s1 s2 s3 d4 0 0 0, , , , ]

[ , , , , , ]0 ( )s2 s3 c2 c3 a3 ( )c2 s3 s2 c3 d4 c2 a2 ( )s2 s3 c2 c3 a3 ( )c2 s3 s2 c3 d4 0 0 0

0 s1 s1 c1 ( )c2 s3 s2 c3 s4 c1 c2 c3 s4 c1 s2 s3 s1 c4[ , , , , ,

s5 s1 s4 c5 c1 s2 c3 c5 c1 c2 s3 c1 c4 s5 c2 c3 c1 c4 s5 s2 s3]

0 c1 c1 s1 ( )c2 s3 s2 c3 s4 s1 c2 c3 s4 s1 s2 s3 c1 c4[ , , , , ,

s1 c4 s5 c2 c3 s1 c4 s5 s2 s3 s5 c1 s4 s1 c5 s2 c3 s1 c5 c2 s3]

[ , , , , , ]1 0 0 s2 s3 c2 c3 s4 ( )c2 s3 s2 c3 c4 s5 c2 s3 c4 s5 s2 c3 c5 c2 c3 c5 s2 s3

The Acceleration

qJqq

JqJ

dt

d

dt

dva

2

Coriolis Centrifugal Inertia Direct Computation

Coriolis

Centrifugal

Inertia

Propagation

# Compute Jdot=DJ.qdot.

# Jacobian of J (DJ) w.r.t q (the d.o.f)

> with(VectorCalculus);

> L := [seq(q || i, i = 1 .. m-1)];

> PDEtools[declare](L(t), prime = t);

> for j to m-1 do

> Jdot || j := Jacobian(J || j, L).

(diff(convert(L(t), Vector), t))

> end do;

> DJdummy := Matrix([Jdot || 1]);

> for j from 2 to m-1 do

> Jdot := Matrix([DJdummy, Jdot || j]);

> DJdummy := Jdot end do;

# Compute Jdot.qdot

> DJqdot := factor(simplify(Jdot.

(diff(convert(L(t), Vector), t))));

# Compute J.q2dot

> Jq2dot := factor(simplify(J.

(diff(convert(L(t), Vector), `$`(t, 2)))));

# Compute the acceleration

> Acc := DJqdot+Jq2dot;

# Extract the linear and angular

acceleration

> vdot := Acc[1 .. 3];

> wdot := Acc[4 .. 6];

Spatial Motion of Rigid Bodies Denavit – Hartenberg Representation Forward Kinematics Joint Velocities & Accelerations The Jacobian Computations and Algorithms

Part 1 : Summary

(Part 2: Wheeled Mobile Robots)


Content of this Talk : Part 2

Instantaneously Coincident Coordinate

Muir and Neuman (1986), “Kinematics of Wheeled Mobile Robots”, CMU Tech. Report CMU-RI-TR-1986-12

The wheel rotates with velocity v and acceleration a, with the frame R attached to its center r whose path is indicated. At some instant t, point r becomes coincident with the point r and frame R coincides with R. At this instant, R is instantaneously coincident with R. r has a velocity v and an acceleration a with respect to r but is stationary with respect to the reference frame F. R is ICC to R at time t.

R

r

F

RR,

The Sheth - Uicker Representation


Floor-Wheel Contact Point

z

x

y

z

x

y

0xv

CC,

F

Frame F is fixed to the floor. Frame C is attached to the contact point, is a moving frame. The frame C is instantaneously coincident coordinate frame (ICC) to C. C’ is stationary w.r. to F – thus at every instant, C’ has a new position and orientation, yet remains stationary w.r. to F.

Floor-Robot

x

y

RR,

x

y

F

Frame F is fixed to the floor. Frame R is attached to the robot, is a moving frame. The frame R’ is instantaneously coincident coordinate frame (ICC) to R. R’ is stationary w.r. to F – thus at every instant, R’ has a new position and orientation, yet remains stationary w.r. to F.

R

F


Floor-Robot

x y

F

Rotation about z axis of the floor frame F and x axis of the robot instantaneously coincident frame R’. The transformation is constant since F and R’ are fixed relative to each other

R

F

z RR,

R

F d

1000

100

0)()(

0)()(

xR

F

xR

F

iR

F

iR

F

xR

F

iR

F

R

F

R

F

d

dcs

dsc

T

The Homogenous Transform

WMR Coordinate System



', RRF

',CCH

S

F

'R

R

H

S

C

'CTF

R '

'R

R

H

S

'C

C

TR

H

TS

C

TF

C '

WMR Coordinate Transformations

F

C

C

C

F

C

S

C

H

S

R

H

R

R

F

R TTTT '

'

'

' .....

ijT = Constant Transformations

ijF = Variable Transformations



ijT = Constant Transformations

ijF = Variable Transformations

WMR Coordinate System

F

RR ,

SH , CC,

1 2

3 4

WMR Coordinate Transformations

F

R

R

H

S

C

C

1 2 3 4


z

y

x

z

y

x CC,

F

R

R

CRR,

C

11 ...

...

TTT

TTT

C

R

C

C

C

F

R

F

R

R

C

C

C

F

C

R

R

R

R

F

TF

R

R

R

TR

C

TF

C

C

C

Monocycle Coordinate System

rr CC ,ll CC ,

z

x

z

x

z

y

x

RR,

F

R

R

rC

rC

TF

Cl

TF

R

TR

ClTR

Cr

lC

lC

TF

Cr

l

l

C

C r

r

C

CR

R

11 ...

...

TTT

TTT

C

R

C

C

C

F

R

F

R

R

C

C

C

F

C

R

R

R

R

F

2-Wheel Coordinate System


1

11111111

.....

..........

.....

C

C

S

C

H

S

R

H

R

R

F

R

F

C

R

H

H

S

S

C

C

C

F

C

F

R

R

H

H

S

S

C

C

C

F

C

F

R

R

R

C

C

F

C

S

C

H

S

R

H

R

R

F

R

TTTT

TTTTTTTT

TTTT

F

R

R

C

TF

R

TF

C

H

S

C

C

R

R

RR,S

HCC,

H

S

C

TR

H

TS

C

F

Steering Coordinate System

1R

2R

3R

F

F

1R

2R

3R

Truck - Trailer Coordinate System


3-WMR Coordinate System

R

Floor

Robot Hip

Steering

Wheel

Kinematics of the Wheeled


R

RC

R

C

C

C

F

R

F

R

R

C

C

C

F

C

R

R

R

R

F

TTT

TTT

11 ...

...

ijT = Constant Transformations (fixed in time)

ijF = Variable Transformations (moving with time)

z

F

R

R

C

y

x

z

y

x CC,

RR,

C

TF

R

R

R

TR

C

TF

C

C

C

11 .. TTT R

C

C

C

F

C

F

R

R

R ?

1.... C

C

R

C

R

R

F

R

F

CTTT

11 .... TT R

C

C

C

C

C

R

C

R

R

R

R

Kinematics of the Wheeled – Time Derivatives


Coriolis Inertia

First Derivatives Second Derivatives

The Wheel Jacobian


ii

RR

R

RR

RR

RRR

RR

R qJrrrrr ....

0

R

Floor

Robot Hip

Steering

Wheel

R

R r

iq

TR

R

yR

R

xR

R

R

R vvr

.

3

2

1

3

2

1

00

00

00

q

q

q

J

J

J

r

I

I

I

R

R

WMR Jacobian

1

......

......

......

qrR

R

1J Wheel Jacobian

3x4

The Wheel Jacobian - Computation # Compute the first time derivatives of the variable

transforms.

# Compute the first time derivatives for RcC

dP||Rc||R:=

subs({thetaRR(t)=theta_R,diff(thetaRR(t),t)=omega_R,diff(r

RRz(t),t)=0},map(diff,P_||Rc||R,t));

dP_||Rc||R:=eval(%,theta_R=0);

# Compute the first time derivaatives for CcCforall wheels

for i to m do

dP||Cc||C||i:=

subs({theta||i||CC(t)=theta_C||i,diff(theta||i||CC(t),t)=omega_

C||i,diff(r||i||CCz(t),t)=0},map(diff,P_||Cc||C||i,t)):

dP_||Cc||C||i:=eval(%,theta_C||i=0);

end do;

# Compute the second time derivaatives of the variable

transforms

d2P||Rc||R:=

subs({thetaRR(t)=theta_R,diff(thetaRR(t),t)=omega_R,diff(th

etaRR(t),t$2)=alpha_R,diff(rRRz(t),t)=0},map(diff,P_||Rc||R,t

$2)) :

d2P_||Rc||R:=eval(%,theta_R=0);

for i to m do

d2P||Cc||C||i:=

subs({theta||i||CC(t)=theta_C||i,diff(theta||i||CC(t),t)=omega_

C||i,diff(theta||i||CC(t),t$2)=alpha_C||i,diff(r||i||CCz(t),t)=0},m

ap(diff,P_||Cc||C||i,t$2)):

d2P_||Cc||C||i:=eval(%,theta_C||i=0);

end do;

# Compute the Jacobian

v:=([vx,vy,omega]);

J0:=expand(P_||Rc||R.T_||R||C||1.MatrixInverse(P_||Cc||C||1

).dP_||Cc||C||1).MatrixInverse(T_||R||C||1):

J1:=subs({omega_C1=omega,diff(r1CCx(t),t)=vx,diff(r1CCy(

t),t)=vy},J0):

J2:=map(combine,simplify(expand(J1),trig));

restart:with(LinearAlgebra):m:=1:

# Enter parameters for F and Rc frames

FRc:=[theta||FRC,r||FRcx,r||FRcy,r||FRcz]:

# Enter parameters for Rc and R frames

RcR:=[theta||RR(t),r||RRx(t),r||RRy(t),r||RRz(t)]:

PDEtools[declare]({theta||RR(t),r||RRx(t),r||RRy(t),r||RRz(t)

}, prime = t):

# Enter parameters for frames RC for all the wheels

w||1||RC:=[theta||1||RC,r||1||RCx,r||1||RCy,r||1||RCz]:

# Enter parameters for frames CcC for all the wheels

w||1||CcC:=[theta||1||CC(t),r||1||CCx(t),r||1||CCy(t),r||1||CCz

(t)]:

PDEtools[declare]({theta||1||CC(t),r||1||CCx(t),r||1||CCy(t),r|

|1||CCz(t)}, prime = t):

# Enter parameters for frames FCc for all the wheels

w||1||FCc:=[theta||1||FCc,r||1||FCcx,r||1||FCcy,r||1||FCcz]:

# Compute Transformation Matrices

P:=(theta,x,y,z) ->Matrix(4,4,[

[cos(theta),-sin(theta),0,x],

[sin(theta),cos(theta),0,y],

[0,0,1,z],

[0,0,0,1]]):

# Transformation Matrices for F||Rc and Rc||R. These are

computed once

T_||F||Rc:=P(seq(FRc[i],i=1..4)):

P_||Rc||R:=P(seq(RcR[i],i=1..4)):

# Now compute the rest of the transforms

for i to m do

T_||R||C||(i):=P(seq(w||i||RC[j],j=1..4)):

P_||Cc||C||(i):=P(seq(w||i||CcC[j],j=1..4)):

T_||F||Cc||(i):=P(seq(w||i||FCc[j],j=1..4)):

end do;

Whe

el F

ram

e P

aram

eter

s T

rans

form

atio

ns

1st D

eriv

ativ

es

2nd D

eriv

ativ

es

Whe

el

Jac

obia

n

Base

v

qJv

Forward Propagation Given joint velocities – compute the End-Effector velocity.

F

FJ T

Back Propagation Given End-Effector forces– compute joint torques.

The Velocity – Torque Duality F

3

2

1

Tzyxzyx FFFF

Applied Force Applied Torque

i

ii xfw .Virtual

work

Applied force

Virtual Displacement

qJFxFq TTT .

FJ T

Dynamics - Newton versus Euler

).( vmdt

dF ).( I

dt

d

.. II

maF

General Motion

Pure Translation

Pure Rotation

General form of EOM

)(),()( qGqqVqqM Coriolis

Centrifugal Inertia Gravity

Torque (Control)

Robot Dynamics - Recursive Newton-Euler Fa

Robot Control – via the Jacobian xJqqJx 1

(J non-singular)

xxx d

Control 1

Forward Kinematics

Joint1

Control 2 Joint2

Control 3 Joint3 3

2

1

q

q

q

1J--

3

2

1

q

q

q

x

x

dx

q

q q

)(qfx

Robot Control = via Over-Writing the Dynamics

)(),()( qGqqVqMu

Control Law

qModified Dynamics

1 q

Unit mass

)(),()( qGqqVqqM

qkqkq dp

qkqkq pd

PD Controller


Part 2 : Summary

Blatantly Avoided

Inverse Kinematics – does the solution exist? … multiple solutions … which one to choose

Kinematic Singularities – J non-invertible … how to locate, avoid and negotiate.

Presence of friction – how to include … how to model and later compensate for.

Force control … hybrid control (position + force)

Legged Creatures … swing, step and propel.

Computation of inertia.

Without W

afe

r W

ith W

afe

r

0 1 20

0.2

0.4

0.6

0.8

1

1.2

1.4

Time (s)

x(t

)

Step Response

Rise Time = 0.043217

Settling Time =0.48267

Over Shoot (%) =33.9612

Data

Model

0 20 40 600

0.05

0.1

0.15

0.2

0.25

(Hz)|X

()|

Step Response FFT

-100

-50

0

Mag

nitud

e (

dB

)

100

102

104

-270

-180

-90

0

90

180

270

360

Pha

se

(d

eg

)

Bode Diagram

Frequency (Hz)

0 0.5 1 1.5 20

0.2

0.4

0.6

0.8

1

1.2

1.4

Time (s)

x(t

)

Step Response

Rise Time = 0.031458

Settling Time = 0.64119

Over Shoot (%) = 29.0969

0 10 20 30 40 500

0.02

0.04

0.06

0.08

0.1

0.12

(Hz)

|X(

)|

Step Response FFT

Data

Model

-150

-100

-50

0

50

Mag

nitud

e (

dB

)10

010

210

4-270

-225

-180

-135

-90

-45

0

Pha

se

(d

eg

)

Bode Diagram

Frequency (Hz)

Robot Dynamics – Step Response

Mechanism Friction Measurement & Model

m F

f

𝑓(𝑡) = g(1)(tanh(g 2 . v t − tanh g 3 . v t + g 4 tanh g 5 . v t + g 6 . v(t)

Striebeck Friction Coulomb Friction Viscous Friction


Upper EE

Lower EE Striebeck Friction

Coulomb Friction

Viscous Friction


0 1000 2000 3000 4000 5000-2

0

2

Time (s)

N

Friction force

0 500 1000 1500 2000 2500 3000 3500 4000 4500-50

0

50Slip speed

Time (s)

mm

/s

0 1000 2000 3000 4000

-1

-0.5

0

0.5

1

1.5Friction force. (sim)

Time (s)

Fri

cti

on

fo

rce

(N

)

zv; measured

nlgr; fit: 64.27%

-100 -50 0 50 100-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

Velocity (mm/s)

Fri

cti

on

(N

)

Friction v Velocity

0 500 1000 1500 2000 2500 3000 3500 4000 4500-6

-4

-2

0

2

4

Time (s)

N

Friction force

0 500 1000 1500 2000 2500 3000 3500 4000 4500-50

0

50

Slip speed

Time (s)

mm

/s

0 500 1000 1500 2000 2500 3000 3500 4000

-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

Friction force. (sim)

Time (s)

Fri

cti

on

fo

rce

(N

)

zv; measured

nlgr; fit: 79.56%

-100 -50 0 50 100-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

Velocity (mm/s)

Fri

cti

on

(N

)

Friction v Velocity

Before PM @ 5M Cycles

After PM

Hysteresis

Rocking of a Rigid Cylinder on Moving Foundation

What is a “Reliable” Robot?

It is the probability (R) that the robot will successfully complete the assigned task (T) under the specified conditions (C).

Specified Conditions (C): Martian Terrain / Contact with Human Body / Assembly Line Assigned Task (T): Move from A to B / Perform Surgery / Spot Weld. Probability (R). On Mars, move from A to B 50 times without failure. On a human perform surgery with failure. On an assembly line do 1 million spot welds before failure. The basic problem is to quantify R during design.

Pain

ting T

itle

: M

any

a M

oon A

go, G

. M

usta

fa ©

2011

Fin

Date post:	13-Aug-2015
Category:	Documents
Upload:	ghulam-mustafa-ph-d
View:	46 times
Download:	0 times

IEEE Robotics Talk

Documents