# Robotics 1

Inverse differential kinematics Statics
Prof. Alessandro De Luca

Robotics 1

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

2

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

6 m/s for T=6 s Robotics 1 4 . executed at constant speed v=0.planar 2R robot in straight line Cartesian motion Simulation results . at α=170° angle with x-axis. q = J-1(q) v regular case end start a line from right to left.

planar 2R robot in straight line Cartesian motion Simulation results q1 path at α=170° q2 regular case only integration error!! (10-10) Robotics 1 5 .

planar 2R robot in straight line Cartesian motion Simulation results . at α=178° angle with x-axis. q = J-1(q) v close to singular case end start a line from right to left.6 m/s for T=6 s Robotics 1 6 . executed at constant speed v=0.

of q1 close to singular case increased integration error (2·10-9) Robotics 1 7 .planar 2R robot in straight line Cartesian motion Simulation results q1 path at α=178° q2 large peak .

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.planar 2R robot in straight line Cartesian motion Simulation results .6 m/s for T=6 s Robotics 1 8 . executed at constant speed v=0. at α=178° angle with x-axis.

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 9 .

Damped Least Squares method JDLS equivalent expressions. 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 . 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.

q = J-1(q) v ..6 m/s for T=6 s Robotics 1 11 .5° angle with x-axis.planar 2R robot in straight line Cartesian motion a comparison of inverse and damped inverse Jacobian methods even closer to singular case Simulation results . executed at constant speed v=0. q = JDLS(q) v end start end start some position error .. a line from right to left. at α=179.

q = J-1(q) v .. q = JDLS(q) v here. Robotics 1 a completely different inverse solution. while crossing the region close to the folded singularity 12 . a very fast reconfiguration of first joint .5° .planar 2R robot in straight line Cartesian motion Simulation results path at α=179..

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 .

q = JDLS(q) v some actual error (25 mm) at singularity crossing. later recovered by feedback action (v ⇒ v+Ke) increased integration error (3·10-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 .planar 2R robot in straight line Cartesian motion Simulation results . q = J-1(q) v .

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 .

with the MATLAB function pinv) Robotics 1 16 .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..

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 ...

“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! . 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.Velocity manipulability   in a given configuration.

the ellipsoid loses a dimension (for m=2.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. it becomes a segment) direction of principal axes: (orthogonal) eigenvectors associated to λi w = det JJ = ∏ σ i ≥ 0 i =1 T m proportional to the volume of the ellipsoid (for m=2. to its area) Robotics 1 20 € .

since it always σ1≠σ2 Robotics 1 21 .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 θ2 r r best posture for manipulation (similar to a human arm!) full isotropy is never obtained in this case.

Transformation of forces τ2 τ1 τ3 τi τn environment •  F   τ = 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 τn •  F .Statics .

e. i. 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 ..Virtual displacements and work dqn dq2 dq1 dq3 dp ω dt = J dq dqi infinitesimal (or “virtual”.

FT ω dt = .Principle of virtual work τndqn τ2dq2 τ1dq1 -F dp .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 .

det J ≠ 0 2.Velocity and force singularities list of possible cases n m 1.  ρ < n 2.  ρ = n Robotics 1 29 .  det J = 0 1.n) ρ 1.  ρ = m 2.  ρ < m ρ = rank(J) = rank(JT) ≤ min(m.

) β 1 2 ℜ(J ) = β l2 1 Robotics 1 l2 1 (for l = l .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 . ) β ℵ(J) = β 1 2 -(l2-l1) 0 30 .

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.Force manipulability   in a given configuration. but with semi-axes of inverse lengths Robotics 1 task force manipulability ellipsoid 31 . 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.

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 m RFB frame of interest for evaluating forces/torques in a task with environmental contact Robotics 1 34 .

θm um . θm = N θ u = N um 35 Robotics 1 . θ u link another simple application of the principle of virtual work! . .Example 2: Gear reduction at joints transmission element with motion reduction ratio N:1 motor .