L ECTURE 2-3: R OBOT P OSE - R EPRESENTATION AND TRANSFORMATION I - - PowerPoint PPT Presentation

16-311-Q INTRODUCTION TO ROBOTICS

LECTURE 2-3: ROBOT POSE - REPRESENTATION

AND TRANSFORMATION

INSTRUCTOR: GIANNI A. DI CARO

2

N E E D E D : P O S I T I O N A N D O R I E N TAT I O N

• A robot is embedded in a physical environment, its actions and their effects

strictly depend on where the robot is precisely located in the environment in terms of position + orientation

• Position and orientation with respect to which reference?
• Position and orientation of which part of the robot?

3

R I G I D B O D Y A N D P O I N T A B S T R A C T I O N

Rigid body assumption: The object is rigid, such that its constituent points maintain a constant relative position with respect to each other and to the object’s coordinate frame This holds under any translation and rotation applied to the rigid body Point abstraction can be applied (note: does not apply to soft or humanoid robots)

P

A robot of any single-body shape can be “reduced” to a point, selected as reference point A multi-body robot, with n moving parts (not rigidly connected) can be “reduced” to n points, selected as reference points Configuration of the object/robot = Complete specification of the location

• f every point: It can be computed from a single reference point

4

R O B O T P O S E

Define a fixed world reference coordinate frame {W} Center a (local) coordinate frame {R} in the robot’s reference point P, (possibly) oriented according to robot’s natural orientation A point in space ( = chosen reference point P) is described by a coordinate vector r representing the displacement of the point with respect to the reference coordinate frame {W} (e.g., using Polar or Cartesian coordinates) The pose/configuration of the object/robot in {W} is described by the position and orientation

• f the (local) coordinate frame {R} wrt {W}

xp yp

P

θ

r x y

P

{R}

xR yR

𝝄R

𝝄R is the relative pose of the frame/robot

with respect to the reference coordinate frame {W}

5

M O B I L E R O B O T C O N F I G U R AT I O N / P O S E

In 3D the pose / configuration (of an object) is a a 6-dimensional vector:

N O N O M N I D I R E C T I O N A L

x

q=(x,y,θ)

y

θ

The configuration of a non-omnidirectional mobile robot in a two-dimensional coordinate systems is fully defined by:

• 1. position (x,y)
• 2. orientation angle θ, with respect to the

coordinate axes The triple q=(x,y,θ) is the robot pose What about an omnidirectional mobile robot?

G E N E R A L I Z E D C O O R D I N AT E S , D O F

Generalized coordinates: The n parameters q= (q1, q2, …, qn), that uniquely describe the configuration of the system relative to some reference configuration/system. Generalized velocities: The time derivatives of generalized coordinates. State of the system: Described by (generalized coordinates, generalized velocities)

q=(q1, q2,…,qn)

q1 q2 q3 qn

Degrees of freedom: A system whose configuration is described by n independent generalized coordinates has n degrees of freedom. If there are m functional relations among the generalized coordinates, the number of DoF becomes n-m

• The state of the system can be changed operating
• n n independent variables
• To reach any point in a 3D embedding workspace

a robot must have “at least” 6 degrees of freedom

7

R O B O T C O N F I G U R AT I O N / P O S E F O R A R M S

Multiple translational and rotational DOFs, each q is either an angle or a distance

Prismatic joint (1 DOF) Revolute joint (1 DOF)

~ q = (q1, q2, . . . , q10)

The configuration of a robot arm is defined by its joints and their characteristics

• Pose of the manipulator: Configuration of all joints (n DoF for n revolute joints)
• Pose of the end-effector: 6-DoF

, can be obtained by the pose of manipulator (reverse doesn’t hold! multiple manipulator poses can generate the same ee-pose)

8

W O R K S PA C E A N D C O N F I G U R AT I O N S PA C E

Workspace: A robot’s workspace (or workspace envelope) is the set

• f all points the robot can reach in the physical embedding volume.

Configuration space (C-space): the set of all possible robot configurations based on robot’s structure and environmental constraints. If the robot has n DOFs, then the configuration space is n-dimensional (usually a non-Cartesian manifold).

R2

R2 × S1

9

W O R K S PA C E A N D C O N F I G U R AT I O N S PA C E

R2

R2 × S1

q = (x,y) q = (θ1,θ2) q = (x,y,θ) q = (x,y,z, ɸ,θ,Ψ) q = (θ) q = (s) q = (s,θ)

10

C O N F I G U R AT I O N S PA C E I N P R E S E N C E O F E X T E R N A L O B S TA C L E S

C-space is obtained wrt the reference point by sliding the robot along the edge of the obstacle regions "blowing them up" by the robot radius One reference point is selected:

11

C - S PA C E A N D P O I N T R E D U C T I O N

Special case: The robot is a polygonal one and can only translate For motion planning and navigation a mobile robot of any shape can be “reduced” to a point, precisely the chosen reference point as long as all reasonings are done in the C-space

12

TA S K S PA C E

• While the pose is naturally expressed as vector of generalized coordinates (in the

configuration space), we might be also interested to express the pose in a task- depending embedding space

• Task space 𝓤: Set of possible poses in the task-defined space
• Example: A robot train, constrained to move on a rail from a start point
• Task is motion along the rail: 𝓤⊂ ℝ, 𝓤 ≡ C
• Task asks about the position of the train in a

plane: 𝓤⊂ ℝ2

• Task requires 3D robot train position and
• rientation: 𝓤⊂ SE(3)
• In the last last two cases the dimension of

the task space exceeds the dimension of the configuration space: robot moves on a manifold in the (higher dimensional) task space, there is a mapping from q to 𝝄 ∈ 𝓤 z

13

C O M P U T I N G A ( R E L AT I V E ) P O S E I S A C O R E P R O B L E M Common questions in mobile robotics:

What is robot’s pose with respect to the world reference frame {W}? What is robot’s pose with respect to the external frame {H}? What is intruder’s pose, observed using robot’s lateral camera, in the world frame {W}? {W} {R} {W} {H} {R} {W} {R} {I} What is robot’s pose in {W} after moving at a velocity v for 1 minute? {W} ? What is the velocity profile that allows to reach a pose 𝝄 in {W}?

14

L E T ’ S G O B A C K T O P O S E …

Define a fixed world reference coordinate frame {W} Center a (local) coordinate frame {R} in the robot’s reference point P, (possibly) oriented according to robot’s natural orientation A point in space ( = chosen reference point P) is described by a coordinate vector r representing the displacement of the point with respect to the reference coordinate frame {W} (e.g., using Polar or Cartesian coordinates) The pose/configuration of the object/robot in {W} is described by the position and orientation

• f the (local) coordinate frame {R} wrt {W}

xp yp

P

θ

r x y

P

{R}

xR yR

𝝄R

𝝄R is the relative pose of the frame/robot

with respect to the reference coordinate frame {W}

15

U S E A N D P R O P E RT I E S O F R E L AT I V E P O S E S

The relative pose W𝝄R describes the frame {R} with respect to the frame {W} The leading superscript denotes the reference coordinate frame, the subscript denotes the frame being described If the initial superscript is missing the reference frame is a world coordinate frame.

𝝄 is the object being described

W𝝄R could be seen as describing some motion: first applying a displacement and then a rotation to {W}

xw

P

{R}

xR yR

W𝝄R

{W}

yw

WP RP

The operator · transforms the vector, resulting in a new vector that describes the same point but with respect to a different coordinate frame. The point P can be described with respect to either coordinate frame:

WP = W𝝄R . RP

the right-hand side expresses the motion {W} {R} P

16

C O M P O S I T I O N O F R E L AT I V E P O S E S

One frame can be described in terms of another by a relative pose ➔ The procedure can be applied sequentially by a relative pose composition operator ⊕ Pose({C} relative to {A}) = Pose({B} relative to {A}) ⊕ Pose({C} relative to {B})

A𝝄C = A𝝄B ⊕ B𝝄C ➔ AP = (A𝝄B ⊕ B𝝄C). CP = A𝝄C . CP A relative pose can transform a point expressed as a vector relative to one frame to a vector relative to another AP = A𝝄B . BP

17

P O S E S F O R M A N A D D I T I V E G R O U P

The set of poses equipped with the combination operator ⊕ form an additive group. In the case of a mobile robot this is the SE(2) Special Euclidean group (SE(3) in 3D) A𝝄B ⊕ B𝝄C = A𝝄C Closure:

𝝄1 ⊕ 𝝄2 = 𝝄3

Composition of two poses results in a new pose A𝝄B ⊕ A𝝄C = B𝝄C Associativity:

(A𝝄B ⊕ B𝝄C) ⊕ C𝝄D = A𝝄B ⊕ (B𝝄C ⊕ C𝝄D) 𝝄 ⊕ 0 = 0 ⊕ 𝝄 = 𝝄

0 is the null relative pose Identity element:

𝝄 ⊖ 0 = 𝝄 𝝄 ⊖ 𝝄 = 0 (⊖𝝄) ⊕ 𝝄 = 𝝄 ⊕ (⊖𝝄) = 0 ⊖ A𝝄B = B𝝄A

Inverse: Composition is NOT commutative: (because of the angle part of poses)

𝝄1 ⊕ 𝝄2 ≠ 𝝄2 ⊕ 𝝄1

18

A M O R E C O M P L E X 3 D E X A M P L E

B

Poses composition allows to represent useful spatial relations … (O is dropped for short)

𝝄F ⊕ F𝝄B = 𝝄R ⊕ R𝝄C ⊕ C𝝄B 𝝄F ⊕ F𝝄R = 𝝄R

… a perform computations (adding the inverse to both sides to 2nd eq.)

⊖𝝄F ⊕ 𝝄F ⊕ F𝝄R = ⊖𝝄F ⊕𝝄R

F𝝄R = ⊖𝝄F ⊕𝝄R

Pose of the robot relative to the fixed camera

F𝝄R = F𝝄O ⊕ 𝝄R

19

A M O R E C O M P L E X 3 D E X A M P L E : G R A P H D E S C R I P T I O N

B

In the graph, each node represents a pose, and each arc is a relative pose

𝝄F ⊕ F𝝄B = 𝝄R ⊕ R𝝄C ⊕ C𝝄B 𝝄F ⊕ F𝝄R = 𝝄R

A spatial relation is a loop in the graph Both sides of an equation start and end at the same node.

20

P O S E S I N 2 D ( F O R M O B I L E R O B O T S )

What are in practice ξ and ⊕? Any mathematical objects/operators that support the described group properties (algebra) and are suited to the problem at hand. In 2D, for a wheeled robot, a concrete representation of a pose is in a Cartesian coordinate system through the (x,y) coordinates and the θ angle for the orientation

xF

{R}

xR yR

F𝝄R

{F}

yF

θ

y x

t To describe a coordinate frame {R} wrt a reference frame {F} it can be noticed that:

• 1. Origin of {R} has been displaced by

the vector t = (x,y) ➔ Translation

• 2. {R} has been rotated counter-

clockwise by an angle θ ➔ Rotation

F𝝄R ~ (x, y, θ)

~ stands for equivalency

Unfortunately this representation is not really convenient for compounding since

(x1, y1, θ1) ⊕ (x2, y2, θ2) would require some complex trigonometric functions

21

U S E O F M AT R I X O P E R AT O R S F O R R E P R E S E N T I N G P O S E S

Let’s consider an arbitrary point P with respect to each of the coordinate frames {F} and {R} and see how the relationship between FP and RP can be (effectively) determined as rotation ⊕ translation Let {V} be a new coordinated frame, centered in {R} but rotated as {F}

xF

P

{R}

xR yR

F𝝄R

{F}

yF

θ

y x

t

FP RP

P

{R}

xR yR xv

{V}

yv

θ

sin(θ) cos(θ)

• sin(θ)

cos(θ)

Rotation

xF

P

{R}

xR yR

{F}

yF y x

t Translation

vy vx

Rx Ry

22

F R A M E R O TAT I O N

xF

P

{R}

xR yR

F𝝄R

{F}

yF

θ

y x

t

FP RP

The new frame {V} has axes parallel to {F} but origin is the same as {R} Coordinate frame {R} is fully described by its orthogonal axes, whose unit vectors can be expressed in terms of {V}’s unit vectors: ⇥ ˆ xR ˆ yR ⇤ = ⇥ ˆ xv ˆ yv ⇤ "cos(θ) − sin(θ) sin(θ) cos(θ) # ˆ xR = cos(θ)ˆ xv + sin(θ)ˆ yv ˆ yR = − sin(θ)ˆ xv + cos(θ)ˆ yv

P

{R}

xR yR xv

{V}

yv

θ

sin(θ) cos(θ)

• sin(θ)

cos(θ)

Rotation

Rx

Ry

vx vy

Point P in {V}:

vP =vx ˆ

xv +vyˆ yv = ⇥ˆ xv ˆ yv ⇤ vx

vy

23

F R A M E R O TAT I O N

⇥ ˆ xR ˆ yR ⇤ = ⇥ ˆ xv ˆ yv ⇤ "cos(θ) − sin(θ) sin(θ) cos(θ) # Substituting …

RP =

⇥ˆ xv ˆ yv ⇤ "cos(θ) − sin(θ) sin(θ) cos(θ) # "Rx

Ry

# "vx

vy

# = "cos(θ) − sin(θ) sin(θ) cos(θ) # "Rx

Ry

# How points are transformed (from {R} to {V}) when the frame is rotated:

"vx

vy

# = vRR(θ) "Rx

Ry

#

Rotation matrices

P

{R}

xR yR xv

{V}

yv

θ

Ry

vy

sin(θ) cos(θ)

• sin(θ)

cos(θ)

Rx

vx

vP =vx ˆ

xv +vyˆ yv = ⇥ ˆ xv ˆ yv ⇤ vx

vy

• RP =Rx ˆ

xR +Ryˆ yR = ⇥ ˆ xR ˆ yR ⇤ Rx

Ry

• "Rx

Ry

# = vR−1

R (θ)

"vx

vy

#

24

R O TAT I O N M AT R I X

R(θ) = "cos(θ) − sin(θ) sin(θ) cos(θ) #

Instead of using one scalar, the angle θ, we are using a 2x2 matrix to represent orientation The rotation matrix is orthonormal: each of its columns is a unit vector and the columns are orthogonal, that is, represent an orthonormal basis (the columns are the unit vectors that define {R} with respect to {V}) → 4 parameters, 3 functional relations/constraints →

• ne independent value (the angle!)

The determinant is +1: R belongs to the special orthogonal group of dimension 2, SO(2), acting as an isometry ➔ the length of a vector is unchanged after a rotation The inverse is the same as the transpose: R-1 = RT

Inverting the matrix is the same as swapping superscript and subscript, which leads to the

identity R(-θ) = RT(θ) = R-1(θ) Has nice properties …

R R R

R R

25

R O TAT I O N M AT R I X

Rz(𝜾)

26

A D D T H E T R A N S L AT I O N

"vx

vy

# = "cos(θ) − sin(θ) sin(θ) cos(θ) # "Rx

Ry

#

{F} and {V} have parallel coordinate axes:

xF

P

{R}

xv yv

{F}

yF y x

t

xv

{V}

yv

θ

vy vx Fx Fy

vP was obtained from the rotation:

In a more compact form: "

Fx Fy

# = " cos(θ) − sin(θ) x sin(θ) cos(θ) y # 2 6 6 4

Rx Ry

1 3 7 7 5

F P =

"Fx

Fy

# = "cos(θ) − sin(θ) sin(θ) cos(θ) # "Rx

Ry

# | {z }

Rotation

+ " x y # |{z}

T ranslation F P =

"Fx

Fy

# = "V x

V y

# + " x y #

27

H O M O G E N E O U S T R A N S F O R M AT I O N M AT R I X

"

Fx Fy

# = " cos(θ) − sin(θ) x sin(θ) cos(θ) y # 2 6 6 4

Rx Ry

1 3 7 7 5 Homogenous 2D vector! (allow to represent a translation by a matrix product) Coordinate vectors for P are now in 2D homogenous form: homogenous coordinate transformation

2 6 6 4

Fx Fy

1 3 7 7 5 = "

FRR

t 01×2 1 # 2 6 6 4

Rx Ry

1 3 7 7 5

F ˜

P = "

FRR

t 01×2 1 #

R ˜

P

F ˜

P =

FTR R ˜

P

The roto-translation matrix T is a homogenous transformation and belongs to SE(2)

FTR =

    cos(θ) − sin(θ) x sin(θ) cos(θ) y 1    

The displacement of a robot / rigid body is fully described by a homogeneous transformation matrix

28

H O M O G E N E O U S T R A N S F O R M AT I O N S I N 2 D A N D 3 D

AP = A𝝄B . BP From relative pose transformation

• f a coordinate point:

F𝝄R ~

F TR ∈ SE(2)

⊕ = Matrix product: A𝝄R ⊕ R𝝄B ≡ ATR RTB

. = Matrix-vector product: AP = A𝝄B . BP ≡ ATB . BP

I = Identity element: TI = I

⊖T

Equal to T-1

SE() Group properties 2D

F ˜

P = "

FR2×2 R

t2×1 01×2 1 #

R ˜

P W𝝄 = [ x y θ ] 3D

F ˜

P = "

FR3×3 R

t3×1 01×3 1 #

R ˜

P W𝝄 = [ x y z ɸ θ 𝞈 ]

29

P O S E T R A N S F O R M AT I O N E X A M P L E , N O C O M M U TAT I V I T Y

𝝄1 𝝄2 𝝄1 = 𝝄0 ⊕ (1,2,30o) 𝝄2 = 𝝄0 ⊕ (2,1,0o) ? 𝝄3 = 𝝄1 ⊕ 𝝄2 𝝄0 ~ (0,0,0) ? 𝝄4 = 𝝄2 ⊕ 𝝄1

T2 =     1 2 1 1 1    

T1 =     0.866 −0.5 1 0.5 0.866 2 1    

30

P O S E T R A N S F O R M AT I O N E X A M P L E , N O C O M M U TAT I V I T Y

𝝄1 𝝄2 𝝄3 = 𝝄1 ⊕ 𝝄2

• 𝝄2 represents a transformation

T2 consisting of: a translation of 2 long x, 1 long y, plus a ccw rotation of 0 degrees

• 𝝄3 = T1 T2 meaning that T2 is

applied wrt T1 : the origin of {3} has coordinates (2,1) in {1} and no further rotation is applied to the coordinated axes

• 𝝄4 = T2 T1 meaning that T1 is

applied wrt T2 : the origin of {4} has coordinates (1,2) in {2} and is rotated of 30 degrees wrt {2}’s coordinate axes

31

O R I E N TAT I O N O F A R I G I D B O D Y I N 3 D : E U L E R A N G L E S

Any target orientation can be reached by composing three elemental rotations around the three coordinate axes

32

3 D A N G L E S

https://www.youtube.com/watch?v=ImPBVQJRSwY

33

H O M O G E N E O U S T R A N S F O R M AT I O N S I N 3 D

Rotations in 3D - SO(3) cx := cos(x), sx := sin(x)

T =

34

N O W W E C A N D E A L W I T H T H E S E P R O B L E M S … .

Common questions in mobile robotics:

What is robot’s pose with respect to the world reference frame {W}? What is robot’s pose with respect to the external frame {H}? What is intruder’s pose, observed using robot’s lateral camera, in the world frame {W}? {W} {R} {W} {H} {R} {W} {R} {I} What is robot’s pose in {W} after moving at a velocity v for 1 minute? {W} ? What is the velocity profile that allows to reach a pose 𝝄 in {W}?

35

C A L C U L AT I O N S … .

A point p expressed in the local (robot) coordinate frame is transformed in {W} coordinates A rigid body 3D pose in {W} after applying a homogeneous transformation T

𝝄A = 𝝄W ⊕ (x,y,z,𝜚,𝜄,𝜔), where T is the transformation representing (x,y,z,𝜚,𝜄,𝜔)

𝝄A (T)~

36

R O S H E L P S !

B

37

R O S H E L P S ! ( N E X T T I M E I N P Y T H O N )

38

R O S H E L P S !

39