Robotics 1

Inverse differential kinematics Statics

Prof. Alessandro De Luca

Robotics 1

Inversion of differential kinematics

find the joint velocity vector that realizes a desired endeffector generalized velocity (linear and angular)
J square and non-singular

generalized velocity

v = J(q) q

q = J-1(q) v

problems

. near a singularity of the Jacobian matrix (high q) for redundant robots (no standard inverse of a rectangular matrix)

in these cases, more robust inversion methods are needed

Robotics 1

Behavior near a singularity

. q = J-1(q) v

problems arise only when commanding joint motion by inversion of a given Cartesian motion task figure shows a linear Cartesian trajectory for a planar 2R robot there is a sudden increase of the displacement/velocity of the first joint near 2=- (endeffector close to the origin), despite the required Cartesian displacement is small
3

Robotics 1

planar 2R robot in straight line Cartesian motion

Simulation results

. q = J-1(q) v

regular case

end start

a line from right to left, at =170 angle with x-axis, executed at constant speed v=0.6 m/s for T=6 s

Robotics 1 4

planar 2R robot in straight line Cartesian motion

Simulation results

q1 path at =170

q2

regular case

only integration error!! (10-10)

Robotics 1

planar 2R robot in straight line Cartesian motion

Simulation results

. q = J-1(q) v

close to singular case

end

start

a line from right to left, at =178 angle with x-axis, executed at constant speed v=0.6 m/s for T=6 s

Robotics 1 6

planar 2R robot in straight line Cartesian motion

Simulation results

q1 path at =178

q2

large peak . of q1

close to singular case

increased integration error (210-9)

Robotics 1

planar 2R robot in straight line Cartesian motion

Simulation results

. q = J-1(q) v

close to singular case with joint velocity saturation at Vi=300/s

end

start

a line from right to left, at =178 angle with x-axis, executed at constant speed v=0.6 m/s for T=6 s

Robotics 1 8

planar 2R robot in straight line Cartesian motion

Simulation results

q1 path at =178

q2

saturated value . of q1

close to singular case

actual position error!! (6 cm)

Robotics 1

Damped Least Squares method

JDLS

equivalent expressions, but this one is more convenient in redundant robots!

inversion of differential kinematics as an optimization problem function H = weighted sum of two objectives (minimum error on achieved end-effector velocity and minimum joint velocity) = 0 when far enough from a singularity . with > 0, there is a (vector) error(=v Jq) in executing the 1 desired end-effector velocity v (check that = ( Im + J J T ) v !), but the joint velocities are always limited (damped) JDLS can be used both for m = n and for m < n

10

Robotics 1

planar 2R robot in straight line Cartesian motion a comparison of inverse and damped inverse Jacobian methods even closer to singular case

Simulation results

. q = J-1(q) v

. q = JDLS(q) v

end

start

end

start

some position error ...

a line from right to left, at =179.5 angle with x-axis, executed at constant speed v=0.6 m/s for T=6 s

Robotics 1 11

planar 2R robot in straight line Cartesian motion

Simulation results
path at =179.5

. q = J-1(q) v

. q = JDLS(q) v

here, a very fast reconfiguration of first joint ...

Robotics 1

a completely different inverse solution, while crossing the region close to the folded singularity
12

planar 2R robot in straight line Cartesian motion

Simulation results

. q = J-1(q) v
q1 q2

. q = JDLS(q) v

extremely large peak velocity of first joint!!

very smooth joint motion with limited joint velocities!

Robotics 1

13

planar 2R robot in straight line Cartesian motion

Simulation results

. q = J-1(q) v

. q = JDLS(q) v
some actual error (25 mm) at singularity crossing, later recovered by feedback action (v v+Ke)

increased integration error (310-8) minimum singular value of T JJ and I+JJT they only differ when damping factor is non-zero
Robotics 1

damping factor is chosen non-zero only close to singularity!

14

Use of the pseudo-inverse

a constrained optimization (minimum norm) problem such that

solution
if else

pseudo-inverse of J
, the constraint is satisfied ( where is feasible)

minimizes the error on

15

orthogonal projection of
Robotics 1

Properties of the pseudo-inverse

it is the unique matrix that satisfies the four relationships

if rank = m = n: if = m < n:

it always exists and is computed in general numerically using the SVD = Singular Value Decomposition of J
(e.g., with the MATLAB function pinv)
Robotics 1 16

Numerical example
Jacobian of 2R arm with l1 = l2 = 1 and q2 = 0 (rank = 1)

y is the minimum norm joint velocity vector that realizes

l1

l2 q1 x

Robotics 1

17

General solution for m<n

all solutions (an infinite number) of the inverse differential kinematics problem can be written as
any joint velocity...

projection matrix in the kernel of J this is also the unique solution to a slightly modified LQ constrained optimization problem (biased toward )
such that
Robotics 1 18

Velocity manipulability

in a given configuration, we wish to evaluate how effective is the mechanical transformation between joint velocities and end-effector velocities

how easily can the end-effector be moved in the various directions of the task space equivalently, how far is the robot from a singular condition

we consider all end-effector velocities that can be obtained by choosing joint velocity vectors of unit norm

task velocity manipulability ellipsoid

Robotics 1

T -1 (J J )

if = m
19

Rem: the core matrix of the ellipsoid equation vT A-1 v=1 is the matrix A!

Manipulability ellipsoid
in velocity
planar 2R arm with unitary links

length of principal (semi-)axes: singular values of J (in its SVD)

in a singularity, the ellipsoid loses a dimension

(for m=2, it becomes a segment)

direction of principal axes: (orthogonal) eigenvectors associated to i

w = det JJ = i 0
i =1

proportional to the volume of the ellipsoid (for m=2, to its area)

Robotics 1 20

Manipulability measure
planar 2R arm with unitary links: Jacobian J is square
det ( JJ T ) = det J det J T = det J = i
i =1 2

1(J) 2(J)

max at 2=/2

max at r=2

best posture for manipulation (similar to a human arm!) full isotropy is never obtained in this case, since it always 12
Robotics 1 21

Statics - Transformation of forces

2 1 3 i n
environment

= vector of forces/torques at the joints F = vector of forces/torques exerted from the robot end-effector to the environment (equal and opposite to the reaction forces/torques from the environment to the robot end-effector) which is the relation between and F in a static equilibrium (i.e., with the robot remaining at rest)?
Robotics 1 22

Statics - Equivalent formulations

2 1 3 i in a given configuration which should be provided by the motors at the joints so that the robot end-effector applies F to the environment? which at the joints balances a -F exerted from the environment? which is the force/torque felt at the joints in the presence of a -F exerted at the robot end-effector?
Robotics 1 23

Virtual displacements and work

dqn dq2 dq1 dq3
dp dt

= J dq

dqi

infinitesimal (or virtual, i.e., satisfying all possible constraints imposed on the system) displacements at an equilibrium without kinetic energy variation (zero acceleration) without dissipative effects (zero velocity) the virtual work is the work done by all forces/torques acting on the system for a given virtual displacement

Robotics 1

24

Principle of virtual work

ndqn 2dq2 1dq1 -F dp - FT dt = - FT J dq principle of virtual work

3dq3

idqi

the sum of the virtual works done by all forces/torques acting on the system = 0

Robotics 1

25

Duality between velocity and force

J(q)
velocity (or displacement ) in the joint space forces/torques at the joints generalized velocity (or E-E displacement in the Cartesian space generalized forces at the Cartesian E-E )

JT(q)
the singular configurations for the velocity map are the same as those for the force map
Robotics 1

(J) = (JT)
26

Dual subspaces of velocity and force

definitions

Robotics 1

27

Dual subspaces of velocity and force

pictorial view

J(q)

JT(q)
all are linear subspaces: origins always belong to the defined sets!
Robotics 1 28

Velocity and force singularities

list of possible cases

n m 1. < m

= rank(J) = rank(JT) min(m,n)

1. det J = 0 1. < n

2. = m

2. det J 0

2. = n

Robotics 1

29

Example of singularity analysis

planar 2R arm with generic link lengths l1 and l2 -l1s1-l2s12 -l2s12 J(q) = l1c1+l2c12 l2c12 det J(q) = l1l2s2 (JT)

singularity at q2= 0 (arm straight) (J) = (JT) -s1 c1 (JT) = c1 s1

-(l1+l2)s1 -l2s1 J= (l1+l2)c1 l2c1 (J)

l1+l2 = l2

l2 (J) = -(l1+l2) (l2-l1)s1 l2s1 J= -(l2-l1)c1 -l2c1

singularity at q2= (arm folded) (J) and (JT) as above l2-l1 0 T (for l = l , ) 1 2 (J ) = l2 1
Robotics 1

l2 1 (for l = l , ) (J) = 1 2 -(l2-l1) 0

30

Force manipulability

in a given configuration, evaluate how effective is the mechanical transformation between joint torques and endeffector forces

how easily can the end-effector apply generalized forces (or balance applied ones) in the various directions of the task space in particular, in a singular configuration, there are directions in the task space where an external force/torque is balanced by the robot without the need of any joint torque

we consider all end-effector forces that can be applied (or balanced) by choosing joint torque vectors of unit norm

same directions of the principal axes of the velocity ellipsoid, but with semi-axes of inverse lengths
Robotics 1

task force manipulability ellipsoid

31

Velocity and force manipulability

a dual comparison
note: velocity and force ellipsoids have here a different scale for a better view

planar 2R arm with unitary links

area det ( JJ T ) = 1 ( J ) 2 ( J ) area det ( JJ T )
1

1 1 1 ( J ) 2 ( J )

Cartesian actuation task (high joint-to-task transformation ratio): preferred velocity (or force) directions are those where the ellipsoid stretches Cartesian control task (low transformation ratio = high resolution): preferred velocity (or force) directions are those where the ellipsoid shrinks
Robotics 1 32

Velocity and force transformations

the same reasoning made for relating end-effector to joint forces/ torques (static equilibrium + principle of virtual work) holds true also for relating forces and torques applied at different places of a rigid body and/or expressed in different reference frames relation among generalized velocities

relation among generalized forces

Robotics 1 33

Example 1: 6D force/torque sensor

frame of measure for the forces/torques (attached to the wrist sensor)

RFA

JBA

f
JBAT

RFB
frame of interest for evaluating forces/torques in a task with environmental contact

Robotics 1

34

Example 2: Gear reduction at joints

transmission element with motion reduction ratio N:1

motor . m um . u

link

another simple application of the principle of virtual work!

. . m = N u = N um
35

Robotics 1