0% found this document useful (0 votes)
113 views69 pages

02 Kinematics

1) The document discusses robot kinematics and describes kinematic maps, Jacobians, and inverse kinematics as applied to robot motion. 2) It introduces the concepts of poses, frames, and homogeneous transformations used to describe the positions and orientations of robot links and joints. 3) Kinematic maps are defined that relate joint angles to features like the position, orientation, or direction of motion of the end effector. The Jacobian is introduced as relating changes in joint angles to changes in these features.

Uploaded by

Md Mohinoddin
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
0% found this document useful (0 votes)
113 views69 pages

02 Kinematics

1) The document discusses robot kinematics and describes kinematic maps, Jacobians, and inverse kinematics as applied to robot motion. 2) It introduces the concepts of poses, frames, and homogeneous transformations used to describe the positions and orientations of robot links and joints. 3) Kinematic maps are defined that relate joint angles to features like the position, orientation, or direction of motion of the end effector. The Jacobian is introduced as relating changes in joint angles to changes in these features.

Uploaded by

Md Mohinoddin
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 69

Robotics

Kinematics

Kinematic map, Jacobian, inverse kinematics


as optimization problem, motion profiles,
trajectory interpolation, multiple simultaneous
tasks, special task variables,
configuration/operational/null space,
singularities

Marc Toussaint
University of Stuttgart
Winter 2014/15
• Two “types of robotics”:
1) Mobile robotics – is all about localization & mapping
2) Manipulation – is all about interacting with the world
0) Kinematic/Dynamic Motion Control: same as 2) without ever making
it to interaction..

• Typical manipulation robots (and animals) are kinematic trees


Their pose/state is described by all joint angles

2/62
Basic motion generation problem
• Move all joints in a coordinated way so that the endeffector makes a
desired movement

01-kinematics: ./x.exe -mode 2/3/4

3/62
Outline

• Basic 3D geometry and notation

• Kinematics: φ : q 7→ y
• Inverse Kinematics: y ∗ 7→ q ∗ = argminq ||φ(q) − y ∗ ||2C + ||q − q0 ||2W
• Basic motion heuristics: Motion profiles

• Additional things to know


– Many simultaneous task variables
– Singularities, null space,

4/62
Basic 3D geometry & notation

5/62
Pose (position & orientation)

• A pose is described by a translation p ∈ R3 and a rotation R ∈ SO(3)


– R is an orthonormal matrix (orthogonal vectors stay orthogonal, unit
vectors stay unit)
– R-1 = R>
– columns and rows are orthogonal unit vectors
– det(R) = 1
R R12 R13 
 
 11
– R=  R21

R22 R23 


R31 R32 R33
6/62
Frame and coordinate transforms

• Let (o, e1:3 ) be the world frame, (o0 , e01:3 ) be the body’s frame.
The new basis vectors are the columns in R, that is,
e01 = R11 e1 + R21 e2 + R31 e3 , etc,
• x = coordinates in world frame (o, e1:3 )
x0 = coordinates in body frame (o0 , e01:3 )
p = coordinates of o0 in world frame (o, e1:3 )

x = p + Rx0
7/62
Rotations
• Rotations can alternatively be represented as
– Euler angles – NEVER DO THIS!
– Rotation vector
– Quaternion – default in code
• See the “geometry notes” for formulas to convert, concatenate & apply
to vectors

8/62
Homogeneous transformations
• xA = coordinates of a point in frame A
xB = coordinates of a point in frame B

• Translation and rotation: xA = t + RxB

• Homogeneous transform T ∈ R4×4 :


 
R t
TA→B = 
 
0 1
 

B B
     
R t x  Rx + t
xA = TA→B xB = 
    =  
0 1 1 1
     

in homogeneous coordinates, we append a 1 to all coordinate vectors

9/62
Is TA→B forward or backward?
• TA→B describes the translation and rotation of frame B relative to A
That is, it describes the forward FRAME transformation (from A to B)

• TA→B describes the coordinate transformation from xB to xA


That is, it describes the backward COORDINATE transformation

• Confused? Vectors (and frames) transform covariant, coordinates


contra-variant. See “geometry notes” or Wikipedia for more details, if
you like.

10/62
Composition of transforms

TW →C = TW →A TA→B TB→C
xW = TW →A TA→B TB→C xC 11/62
Kinematics

12/62
Kinematics

B eff

joint B'
transf.
A
relative
C'

A'
C eff.
offset

link
transf.
W

• A kinematic structure is a graph (usually tree or chain)


of rigid links and joints

TW →eff (q) = TW →A TA→A0 (q) TA0 →B TB→B 0 (q) TB 0 →C TC→C 0 (q) TC 0 →eff
13/62
Joint types
• Joint transformations: TA→A0 (q) depends on q ∈ Rn

revolute joint: joint angle q ∈ R determines rotation about x-axis:


 
1 

0 0 0

0 cos(q) − sin(q) 0
 

TA→A0 (q) = 



0 sin(q) cos(q) 0
 

 
 
0 0 0 1
 

prismatic joint: offset q ∈ R determines translation along x-axis:


 
1 

0 0 q

0 1 0 0
 

TA→A0 (q) = 



0 0 1 0
 

 
 
0 0 0 1
 

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


(2dof)
14/62
15/62
Kinematic Map

• For any joint angle vector q ∈ Rn we can compute TW →eff (q)


by forward chaining of transformations

TW →eff (q) gives us the pose of the endeffector in the world frame

16/62
Kinematic Map

• For any joint angle vector q ∈ Rn we can compute TW →eff (q)


by forward chaining of transformations

TW →eff (q) gives us the pose of the endeffector in the world frame

• In general, a kinematic map is any (differentiable) mapping

φ : q 7→ y

that maps to some arbitrary feature y ∈ Rd of the pose q ∈ Rn

16/62
Kinematic Map
• The three most important examples for a kinematic map φ are
– A position v on the endeffector transformed to world coordinates:

φpos
eff,v (q) = TW →eff (q) v ∈ R3

– A direction v ∈ R3 attached to the endeffector in world coordinates:

φvec
eff,v (q) = RW →eff (q) v ∈ R3

Where RA→B is the rotation in TA→B .


– The (quaternion) orientation q ∈ R4 of the endeffector:

φquat
eff (q) = RW →eff (q) ∈ R4

• See the technical reference later for more kinematic maps, especially
relative position, direction and quaternion maps.

17/62
Jacobian
• When we change the joint angles, δq, how does the effector position
change, δy?


• Given the kinematic map y = φ(q) and its Jacobian J(q) = ∂q φ(q), we
have:
δy = J(q) δq


∂φ1 (q) ∂φ1 (q) ∂φ1 (q) 
 ∂q1

∂q2 ... ∂qn  
 ∂φ2 (q) ∂φ2 (q) ∂φ2 (q) 

∂ ...

 
 ∂q1 ∂q2 ∂qn 
∈ Rd×n

J(q) = φ(q) = 
.. ..


∂q
 
 


 . . 


 ∂φd (q) ∂φd (q) ∂φd (q) 
 

∂q1 ∂q2 ... ∂qn

18/62
Jacobian for a rotational degree of freedom

eff

axis

point
i-th joint

• Assume the i-th


 
joint is located at pi = tW →i (q) and has rotation axis
1
 
ai = RW →i (q)
0

 
 
0
 

• We consider an infinitesimal variation δqi ∈ R of the ith joint and see


how an endeffector position peff = φpos
eff,v (q) and attached vector
vec
aeff = φeff,v (q) change.

19/62
Jacobian for a rotational degree of freedom

eff
Consider a variation δqi
axis → the whole sub-tree rotates
point
i-th joint
δpeff = [ai × (peff − pi )] δqi
δaeff = [ai × aeff ] δqi

⇒ Position Jacobian:
 
[an × (peff − pn )]
[a1 × (peff − p1 )]

[a2 × (peff − p2 )]






 ⇒ Vector Jacobian:
 

[an × aeff ]
[a1 × aeff ]
[a2 × aeff ]
 
 
   
   
pos
 
3×n vec
 
Jeff,v (q) = ∈R Jeff,v (q) = ∈ R3×n
   

..
.
   
   
   
   
   
..
.

   
 
 
 
 
 

20/62
Jacobian for general degrees of freedom
• Every degree of freedom qi generates (infinitesimally, at a given q)
– a rotation around axis ai at point pi
– and/or a translation along the axis bi
For instance:
– the DOF of a hinge joint just creates a rotation around ai at pi
– the DOF of a prismatic joint creates a translation along bi
– the DOF of a rolling cylinder creates rotation and translation
– the first DOF of a cylindrical joint generates a translation, its second DOF
a translation

• We can compute all Jacobians from knowing ai , pi and bi for all DOFs
(in the current configuration q ∈ Rn )

21/62
Inverse Kinematics

22/62
Inverse Kinematics problem
• Generally, the aim is to find a robot configuration q such that φ(q) = y ∗
• Iff φ is invertible
q ∗ = φ-1 (y ∗ )

• But in general, φ will not be invertible:

1) The pre-image φ-1 (y ∗ ) = may be empty: No configuration can


generate the desired y ∗

2) The pre-image φ-1 (y ∗ ) may be large: many configurations can


generate the desired y ∗

23/62
Inverse Kinematics as optimization problem
• We formalize the inverse kinematics problem as an optimization
problem
q ∗ = argmin ||φ(q) − y ∗ ||2C + ||q − q0 ||2W
q

• The 1st term ensures that we find a configuration even if y ∗ is not


exactly reachable
The 2nd term disambiguates the configurations if there are many
φ-1 (y ∗ )

24/62
Inverse Kinematics as optimization problem

q ∗ = argmin ||φ(q) − y ∗ ||2C + ||q − q0 ||2W


q

• The formulation of IK as an optimization problem is very powerful and


has many nice properties
• We will be able to take the limit C → ∞, enforcing exact φ(q) = y ∗ if
possible
• Non-zero C -1 and W corresponds to a regularization that ensures
numeric stability
• Classical concepts can be derived as special cases:
– Null-space motion
– regularization; singularity robutness
– multiple tasks
– hierarchical tasks

25/62
Solving Inverse Kinematics
• The obvious choice of optimization method for this problem is
Gauss-Newton, using the Jacobian of φ
• We first describe just one step of this, which leads to the classical
equations for inverse kinematics using the local Jacobian...

26/62
Solution using the local linearization
• When using the local linearization of φ at q0 ,

φ(q) ≈ y0 + J (q − q0 ) , y0 = φ(q0 )

• We can derive the optimum as

f (q) = ||φ(q) − y ∗ ||2C + ||q − q0 ||2W


= ||y0 − y ∗ + J (q − q0 )||2C + ||q − q0 ||2W

f (q) = 0> = 2(y0 − y ∗ + J (q − q0 ))>CJ + 2(q − q0 )T W
∂q
J>C (y ∗ − y0 ) = (J>CJ + W ) (q − q0 )

q ∗ = q0 + J ] (y ∗ − y0 )
with J ] = (J>CJ + W )-1 J>C = W -1 J>(JW -1 J> + C -1 )-1 (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) 27/62
“Small step” application
• This approximate solution to IK makes sense
– if the local linearization of φ at q0 is “good”
– if q0 and q ∗ are close
• This equation is therefore typically used to iteratively compute small
steps in configuration space


qt+1 = qt + J ] (yt+1 − φ(qt ))


where the target yt+1 moves smoothly with t

28/62
Example: Iterating IK to follow a trajectory
• Assume initial posture q0 . We want to reach a desired endeff position
y ∗ in T steps:

Input: initial state q0 , desired y ∗ , methods φpos and J pos


Output: trajectory q0:T
1: Set y0 = φpos (q0 ) // starting endeff position
2: for t = 1 : T do
3: y ← φpos (qt-1 ) // current endeff position
4: J ← J pos (qt-1 ) // current endeff Jacobian
5: ŷ ← y0 + (t/T )(y ∗ − y0 ) // interpolated endeff target
6: qt = qt-1 + J ] (ŷ − y) // new joint positions
7: Command qt to all robot motors and compute all TW →i (qt )
8: end for

01-kinematics: ./x.exe -mode 2/3

29/62
Example: Iterating IK to follow a trajectory
• Assume initial posture q0 . We want to reach a desired endeff position
y ∗ in T steps:

Input: initial state q0 , desired y ∗ , methods φpos and J pos


Output: trajectory q0:T
1: Set y0 = φpos (q0 ) // starting endeff position
2: for t = 1 : T do
3: y ← φpos (qt-1 ) // current endeff position
4: J ← J pos (qt-1 ) // current endeff Jacobian
5: ŷ ← y0 + (t/T )(y ∗ − y0 ) // interpolated endeff target
6: qt = qt-1 + J ] (ŷ − y) // new joint positions
7: Command qt to all robot motors and compute all TW →i (qt )
8: end for

01-kinematics: ./x.exe -mode 2/3

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

29/62
Two additional notes
• What if we linearize at some arbitrary q 0 instead of q0 ?

φ(q) ≈ y 0 + J (q − q 0 ) , y 0 = φ(q 0 )
q ∗ = argmin ||φ(q) − y ∗ ||2C + ||q − q 0 + (q 0 − q0 )||2W
q

= q 0 + J ] (y ∗ − y 0 ) + (I − J ] J) h , h = q0 − q 0 (1)

• What if we want to find the exact (local) optimum? E.g. what if we want
to compute a big step (where q ∗ will be remote from q) and we cannot
not rely only on the local linearization approximation?
– Iterate equation (1) (optionally with a step size < 1 to ensure
convergence) by setting the point y 0 of linearization to the current q ∗
– This is equivalent to the Gauss-Newton algorithm

30/62
Where are we?
• We’ve derived a basic motion generation principle in robotics from
– an understanding of robot geometry & kinematics
– a basic notion of optimality

31/62
Where are we?
• We’ve derived a basic motion generation principle in robotics from
– an understanding of robot geometry & kinematics
– a basic notion of optimality

• In the remainder:
A. Discussion of classical concepts
B. Heuristic motion profiles for simple trajectory generation
C. Extension to multiple task variables

31/62
Discussion of classical concepts

– Singularity and singularity-robustness


– Nullspace, task/operational space, joint space
– “inverse kinematics” ↔ “motion rate control”

32/62
Singularity
• In general: A matrix J singular ⇐⇒ rank(J) < d
– rows of J are linearly dependent
– dimension of image is < d
– δy = Jδq ⇒ dimensions of δy limited
– Intuition: arm fully stretched

33/62
Singularity
• In general: A matrix J singular ⇐⇒ rank(J) < d
– rows of J are linearly dependent
– dimension of image is < d
– δy = Jδq ⇒ dimensions of δy limited
– Intuition: arm fully stretched

• Implications:
det(JJ>) = 0
→ pseudo-inverse J>(JJ>)-1 is ill-defined!
→ inverse kinematics δq = J>(JJ>)-1 δy computes “infinite” 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 33/62
Null/task/operational/joint/configuration spaces
• The space of all q ∈ Rn is called joint/configuration space
The space of all y ∈ Rd is called task/operational space
Usually d < n, which is called redundancy

34/62
Null/task/operational/joint/configuration spaces
• The space of all q ∈ Rn is called joint/configuration 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 configurations q:

nullspace(y ∗ ) = {q | φ(q) = y ∗ }

• We have

δq = argmin ||q − a||2W + ||Jq − δy||2C


q

= J δy + (I − J # J)a ,
#
J # = W -1 J>(JW -1 J> + C -1 )-1

In the limit C → ∞ it is guaranteed that Jδq = δy (we are exacty on the


manifold). The term a introduces additional “nullspace motion”. 34/62
Inverse Kinematics and Motion Rate Control
Some clarification of concepts:

• The notion “kinematics” describes the mapping φ : q 7→ y, which


usually is a many-to-one function.
• The notion “inverse kinematics” in the strict sense describes some
mapping g : y 7→ q such that φ(g(y)) = y, which usually is non-unique
or ill-defined.
• 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/τ and q̇ = δq/τ and q̇ = J ] ẏ. Therefore
the control cycle effectively controls the endeffector velocity—this is
why it is called motion rate control.

35/62
Heuristic motion profiles

36/62
Heuristic motion profiles
• Assume initially x = 0, ẋ = 0. After 1 second you want x = 1, ẋ = 0.
How do you move from x = 0 to x = 1 in one second?

The sine profile xt = x0 + 12 [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
37/62
Motion profiles
• Generally, let’s define a motion profile as a mapping

MP : [0, 1] 7→ [0, 1]

with MP(0) = 0 and MP(1) = 1 such that the interpolation is given as

xt = x0 + MP(t/T ) (xT − x0 )

• For example

MPramp (s) = s
1
MPsin (s) = [1 − cos(πs)]
2

38/62
Joint space interpolation

1) Optimize a desired final configuration qT :


Given a desired final task value yT , optimize a final joint state qT to minimize
the function
f (qT ) = ||qT − q0 ||2W/T + ||yT − φ(qT )||2C

– The metric T1 W is consistent with T cost terms with step metric W .


– In this optimization, qT will end up remote from q0 . So we need to iterate
Gauss-Newton, as described on slide 30.

2) Compute q0:T as interpolation between q0 and qT :


Given the initial configuration q0 and the final qT , interpolate on a straight line
with a some motion profile. E.g.,

qt = q0 + MP(t/T ) (qT − q0 )

39/62
Task space interpolation

1) Compute y0:T as interpolation between y0 and yT :


Given a initial task value y0 and a desired final task value yT , interpolate on a
straight line with a some motion profile. E.g,

yt = y0 + MP(t/T ) (yT − y0 )

2) Project y0:T to q0:T using inverse kinematics:


Given the task trajectory y0:T , compute a corresponding joint trajectory q0:T
using inverse kinematics

qt+1 = qt + J ] (yt+1 − φ(qt ))

(As steps are small, we should be ok with just using this local linearization.)

40/62
peg-in-a-hole demo

41/62
Multiple tasks

42/62
Multiple tasks

43/62
Multiple tasks

RightHand
position
LeftHand
position

44/62
Multiple tasks
• Assume we have m simultaneous tasks; for each task i we have:
– a kinematic map φi : Rn → Rdi
– a current value φi (qt )
– a desired value yi∗
– a precision %i (equiv. to a task cost metric Ci = %i I)

45/62
Multiple tasks
• Assume we have m simultaneous tasks; for each task i we have:
– a kinematic map φi : Rn → Rdi
– a current value φi (qt )
– a desired value yi∗
– a precision %i (equiv. to a task cost metric Ci = %i I)

• Each task contributes a term to the objective function

q ∗ = argmin ||q − q0 ||2W + %1 ||φ1 (q) − y1∗ ||2 + %2 ||φ2 (q) − y2∗ ||2 + · · ·
q

45/62
Multiple tasks
• Assume we have m simultaneous tasks; for each task i we have:
– a kinematic map φi : Rn → Rdi
– a current value φi (qt )
– a desired value yi∗
– a precision %i (equiv. to a task cost metric Ci = %i I)

• Each task contributes a term to the objective function

q ∗ = argmin ||q − q0 ||2W + %1 ||φ1 (q) − y1∗ ||2 + %2 ||φ2 (q) − y2∗ ||2 + · · ·
q

which we can also write as

q ∗ = argmin ||q − q0 ||2W + ||Φ(q)||2


q

% (φ (q) − y1∗ )
 

√ 1 1

%2 (φ2 (q) − y2∗ )


  P
 
di
where Φ(q) := 



 ∈R i

 .. 

.
 

45/62
Multiple tasks
• We can “pack” together all tasks in one “big task” Φ.

Example: We want to control the 3D position of the left hand and of the right
hand. Both are “packed” to one 6-dimensional task vector which becomes zero
if both tasks are fulfilled.

• The big Φ is scaled/normalized in a way that


– the desired value is always zero
– the cost metric is I

∂Φ(q0 )
• Using the local linearization of Φ at q0 , J = ∂q , the optimum is

q ∗ = argmin ||q − q0 ||2W + ||Φ(q)||2


q

≈ q0 − (J>J + W )-1 J> Φ(q0 ) = q0 − J # Φ(q0 )

46/62
Multiple tasks

• We learnt how to “puppeteer a robot”


• We can handle many task variables
(but specifying their precisions %i be-
comes cumbersome...)

RightHand • In the remainder:


position
LeftHand
A. Classical limit of “hierarchical IK” and
position
nullspace motion
B. What are interesting task variables?

47/62
Hierarchical IK & nullspace motion
• In the classical view, tasks should be executed exactly, which means taking the
limit %i → ∞ in some prespecified hierarchical order.
• We can rewrite the solution in a way that allows for such a hierarchical limit:
• One task plus “nullspace motion”:

f (q) = ||q − a||2W + %1 ||J1 q − y1 ||2


q ∗ = [W + %1 J> -1 >
1 J1 ] [W a + %1 J1 y1 ]

= J1# y1 + (I − J1# J1 )a
J1# = (W/%1 + J> -1 > -1 > -1 >
1 J1 ) J1 = W J1 (J1 W J1 + I/%1 )
-1

• Two tasks plus nullspace motion:

f (q) = ||q − a||2W + %1 ||J1 q − y1 ||2 + %2 ||J2 q − y2 ||2


q ∗ = J1# y1 + (I − J1# J1 )[J2# y2 + (I − J2# J2 )a]
J2# = (W/%2 + J> -1 > -1 > -1 >
2 J2 ) J2 = W J2 (J2 W J2 + I/%2 )
-1

• etc...
48/62
Hierarchical IK & nullspace motion
• The previous slide did nothing but rewrite the nice solution
q ∗ = −J # Φ(q0 ) (for the “big” Φ) in a strange hierarchical way that
allows to “see” nullspace projection

• The benefit of this hierarchical way to write the solution is that one can
take the hierarchical limit %i → ∞ and retrieve classical hierarchical IK

• The drawbacks are:


– It is somewhat ugly
– In practise, I would recommend regularization in any case (for numeric
stability). Regularization corresponds to NOT taking the full limit %i → ∞.
Then the hierarchical way to write the solution is unnecessary. (However,
it points to a “hierarchical regularization”, which might be numerically more
robust for very small regularization?)
– The general solution allows for arbitrary blending of tasks

49/62
Reference: interesting task variables

The following slides will define 10 different types of task variables.


This is meant as a reference and to give an idea of possibilities...

50/62
Position
Position of some point attached to link i
dimension d=3
parameters link index i, point offset v
kin. map φpos
iv (q) = TW →i v
pos
Jacobian Jiv (q)·k = [k ≺ i] ak × (φpos
iv (q) − pk )

Notation:
– ak , pk are axis and position of joint k
– [k ≺ i] indicates whether joint k is between root and link i
– J·k is the kth column of J

51/62
Vector
Vector attached to link i
dimension d=3
parameters link index i, attached vector v
kin. map φvec
iv (q) = RW →i v
vec
Jacobian Jiv (q) = Ai × φvec
iv (q)

Notation:
– Ai is a matrix with columns (Ai )·k = [k ≺ i] ak containing the joint axes or
zeros
– the short notation “A × p” means that each column in A takes the
cross-product with p.

52/62
Relative position
Position of a point on link i relative to point on link j
dimension d=3
parameters link indices i, j, point offset v in i and w in j
kin. map φpos -1 pos pos
iv|jw (q) = Rj (φiv − φjw )
pos pos pos
Jacobian Jiv|jw (q) = Rj-1 [Jiv − Jjw − Aj × (φpos pos
iv − φjw )]

Derivation:
For y = Rp the derivative w.r.t. a rotation around axis a is
y 0 = Rp0 + R0 p = Rp0 + a × Rp. For y = R-1 p the derivative is
y 0 = R-1 p0 − R-1 (R0 )R-1 p = R-1 (p0 − a × p). (For details see
http://ipvs.informatik.uni-stuttgart.de/mlr/marc/notes/3d-geometry.pdf)

53/62
Relative vector
Vector attached to link i relative to link j
dimension d=3
parameters link indices i, j, attached vector v in i
kin. map φvec -1 vec
iv|j (q) = Rj φiv
vec
Jacobian Jiv|j (q) = Rj-1 [Jiv
vec
− Aj × φvec
iv ]

54/62
Alignment
Alignment of a vector attached to link i with a reference v ∗
dimension d=1
parameters link index i, attached vector v, world reference v ∗
align
kin. map φiv (q) = v ∗> φvec
iv
align
Jacobian Jiv (q) = v ∗> Jiv
vec

Note: φalign = 1 ↔ align φalign = −1 ↔ anti-align φalign = 0 ↔ orthog.

55/62
Relative Alignment
Alignment a vector attached to link i with vector attached to j
dimension d=1
parameters link indices i, j, attached vectors v, w
align > vec
kin. map φiv|jw (q) = (φvec
jw ) φiv
align > vec vec> vec
Jacobian Jiv|jw (q) = (φvec
jw ) Jiv + φiv Jjw

56/62
Joint limits
Penetration of joint limits
dimension d=1
parameters joint limits qlow , qhi , margin m
1
Pn + +
kin. map φlimits (q) = m i=1 [m − qi + qlow ] + [m + qi − qhi ]
1 1
Jacobian Jlimits (q)1,i = − m [m−qi +qlow > 0]+ m [m+qi −qhi > 0]

[x]+ = x > 0?x : 0 [· · · ]: indicator function

57/62
Collision limits
Penetration of collision limits
dimension d=1
parameters margin m
1
PK
kin. map φcol (q) = m k=1 [m − |pak − pbk |]+
1
PK
Jacobian Jcol (q) = m k=1 [m − |pak − pbk | > 0]
pa −pb
(−Jppos pos > k
a + J b )
p
k
|pa −pb |
k k k k

A collision detection engine returns a set {(a, b, pa , pb )K


k=1 } of potential
collisions between link ak and bk , with nearest points pak on a and pbk on b.

58/62
Center of gravity
Center of gravity of the whole kinematic structure
dimension d=3
parameters (none)
φcog (q) = massi φpos
P
kin. map i ici
pos
J cog (q) =
P
Jacobian i massi Jic i

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

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

Example: Set the target y ∗ = 0 and the precision % very low → this task
describes posture comfortness in terms of deviation from the joints’ zero
position. In the classical view, it induces “nullspace motion”.

60/62
Task variables – conclusions

• There is much space for creativity in defin-


ing task variables! Many are extensions of
φpos and φvec and the Jacobians combine
the basic Jacobians.

• What the right task variables are to de-


sign/describe motion is a very hard prob-
lem! In what task space do humans con-
trol their motion? Possible to learn from
nearest data (“task space retrieval”) or perhaps via
distance
LeftHand Reinforcement Learning.
position

• In practice: Robot motion design (includ-


ing grasping) may require cumbersome
hand-tuning of such task variables.

61/62
• Technical Reference: The four rotation axes of a quaternion joint:

A quaternion joint has four DOFs. If it is currently in configuration q ∈ R4 , the ith DOFs
generates (infinitesimally) a rotation around the axis

−2
ai = p [ei ◦ q -1 ]1:3
q2

where ei ∈ R4 is the ith unit vector, ◦ is the concatenation of quaternions, q -1 the inverse
quaternion, q 2 the quaternion 2-norm (in case it is not normalized), and [·]1:3 pics the
vector elements of the quaternion (derivation: see geometry notes). As for the hinge
joint, these four axes are further transformed to world coordinates, ai ← RW →j ai , if the
quaternion joint is located in the coordinate frame j.

62/62

You might also like