Robotics

Kinematics

Kinematic map, Jacobian, optimality principle of inverse kinematics, singularities, conguration/operational/null space, multiple simultaneous tasks, special task variables, trajectory interpolation, motion proles

Marc Toussaint FU Berlin

Problem
Move all joints in a coordinated way so that the endeffector makes a desired movement

2/47

3 ingredients
1) When we know/set the joint angles, where is the endeffector? 2) When we change the joint angles, how does the eff. change position? 3) When we want a certain change in eff. position, how do we have to change the joint angles?

3/47

Notation
q Rn q R
n

vector of joint angles (robot conguration) vector of joint angular velocities small step in joint angles some endeffector(s) feature(s) e.g. position R3 or vector R3 kinematic map Rdn Jacobian squared norm of v w.r.t. metric W
q

q Rn y Rd : qy J (q ) = ||v ||2 W = v Wv

4/47

3 ingredients

kinematic map

Jacobian

optimality criteria

1) When we know/set the joint angles q , where is the endeffector y ? 2) When we change the joint angles q , how does the eff. change position y ? 3) When we want a certain change y in eff. position, how do we have to
5/47

Kinematics

A kinematic structure is a graph (usually tree or chain) of rigid links and joints
6/47

Kinematics

B
B'

eff

A
A'

C'

A kinematic structure is a graph (usually tree or chain) of rigid links and joints
6/47

Kinematics

eff

joint transf.

B'

A
A'

C'

relative eff. offset

link transf.

TW e (q ) = TW A TAA (q ) TA B TB B (q ) TB

TC C (q ) TC

7/47

Joint types
link transformations: TW A joint transformations: TAA (q ) depends on q Rn

revolute joint: joint angle q R determines rotation about x-axis: 1 0 (q ) = 0 0

TAA

0 0 0 cos(q ) sin(q ) 0 sin(q ) cos(q ) 0 0 0 1

prismatic joint: offset q R determines translation along x-axis: 1 0 (q ) = 0 0

TAA

0 1 0 0

0 0 1 0

q 0 0 1

others: screw (1dof), cylindrical (2dof), spherical (3dof), universal (2dof)

8/47

9/47

Kinematic Map
For any joint angle vector q Rn we can compute TW e (q ) by forward chaining of transformations TW e (q ) gives us the pose of the endeffector

10/47

Kinematic Map
For any joint angle vector q Rn we can compute TW e (q ) by forward chaining of transformations TW e (q ) gives us the pose of the endeffector Two basic ways to dene a kinematic map : q y are pos (q ) = TW e (q ).translation and
1 vec (q ) = [TW e (q ).rotation] 0 0

R3 R3

10/47

3 ingredients
kinematic map Jacobian

optimality criteria

11/47

Jacobian
2) When we change the joint angles q , how does the eff. change position y ?

Given the kinematic map y = (q ) (q ) ? what is the Jacobian J (q ) = q

J (q ) =

(q ) = q

1 (q ) q1 2 (q ) q1 d (q )

. . .

1 (q ) q2 2 (q ) q2

... ... ...

q1

d (q ) q2

d (q )

1 (q ) qn 2 (q ) qn

. . .

Rdn

qn

12/47

Jacobian
eff
axis point

i-th joint

We consider an innitesimal variation qi of the ith joint and see how the endeffectors position pe = pos (q ) and attached vector ae = vec (q ) change. It must hold pe = Jpos (q )i qi ae = Jvec (q )i qi

13/47

Jacobian
eff
axis point

i-th joint

We consider an innitesimal variation qi of the ith joint and see how the endeffectors position pe = pos (q ) and attached vector ae = vec (q ) change. It must hold pe = Jpos (q )i qi

ae = Jvec (q )i qi

1 ai = [TW i (q ).rot] 0 is rotation axis and pi = [TW i (q ).pos] position of ith joint 0
13/47

Jacobian
eff
axis point

Consider a variation qi the whole sub-tree rotates pe = qi [ai (pe pi )] ae = qi [ai ae ]

i-th joint

Position Jacobian:

Vector Jacobian: R
3n

[a1 (pe p1 )]

[a2 (pe p2 )]

[an (pe pn )]

Jpos (q ) =

Jvec (q ) =

[a1 ae ]

[a2 ae ]

[an ae ]

. . .

R3n

. . .

14/47

Jacobian
To compute the Jacobian of some endeffector position or vector, we only need to know the position and rotation axis of each joint. The two kinematic maps pos and vec are only two basic examples but many more complex geometric features can be computed from these, as we will see later.

15/47

3 ingredients
kinematic map Jacobian

optimality criteria

16/47

Inverse Kinematics problem

3) When we want a certain change y in eff. position, how do we have to change the joint angles q ? The Jacobian gives us: y = J (q ) q Iff the Jacobian were invertible: q = J (q )-1 y but typically is not invertible! (J Rdn with d = n) We formulate an optimality principle to choose q given y related to taking the pseudo-inverse J instead of the undened J -1

17/47

Inverse Kinematics optimality principle

Given current qt and yt = (qt ) given desired y compute qt+1 such that 1) 2) (qt+1 ) is close to y qt+1 is close to qt move eector be lazy

18/47

Inverse Kinematics optimality principle

Given current qt and yt = (qt ) given desired y compute qt+1 such that 1) 2) (qt+1 ) is close to y qt+1 is close to qt move eector be lazy

Formalize this as objective function 2 f (qt+1 ) = ||qt+1 qt ||2 W + ||(qt+1 ) y ||C

18/47

Inverse Kinematics
2 f (qt+1 ) = ||qt+1 qt ||2 W + ||(qt+1 ) y ||C

When using the local linearization (qt+1 ) (qt ) + J (qt+1 qt ), the optimal next joint state qt+1 that minimizes f (qt+1 ) is (see excercises): qt+1 = qt + J (y yt ) q = J y J = (J CJ + W )-1 J C = W -1 J (JW -1 J + C -1 )-1
(last equality: Woodbury identity)

for C and W = I, J = J (JJ )-1 is called pseudo-inverse W generalizes the metric in q -space C regularizes this pseudo-inverse (see later section on singularities)
19/47

3 ingredients
kinematic map Jacobian

optimality criteria

20/47

Iterating inverse kinematics

Assume initial posture q0 . We want to reach a desired endeff position y in T steps:
1: 2: 3: 4: 5: 6: 7: 8: 9: 10: Input: initial state q0 , desired y , methods pos and Jpos Output: trajectory q0:T Set y0 = pos (q0 ) current (old) endeff position for t = 1 : T do y pos (qt-1 ) current endeff position J Jpos (qt-1 ) current endeff Jacobian y y0 + (t/T )(y y0 ) interpolated endeff target qt = qt-1 + J ( y y) new joint positions Command qt to all robot motors and compute all TW i (qt ) end for
[demo: roboticsCourse2 ./x.exe -mode 2 3 4]

21/47

Iterating inverse kinematics

Assume initial posture q0 . We want to reach a desired endeff position y in T steps:
1: 2: 3: 4: 5: 6: 7: 8: 9: 10: Input: initial state q0 , desired y , methods pos and Jpos Output: trajectory q0:T Set y0 = pos (q0 ) current (old) endeff position for t = 1 : T do y pos (qt-1 ) current endeff position J Jpos (qt-1 ) current endeff Jacobian y y0 + (t/T )(y y0 ) interpolated endeff target qt = qt-1 + J ( y y) new joint positions Command qt to all robot motors and compute all TW i (qt ) end for
[demo: roboticsCourse2 ./x.exe -mode 2 3 4]

Why does this not follow the interpolated trajectory y 0:T exactly? What happens if T = 1 and y is far?

21/47

Where are we?

Weve derived the most basic motion generation principles in roboticsinverse kinematicsfrom an understanding of robot geometry & kinematics a basic notion of optimality

22/47

Where are we?

Weve derived the most basic motion generation principles in roboticsinverse kinematicsfrom an understanding of robot geometry & kinematics a basic notion of optimality In the remainder:
inverse kinematics motion rate control Singularity and singularity-robustness Nullspace, task/operational space, joint space Extension to multiple task variables Extension to other types of task variables, collisions Heuristics for simple trajectory generation

22/47

Inverse Kinematics and Motion Rate Control

Some clarication of notions: The notion kinematics describes the mapping : q y , which usually is a many-to-one function. The notion inverse kinematics in the strict sense describes some mapping g : y q such that (g (y )) = y , which usually is non-unique (and non-optimal in our setting). In practice, one often refers to q = J y as inverse kinematics. When iterating q = J y in a control cycle with time step (typically 1 10 msec), then y = y/ and q = q/ and q =J y . Therefore the control cycle effectively controls the endefector velocitythis is why it is called motion rate control.

23/47

Null/task/operational/joint/conguration spaces
The space of all q Rn is called joint/conguration space The space of all y Rd is called task/operational space Usually d < n, which is called redundancy

24/47

Null/task/operational/joint/conguration spaces
The space of all q Rn is called joint/conguration space The space of all y Rd is called task/operational space Usually d < n, which is called redundancy For a desired endeffector state y there exists a whole manifold (assuming is smooth) of joint congurations q : nullspace(y ) = {q | (q ) = y }

Plain q = J y resolves redundancy based on the be lazy criterion. One can also add null space motion: an additional drift h Rn in the nullspace of the task: q = J y + (I J J ) h This corresponds to a cost term ||qt+1 qt h||2 W in f (qt+1 )!
24/47

Singularity
In general: A matrix J singular rank(J ) < d rows of J are linearly dependent dimension of image is < d y = Jq dimensions of y limited Intuition: arm fully stretched

25/47

Singularity
In general: A matrix J singular rank(J ) < d rows of J are linearly dependent dimension of image is < d y = Jq dimensions of y limited Intuition: arm fully stretched Implications: det(JJ ) = 0 pseudo-inverse J (JJ )-1 is ill-dened! inverse kinematics q = J (JJ )-1 y computes innite steps! Singularity robust pseudo inverse J (JJ + I)-1 The term I is called regularization Recall our general solution (for W = I) J = J (JJ + C -1 )-1 is already singularity robust

25/47

Multiple tasks

26/47

Multiple tasks

RightHand position LeftHand position

27/47

Multiple tasks
Assume we have m simultaneous tasks; for each task i we have: a kinematic mapping yi = i (q ) Rdi a current value yi,t = i (qt ) a desired value yi a metric Ci or precision i (related via Ci = i I)

28/47

Multiple tasks
Assume we have m simultaneous tasks; for each task i we have: a kinematic mapping yi = i (q ) Rdi a current value yi,t = i (qt ) a desired value yi a metric Ci or precision i (related via Ci = i I) Each task contributes a term to the objective function f (qt+1 ) = ||qt+1 qt ||2 W
2 + ||1 (qt+1 ) y1 ||C1

2 ||2 (qt+1 ) y2 ||

28/47

Multiple tasks
Assume we have m simultaneous tasks; for each task i we have: a kinematic mapping yi = i (q ) Rdi a current value yi,t = i (qt ) a desired value yi a metric Ci or precision i (related via Ci = i I) Each task contributes a term to the objective function f (qt+1 ) = ||qt+1 qt ||2 W
2 + ||1 (qt+1 ) y1 ||C1

2 ||2 (qt+1 ) y2 ||

+ A bit algebra the optimal joint step is:

m -1 m J Ci (yi yi,t ) i=1

qt+1 = qt +
i=1

J Ci J + W

28/47

Multiple tasks
A much nicer way to write (and code) exactly the same:

29/47

Multiple tasks
A much nicer way to write (and code) exactly the same: f (qt+1 ) = ||qt+1 qt ||2 W + (qt+1 ) (qt+1 )
) M ( (q ) y1 1 1 t+1 2 (2 (qt+1 ) y2 ) with the big task vector (qt+1 ) := . . .

di

where M1 is the Cholesky decomposition C1 = M1 M1 . The optimal joint step now is: qt+1 = qt (J J + W )-1 J (qt ) with J
(q ) q

the big Jacobian.

29/47

Multiple tasks conclusions

We learnt how to puppeteer a robot We can handle many task variables (but specifying their precisions becomes cumbersome...)
RightHand position LeftHand position

What are interesting task variables?

30/47

The following slides will dene 10 different types of task variables. This is meant to give an idea of possibilities and as reference.

31/47

Task Variable 1: Endeffector position

Position of some point attached to link i dimension parameters kin. map Jacobian d=3 link index i, point offset v posi,v (q ) = posi + roti v Jposi,v (q )1:3,k = [k i] ak (posi (q ) pk )

Notation: posi and roti denote position and rotation in TW i ak , pk are axis and position of joint k [k i] indicates whether joint k is between root and link i Jposi (q )1:3,k is the kth row

32/47

Task Variable 2: Endeffector direction

Vector attached to link i dimension parameters kin. map Jacobian d=3 link index i, attached vector v veci,v (q ) = roti v Jveci,v (q )1:3,k = [k i] ak veci (q )

Notation: posi and roti denote position and rotation in TW i ak , pk are axis and position of joint k [k i] indicates whether joint k is between root and link i Jposi (q )1:3,k is the kth row

33/47

Task Variable 3: Relative endeffector position

Position of a point on link i relative to link j dimension parameters kin. map Jacobian d=3 link indices i, j , point offset v in i posi,v|j (q ) = rot-1 j (posi,v posj ) Jposi,v|j (q ) = rot-1 j [Jposi,v Jposj Jrotj (posi,v posj )]

Derivation: For y = Rp the derivative w.r.t. a rotation around axis a is y = Rp + R p = Rp + a Rp where a is the skew matrix of a. For y = R-1 p the derivative is y = R-1 p R-1 (R )R-1 p = R-1 (p a p), where a p = a p. Notation: Jrotj (q )1:3,k = [k j ] ak contains the joint axesthe short notation J p means that each column in J takes the cross-product with p.

34/47

Task Variable 4: Relative endeffector direction

Vector attached to link i relative to link j dimension parameters kin. map Jacobian d=3 link indices i, j , attached vector v in i veci,v|j (q ) = rot-1 j veci,v Jveci,v|j (q ) = rot-1 j [Jveci,v Jrotj veci,v ]

Derivation: For y = Rp the derivative w.r.t. a rotation around axis a is y = Rp + R p = Rp + a Rp where a is the skew matrix of a. For y = R-1 p the derivative is y = R-1 p R-1 (R )R-1 p = R-1 (p a p), where a p = a p. (For details see http://userpage.fu-berlin.de/mtoussai/notes/3d-geometry.pdf) Notation: Jrotj (q )1:3,k = [k j ] ak contains the joint axesthe short notation J p means that each column in J takes the cross-product with p.

35/47

Task Variable 5: Endeffector alignment

Alignment a vector attached to link i with a reference v dimension parameters kin. map Jacobian
Note:

d=1 link index i, attached vector v , world reference v aligni,v (q ) = v veci,v Jaligni,v (q ) = v Jveci,v
align = 1 anti-align align = 0 orthog.

align = 1 align

36/47

Task Variable 6: Relative endeffector alignment

Alignment a vector attached to link i with vector attached to j dimension parameters kin. map Jacobian
Note:

d=1 link indices i, j , attached vectors vi , vj alignij,vi ,vj (q ) = vecj,vj veci,vi Jalignij,vi ,vj (q ) = vecj,vj Jveci,vi + Jvecj,vj veci,vi
align = 1 anti-align align = 0 orthog.

align = 1 align

37/47

Task Variable 7: Joint Limits

Penetration of joint limit constraints dimension parameters kin. map Jacobian d=1 joint limits qlow , qhi , margin m limits (q ) =
1 m n + + i=1 [qlow qi + m] + [qi qhi + m] 1 1 m [qlow qi + m > 0]+ m [qi qhi + m > 0]

Jlimits (q )1,i =

[x]+ = x > 0?x : 0

[ ]: indicator function

38/47

Task Variable 8: Collision Avoidance

Penetration of collision constraints dimension parameters kin. map Jacobian d=1 margin m col (q ) = Jcol (q ) =
1 m 1 m K a k=1 [m |pk K a k=1 [m |pk + pb k |]

pb k | > 0]
b pa k p k b| |pa p k k

(Jpospa + Jpospb ) k k

A collision detection engine returns a set {(a, b, pa , pb )K k=1 } of potential b collisions between link ak and bk , with nearest points pa k on a and pk on b.

39/47

Task Variable 9: Center of gravity

Center of gravity of the whole kinematic structure dimension parameters kin. map Jacobian d=3 (none) cog (q ) = Jcog (q ) =
i i

massi posi,ci massi Jposi,ci

ci denotes the center-of-mass of link i (in its own frame)

40/47

Task Variable 10: Joint angles (comfort)

The joint angles themselves dimension parameters kin. map Jacobian d=n (none) qitself (q ) = q Jqitself (q ) = In

Example: Set the target y = 0 and the precistion very low this task describes posture comfortness in terms of deviation from the joints zero position.

41/47

Task variables conclusions

There is much space for creativity in dening task variables! Many are extensions of pos and vec and the Jacobians combine the basic Jacobians. What the right task variables are to design/describe motion is a very hard problem! In what task space do humans control their motion? Possible to learn from data (task space retrieval) or perhaps via Reinforcement Learning. In practice: Robot motion design (including grasping!) requires often cumbersome handtuning of such task variables.
42/47

nearest distance

LeftHand position

How do we get smooth trajectories?

So far, all our methods only look one step ahead: f (qt+1 ) is a cost function for the next joint step q = J y desribed the next joint step What if we want to have a nice trajectory that smoothly accelerates and comes to a halt at the target?

43/47

How do we get smooth trajectories?

So far, all our methods only look one step ahead: f (qt+1 ) is a cost function for the next joint step q = J y desribed the next joint step What if we want to have a nice trajectory that smoothly accelerates and comes to a halt at the target? The next lectures will cover path nding and trajectory optimization. Here we discuss common heuristics used in engineering that are quite useful.

43/47

Trajectory generation as interpolation

A trajectory q0:T is a sequence of robot congurations qt Rn . This corresponds to T +1 time slices but T time steps (or transitions)! In software: typically stored as (T +1) n-matrix! The basic heuristic for trajectory generation: If you know a desired start point x0 and target point xT , interpolate on a straight line and choose a nice motion prole.

44/47

Heuristic motion proles

Assume initially x = 0, x = 0. After 1 second you want x = 1, x = 0. How do you move from x = 0 to x = 1 in one second?

The sine prole xt = x0 + 1 2 [1 cos(t/T )](xT x0 ) is a compromise for low max-acceleration and max-velocity
Taken from http://www.20sim.com/webhelp/toolboxes/mechatronics_toolbox/ motion_profile_wizard/motionprofiles.htm 45/47

Task space interpolation

1) Task space interpolation:
Given a initial task value y0 and a desired nal task value yT , interpolate on a straight line with a some motion prole. This gives y0:T .

2) Projection to joint space:

Given the task trajectory y0:T , compute a corresponding joint trajectory q0:T using inverse kinematics qt+1 = qt + J (yt+1 (qt ))

46/47

Joint space interpolation

1) Optimize directly nal conguration state:
Given a desired nal task value yT , optimize a nal joint state qT to minimize the function 2 f (qT ) = ||qT q0 ||2 W/T + ||yT (qT )||C
Note the step metric
1 T

W , which is consistent with T cost terms with metric W .

2) Joint space interpolation.

Given the initial conguration q0 and the nal qT , interpolate on a straight line with a some motion prole. This gives q0:T .

47/47