Matrix Methods in Kinematics Rigid Body Rotation Matrices Rigid Body – points have same relative position Displacement = Rotation and Translation Rotation about 1. Right hand Cartesian axes (x,y,z) 2. Arbitrary Axis 3. Euler Angles

Matrix Methods in Kinematics Rotate about Z Components in the fixed system x-y Rotation matrix

Matrix Methods in Kinematics Rotation about three Cartesian axes y z v2 v2 v1 v2 y x z X,Y,Z axes fixed in space x

Matrix Methods in Kinematics Plane rotation (2D) Spatial Rotation

Matrix Methods in Kinematics

Matrix Methods in Kinematics y u Rotation about axis u Rotate u to z and then back again v1 x z

Matrix Methods in Kinematics ux,uy,uz are dir cos of unit vector along u pretty useful u is fixed in space

Matrix Methods in Kinematics Euler Angles Each rotation about axis That depends preceding rotation Moving reference frame 1 2 3

Matrix Methods in Kinematics

Matrix Methods in Kinematics Rigid Body Displacement Matrix 2d Cartesian Rotation p1 q1 Fixed x-y May know p1,p Find position q

Matrix Methods in Kinematics Now a displacement matrix 3x3 planar (2D)

Matrix Methods in Kinematics Spatial (3D) Rigid Body Displacement Replace Cartesian Axis Euler Using Cartesian new original 4x4 matrix (3D)

Matrix Methods in Kinematics Example Displacement of a point Moving with a rigid body

Matrix Methods in Kinematics 3x3 Displacement matrix

Matrix Methods in Kinematics Finite Rotation Pole – plane rotation about p0 With new position vectors p1=p2=p0

Original displacement matrix Matrix Methods in Kinematics Previously p1 and p2 in D13 and D23 Displacement matrix D now written as: q2 is the same point Original displacement matrix

Matrix Methods in Kinematics

Matrix Methods in Kinematics Screw displacement matrix y q u q1 p=p1+su s p1 z x Screw displacement matrix

Matrix Methods in Kinematics HW #3 Salute z q shoulder 30◦ p y x p1=elbow q1=tip of finger Use Euler angles, [D]

Matrix Methods in Kinematics HW #3 Salute Y,x’ x” Y Changed axes notation 2 X,y’,y” X q shoulder 1 30◦ Z,z’,z” p Z p1=elbow Y,x’ x” q1=tip of finger X,y’ y” z-x-z rotation 3 Z,z’ z”

Matrix Methods in Kinematics Finding the Displacement Matrix by Inversion y B1 Known points C1 C2 A1 A2 B2 x

Matrix Methods in Kinematics Displacement Matrix by Inversion

Matrix Methods in Kinematics q q1 d a x y 2D Planar motion z D=d*a-1

Matrix Methods in Kinematics Finding the inverse of A (by hand – the long way) adj - adjoint ith row=ith column α=co factor Mij= minor of A

Matrix Methods in Kinematics Using MATLAB inv(a) ans = 0.5000 -0.5000 2.0000 0.5000 0.5000 -3.0000 -1.0000 0 2.0000 d = 5 7 6 1 1 2 1 1 1 a = 2 2 1 4 6 5 1 1 1 >> e=d*inv(a) e = 0 1 1 -1 0 3 0 0 1 Displacement matrix

Matrix Methods in Kinematics Coordinate transformations – Vector Rigid body motion in terms of axes fixed in the body x’,y’ moves with the body p 2 co-ord systems moving 1 fixed co-ord system fixed

Matrix Methods in Kinematics Transformation [T] (between 2 co-ord sys) = inverse rotation[R] (one fixed system)

Matrix Methods in Kinematics Coordinate transformations – Point p x’,y’ initially coincident with x,y [T]=[R]-1 Transformation Matrix is inverse of Displacement Matrix

Matrix Methods in Kinematics Increase rotation by p

Matrix Methods in Kinematics Hartenberg-Denavit Notation (J.ASME 1955) Co-ordinate trans for axes fixed in a rigid body (x1,y1,z1) and second set (x2,y2,z2) fixed in a second adjacent body (kinematic chain)

Matrix Methods in Kinematics

Matrix Methods in Kinematics a1=Perpendicular distance between z1 and z2 (may not be physical link length) α1= twist angle z1 into z2 (along a1) θ1= screws x1 into x2 (along S1) S1 (d1)= distance from axes x1 to x2

Matrix Methods in Kinematics 4 motions – 2 rotations, 2 translations Hartenberg-Denavit matrix

Matrix Methods in Kinematics Forward Kinematics – determine the position of the end effector given the joint variables (angles/extensions) Inverse Kinematics – what are joint variables for a desired end effector position

Matrix Methods in Kinematics 2D Planar Elbow z (rotation) axes are all parallel ai= _ dist between z (z0,z1,z2) ai = link length αi=angle between Z, αi =0 Si=di=dist between origins along z, di =0 θi=rotation angles Link ai αi di θi 1 a1 θ1 2 a2 θ2 Base frame x0 direction is arbitrary

Matrix Methods in Kinematics H-D Matrix

Matrix Methods in Kinematics

Matrix Methods in Kinematics We end up with

Matrix Methods in Kinematics Forward Kinematics Rotation matrix of 2 to 0 Translation of 2 to 0

Matrix Methods in Kinematics a1=a2=10 in. θ1=θ2

Matrix Methods in Kinematics Example: 3 Link cylindrical manipulator d3 L3 J2 d2 L2 L1 O1 at J1 J1

Matrix Methods in Kinematics Example: 3 Link cylindrical manipulator Prismatic joint (Video) d3 L3 O2 O3 J2 Prismatic joint d2 L2 L1 J1 O0 O1 Link ai αi di (Si) θi 1 d1 θ1 (var) 2 -90 d2 (var) 3 d3 (var)

Matrix Methods in Kinematics H-D Matrix Link ai αi di (Si) θi 1 d1 θ1 (var) 2 -90 d2 (var) 3 d3 (var)

Matrix Methods in Kinematics

Matrix Methods in Kinematics J1 J2 O0 L1 L2 L3 d3 d2 O3 O2 O1

Matrix Methods in Kinematics Spherical Wrist z3-z4 plane d6 Link ai αi di θi 4 -90 θ4 5 +90 θ5 6 d6 θ6 H-D Matrix

Matrix Methods in Kinematics

Matrix Methods in Kinematics

Matrix Methods in Kinematics Cylindrical Manipulator with spherical wrist Slide 46 Seems like it should be this direction a=approach s=sliding n=normal

Matrix Methods in Kinematics

Matrix Methods in Kinematics

Matrix Methods in Kinematics Puma 560 Manipulator

Matrix Methods in Kinematics Puma 560 Manipulator

Matrix Methods in Kinematics Puma Manipulator HW #4 Determine T Matrix 10 in.

Matrix Methods in Kinematics

Matrix Methods in Kinematics Co-Ordinate System for offset slider using H-D notation S&R fig 3.11 a1=Perpendicular distance between z axes (may not be physical link length) α1= twist angle zn into zn+1 (along a1) θ1= screws xn into xn+1 (along S1) S1 = distance from axes xn to xn+1

Matrix Methods in Kinematics

Matrix Methods in Kinematics Assume θ4 defined Solve for other parameters, equate elements of [A] and [I]

Matrix Methods in Kinematics http://www.cs.duke.edu/brd/Teaching/Bio/asmb/current/Papers/chap3-forward-kinematics.pdf http://medusa.sdsu.edu/Robotics/CS656/Lectures/CHAP4.pdf

Matrix Methods in Kinematics Inverse Kinematics- finding required angles for given position Cincinnati Milacron T3

Matrix Methods in Kinematics Kinematic Model i αi ai si θi 1 +90 θ1 0+/- 120 2 a2 θ2 45+/-45 3 a3 θ3 -60+/-60 4 -90 a4 θ4 0+/120 5 θ5 90+/-120 6 l θ6 0+/-180 HD notation

Matrix Methods in Kinematics Forward Kinematics, find AH for given angles θ Inverse Kinematics, For given AH, find explicit solutions for θs No general approach or closed form solutions available However, analytic solutions developed for manipulators with 6 DOF 3(7-1)-2(6) 3 consecutive intersecting axes or 3 consecutive parallel axes Called simple manipulators

Matrix Methods in Kinematics

Matrix Methods in Kinematics

Matrix Methods in Kinematics A2,A3,A4 are parallel, so multiply Combining using cosθ23=cos(θ2+θ3), sinθ23=sin(θ2+θ3)

Matrix Methods in Kinematics A5 and A6 are intersecting (Zs) so multiply them…. then

Matrix Methods in Kinematics A5 and A6 are intersecting (Zs) so multiply them…. Call this R.H.S.

Matrix Methods in Kinematics Bring A1 to other side..call this L.H.S A1 is othogonal matrix A-1=AT

Matrix Methods in Kinematics Look at RHS and LHS for elements with one variable e.g. L33 =R33, L34=R34 0+/-120 2 values – branch cases

Matrix Methods in Kinematics Since θ1 known, can find θ5 (previous equations) 2 solutions Also all elements of LHS are now known ( AT1AH) Now pick 2 other elements with single unknown (θ6) R31 and R32 Are known

Matrix Methods in Kinematics Use ATan2 for correct quadrant May have a singular point, sθ5=0 y (1,1) x (-1,-1)

Matrix Methods in Kinematics Tracking which element pairs selected 1 2 3

Matrix Methods in Kinematics Use L13=R13, L23=R23 Sθ5=0 singularity

Matrix Methods in Kinematics known Eliminates θ23

Matrix Methods in Kinematics

Matrix Methods in Kinematics So far Θ1= 2 values Θ5= 2 values Θ6= 1 value (ATan2 recognizes correct quadrant) Θ234 = 1 value (same with ATan2) Θ2= 2tan-1(t) 2 values of t (roots of second order eqn) ex. Tanα = 1 α = 45 or 225 tan(α/2) = 1 α=90 tan(α/2) = 0.7 α = 2tan-1(0.7) 2 x 35 = 70 2 X 215 = 430-360=70 1 solution

Matrix Methods in Kinematics From slide 74 Complete solution Θ1= 2 solutions Θ5= 2 values Θ6= 1 value Θ2= 2tan-1(t) 2 values for t Θ3=θ23-θ2 Θ4=θ234-θ23

Matrix Methods in Kinematics Another example Adept One Robot

Matrix Methods in Kinematics Kinematic Model

Matrix Methods in Kinematics In Base frame

Matrix Methods in Kinematics In Base frame Base (0,0,0) p3

Matrix Methods in Kinematics For x and y Typically have these transformation equations

Matrix Methods in Kinematics From the position equations 1) Squaring and adding, eliminates θ12 get Rotation about z by θ1 2)

Matrix Methods in Kinematics 2) Solution of 2) in the form

Matrix Methods in Kinematics Alternate form: Eq 1) multiply each by p1 and p2, add and subtract Reverse arguments and other stuff Substituting for θ1

Matrix Methods in Kinematics Equal length links

Matrix Methods in Kinematics For angular position of the effector, from picture

Matrix Methods in Kinematics Summary a1=a2 case

Matrix Methods in Kinematics Because σ1 is +/-

Matrix Methods in Kinematics All possible solutions For real solutions Limitations on angles Vector length squared

Matrix Methods in Kinematics Solution approach for slide 83

Matrix Methods in Kinematics

Matrix Methods in Kinematics

Matrix Methods in Kinematics

Matrix Methods in Kinematics