Inverse differential kinematics Statics and force transformations

Transcription

1 Robotics 1 Inverse differential kinematics Statics and force transformations Prof Alessandro De Luca Robotics 1 1

2 Inversion of differential kinematics! find the joint velocity vector that realizes a desired endeffector generalized velocity (linear and angular) generalized velocity v = J(q) q J square and non-singular 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

3 Incremental solution to inverse kinematics problems! joint velocity inversion can be used also to solve on-line and incrementally a sequence of inverse kinematics problems! each problem differs by a small amount dr from previous one r = f r (q) direct kinematics dr = "f (q) r "q dq = J r(q)dq differential kinematics r " r + dr r + dr = f r (q) first, increment the desired task variables dq = J r "1 (q)dr first, solve the inverse differential kinematics problem q = f r "1 (r + dr) then, solve the inverse kinematics problem q " q+ dq then, increment the original joint variables Robotics 1 3

4 Behavior near a singularity q = J -1 (q) v v constant motion start! problems arise only when commanding joint motion by inversion of a given Cartesian motion task! here, 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 Robotics 1 4

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

6 Simulation results planar 2R robot in straight line Cartesian motion path at #=170 q 1 q 2 regular case error due only to numerical integration (10-10 ) Robotics 1 6

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

8 Simulation results planar 2R robot in straight line Cartesian motion path at #=178 q 1 q 2 large peak of q 1 close to singular case still very small, but increased numerical integration error ( ) Robotics 1 8

9 q = J -1 (q) v Simulation results planar 2R robot in straight line Cartesian motion close to singular case with joint velocity saturation at V i =300 /s end start a line from right to left, at #=178 angle with x-axis, executed at constant speed v=06 m/s for T=6 s$ Robotics 1 9

10 Simulation results planar 2R robot in straight line Cartesian motion path at #=178 q 1 q 2 saturated value of q 1 close to singular case actual position error!! (6 cm) Robotics 1 10

11 Damped Least Squares method J DLS 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 norm on achieved end-effector velocity and minimum norm of joint velocity)!! = 0 when far enough from a singularity! with! > 0, there is a (vector) error (= v Jq) in executing the desired end-effector velocity v (check that " = # (#I m + J J T ) $1 v!), but the joint velocities are always reduced ( damped )! J DLS can be used for both m = n and m < n cases Robotics 1 11

12 Simulation results planar 2R robot in straight line Cartesian motion a comparison of inverse and damped inverse Jacobian methods even closer to singular case q = J -1 (q) v q = J DLS (q) v end start end start some position error a line from right to left, at #=1795 angle with x-axis, executed at constant speed v=06 m/s for T=6 s$ Robotics 1 12

13 Simulation results planar 2R robot in straight line Cartesian motion q = J -1 (q) v path at #=1795 q = J DLS (q) v here, a very fast reconfiguration of first joint a completely different inverse solution, around/after crossing the region close to the folded singularity Robotics 1 13

14 Simulation results planar 2R robot in straight line Cartesian motion q = J -1 (q) v q = J DLS (q) v q 1 extremely large peak velocity of first joint!! q 2 smooth joint motion with limited joint velocities! Robotics 1 14

15 q = J -1 (q) v Simulation results planar 2R robot in straight line Cartesian motion q = J DLS (q) v increased numerical integration error ( ) error (25 mm) when crossing the singularity, later recovered by feedback action (v v+ke) minimum singular value of JJ T and %I+JJ T they differ only when damping factor is non-zero damping factor % is chosen non-zero only close to singularity! Robotics 1 15

16 Pseudoinverse method a constrained optimization (minimum norm) problem min q H = 1 q 2 2 such that J q " v = 0 min H = 1 q "S 2 q 2 { } S = q " R n : J q # v is minimum solution pseudoinverse of J " if, the constraint is satisfied ( is feasible) " else, where minimizes the error orthogonal projection of Robotics 1 16 on

17 ! Properties of the pseudoinverse 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 (eg, with the MATLAB function pinv) Robotics 1 17

18 Numerical example Jacobian of 2R arm with l 1 = l 2 = 1 and q 2 = 0 (rank & = 1) y is the minimum norm joint velocity vector that realizes l 1 l 2 q 1 x Robotics 1 18

19 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 null space of J this is also the solution to a slightly modified constrained optimization problem ( biased toward the joint velocity ', chosen to avoid obstacles, joint limits, etc) min q H = 1 2 q " # 2 such that J q " v = 0 min H = 1 q "S 2 q # $ 2 { } S = q " R n : J q # v is minimum verification of which actual task velocity is going to be obtained v actual = J q = J( J # v + (I " J # J)# ) = JJ # v + (J " JJ # J)# = JJ # (Jw) = (JJ # J)w = Jw = v if v "#(J) $ v = Jw, for some w Robotics 1 19

20 Geometric interpretation a simple case with n=2, m=1 at a given configuration: associated homogeneous equation J q = 0 task equality constraint J q = v q 2 minimum norm solution J # v [ ] $ J q = j 1 j 2 " # q 1 q 2 % ' = v ( ) & biasing joint velocity (in general, not a solution) solution with minimum norm q " # 2 " q 1 space of joint velocities (at a configuration q)! orthogonal projection of " on #(J) linear subspace (I " J # J)# { } "(J) = q # $ 2 : J q = 0 all possible solutions lie on this line Robotics 1 20

21 Higher-order differential inversion! inversion of motion from task to joint space can be performed also at a higher differential level! acceleration-level: given q, q! jerk-level: given q, q, q q = J "1 r (q) r " J r (q) q! the (inverse) of the Jacobian is always the leading term! smoother joint motions are expected (at least, due to the existence of higher-order time derivatives r, r, ) ( ) ( ) q = J "1 r (q) r " J r (q) q " 2 J r (q) q Robotics 1 21

22 Generalized forces and torques ( 2 ( n F environment ( 1 ( 3 ( i F e generalized vectors: may contain linear and/or angular components convention: generalized forces are positive when applied on the robot " ( = forces/torques exerted by the motors at the robot joints " F = equivalent forces/torques exerted at the robot end-effector " F e = forces/torques exerted by the environment at the end-effector " principle of action and reaction: F e = - F reaction from environment is equal and opposite to the robot action on it Robotics 1 22

23 Transformation of forces Statics ( 2 ( n environment F ( 1 ( 3 ( i F e in a given configuration " what is the transformation between F at robot end-effector and ( at joints? in static equilibrium conditions (ie, no motion): " what F will be exerted on environment by a ( applied at the robot joints? " what ( at the joints will balance a F e (= -F) exerted by the environment? all equivalent formulations Robotics 1 23

24 Virtual displacements and works dq n dq 1 dq 2 dq 3 dq i dp ) dt = J dq infinitesimal (or virtual, ie, 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

25 Principle of virtual work ( n dq n F e = - F ( 1 dq 1 ( 2 dq 2 dp ( 3 dq 3 ( i dq i - F T ) dt = - FT J dq the sum of the virtual works done by all forces/torques acting on the system = 0 principle of virtual work Robotics 1 25

26 Duality between velocity and force velocity (or displacement ) in the joint space J(q) generalized velocity (or e-e displacement in the Cartesian space ) forces/torques at the joints J T (q) generalized forces at the Cartesian e-e the singular configurations for the velocity map are the same as those for the force map &(J) = &(J T ) Robotics 1 26

27 Dual subspaces of velocity and force summary of definitions Robotics 1 27

28 Velocity and force singularities list of possible cases n & = rank(j) = rank(j T ) * min(m,n) m & 1 & = m 1 det J! 0 1 & = n 2 & < m 2 det J = 0 2 & < n Robotics 1 28

29 Example of singularity analysis planar 2R arm with generic link lengths l 1 and l 2 J(q) = - l 1 s 1 -l 2 s 12 -l 2 s 12 l 1 c 1 +l 2 c 12 l 2 c 12 det J(q) = l 1 l 2 s 2 singularity at q 2 = 0 (arm straight) J = - (l 1 +l 2 )s 1 -l 2 s 1 (l 1 +l 2 )c 1 l 2 c 1,(J T ) +(J) = # -s 1 c 1,(J T ) = # c 1 s 1 l +(J T 1 +l 2 ) = -,(J) = l 2 - l 2 -(l 1 +l 2 ) +(J) singularity at q 2 = " (arm folded) J = (l 2 -l 1 )s 1 l 2 s 1 -(l 2 -l 1 )c 1 -l 2 c 1 +(J) and,(j T ) as above +(J l 2 -l T ) = (for l 1 = l 2, - ) l,(j) = 2 1 l (for l 1 = l 2, - ) -(l 2 -l 1 ) 0 Robotics 1 29

30 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 (J J T ) -1 if & = m note: the core matrix of the ellipsoid Robotics 1 equation v T A -1 v=1 is the matrix A! 30

31 Manipulability ellipsoid in velocity 1 planar 2R arm with unitary links length of principal (semi-)axes: singular values of J (in its SVD) manipulability ellipsoid scale of ellipsoid manipulability measure in a singularity, the ellipsoid loses a dimension (for m=2, it becomes a segment) direction of principal axes: (orthogonal) eigenvectors associated to % i m w = det JJ T = " i # $ 0 i=1 proportional to the volume of the ellipsoid (for m=2, to its area) Robotics 1 31

32 Manipulability measure planar 2R arm with unitary links: Jacobian J is square ( ) = det J" det J T 2 = det J = $ # i det JJ T i=1 / 1 (J) max at! 2 ="/2 max at r=2 / 2 (J)! 2 r r best posture for manipulation (similar to a human arm!) full isotropy is never obtained in this case, since it always / 1!/ 2 Robotics 1 32

33 Force manipulability! in a given configuration, evaluate how effective is the transformation between joint torques and end-effector forces! how easily can the end-effector apply generalized forces (or balance applied ones) in the various directions of the task space! in singular configurations, there are directions in the task space where external forces/torques are 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 task force manipulability ellipsoid Robotics 1 33

34 Velocity and force manipulability dual comparison of actuation vs control note: velocity and force ellipsoids have here a different scale for a better view area " planar 2R arm with unitary links det( JJ T ) = # 1 (J)$ # 2 (J) area " det( JJ T ) #1 = 1 $ 1 (J) % 1 $ 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 34

35 Velocity and force transformations! same reasoning made for relating end-effector to joint forces/torques (virtual work principle + static equilibrium) used also transforming forces and torques applied at different places of a rigid body and/or expressed in different reference frames # % $ A v A A " transformation among generalized velocities & ( = ' # A R B ) A R B S( B r BA )&# % (% $ % 0 '( $ A R B B v B B " & # ( = J ' BA % $ B v B B " & ( ' " $ # B f B B m % " T ' = J & BA $ # A f A A m % ' = & " B R A 0 $ # $ (S T ( B r BA ) B R B A R A transformation among generalized forces %" ' $ &' # A f A A m % ' & note: for skew-symmetric matrices, "S T (r) = S(r) Robotics 1 35

36 Example: 6D force/torque sensor frame of measure for the forces/torques (attached to the wrist sensor) RF A J BA! J BAT! m f RF B frame of interest for evaluating forces/torques in a task with environment contact Robotics 1 36

37 Example: Gear reduction at joints transmission element with motion reduction ratio N:1 motor link! m u m torque at motor side! u torque at link side one of the simplest applications of the principle of virtual work! m = N! u = N u m here, J = J T = N (a scalar!) Robotics 1 37