autorob.github.io Inverse Kinematics UM EECS 398/598 - autorob.github.io

Transcription

1 autorob.github.io Inverse Kinematics

2 Objective (revisited) Goal: Given the structure of a robot arm, compute Forward kinematics: predicting the pose of the end-effector, given joint positions. Inverse kinematics: inferring the joint positions necessary to reach a desired end-effector pose. We need to solve for configuration from a transform between world and endeffector frames.

3 Forward kinematics: many-to-one mapping of robot configuration to reachable workspace endeffector poses Global (Frame w) Find this, From these Base (Frame 0) Transform of endeffector wrt. base Endeffector (Frame 6)

4 Inverse kinematics: one-to-many mapping of workspace endeffector pose to robot configuration Global (Frame w) From this, Find these Base (Frame 0) Transform of endeffector wrt. base Endeffector (Frame 6)

5 Inverse kinematics: how to solve for q = {θ1,,θn} from T 0 N? Global (Frame w) From this, Find these Base (Frame 0) Transform of endeffector wrt. base Endeffector (Frame 6)

6 Let s define IK starting from FK

7 Consider a planar 2-link arm as an example

8 Consider a planar 2-link arm as an example with 2 links with length αi,

9 Consider a planar 2-link arm as an example with 2 links, 2 joints,

10 Consider a planar 2-link arm as an example with 2 links, 2 joints, coordinate frames at each body, and

11 Consider a planar 2-link arm as an example Robot configuration defined by DoF state q 2 angular DOFs q = [θ 1,θ 2 ] joint axes out of plane with 2 links, 2 joints, coordinate frames at each body, and configuration over DoFs

12 Consider a planar 2-link arm as an example Do not forget endeffector Remember a robot has N joint frames and N+1 link frames

13 Consider a planar 2-link arm as an example Frame 2 is the tool frame Robot endeffector is the gripper pose in world frame Endeffector pose has position can consider orientation Endeffector defines tool frame with transform world frame 2 Cartesian DOFs o 0 N = p 0 = (p x0,p y0 ) Endeffector can be specified as a point p 2 (origin of tool frame) wrt. Frame 1 Endeffector transforms into p 0 in world frame

14 Endeffector axes Sliding axis Normal axis Approach axis

15 Checkpoint: Transform endeffector on link to world endeffector link4 endeffector world tool frame world frame

16 Checkpoint: Transform endeffector on link to world endeffector link4 endeffector world tool frame world frame

17 Forward kinematics: given configuration, compute endeffector Robot configuration defined by DoF state 2 angular DOFs q = [θ 1,θ 2 ] Robot endeffector is the gripper pose in world frame 2 Cartesian DOFs o 0 N = p 0 = (p x0,p y0 )

18 Forward kinematics: [o 0 N,R 0 N] = f(q) p 0 = f(θ 1,θ 2 ) Robot configuration defined by DoF state 2 angular DOFs q = [θ 1,θ 2 ] Robot endeffector is the gripper pose in world frame 2 Cartesian DOFs o 0 N = p 0 = (p x0,p y0 )

19 Forward kinematics: [o 0 N,R 0 N] = f(q) What is the position and orientation of the tool wrt. the world? remember: What are the elements of this matrix? What are the elements of this vector?

20 Forward kinematics: [o 0 N,R 0 N] = f(q) remember: p0 = R2 0 p2 + d2 0 where What is the position and orientation of the tool wrt. the world? R2 0 = R1 0 R2 1 d2 0 = R1 0 d2 1 + d1 0 What are the elements of this vector?

21 Start with: d2 0 = R1 0 d2 1 + d1 0 substitute in variables then perform operations: then substitute trig identities to get: What are the elements of this vector?

22 Forward kinematics: [o 0 N,R 0 N] = f(q)

23 Forward kinematics: [o 0 N,R 0 N] = f(q) p 0 = f(θ 1,θ 2 )

24 Inverse kinematics: given endeffector, compute configuration??

25 Inverse kinematics: q = f -1 ([o 0 N,R 0 N]) [θ 1,θ 2 ] = f -1 (p 0 )?? Just consider endeffector position for now

26 1DOF pendulum example desired endeffector position (o 0 N) given as an x,y location assume: 1DOF motor at pendulum axis, motor servo moves arm to angle θ1 what is θ1? θ 1

27 1DOF pendulum example desired endeffector position (o 0 N) given as an x,y location assume: 1DOF motor at pendulum axis, motor servo moves arm to angle θ1 what is θ1? θ 1

28 Inverse kinematics: q = f -1 ([o 0 N,R 0 N]) [θ 1,θ 2 ] = f -1 (x,y) desired endeffector position (o 0 N) given as location x,y let s start by solving for θ 2

29 Inverse kinematics: q = f -1 ([o 0 N,R 0 N]) [θ 1,θ 2 ] = f -1 (x,y) solve for θ 2

30 Inverse kinematics: q = f -1 ([o 0 N,R 0 N]) [θ 1,θ 2 ] = f -1 (x,y) solve for θ 2 Law of Cosines

31 Inverse kinematics: q = f -1 ([o 0 N,R 0 N]) [θ 1,θ 2 ] = f -1 (x,y) solve for θ 2 solve for θ 1 Consider two triangles

32 Inverse kinematics: q = f -1 ([o 0 N,R 0 N]) [θ 1,θ 2 ] = f -1 (x,y) solve for θ 2 solve for θ 1

33 inverse kinematics: (θ1,θ2) = f -1 (x,y) is this the right solution?

34 when is there one solution?

35 when is there no solution?

36 when is there no solution?

37 Can we do IK for 3 links?

38 Try this

39 How many solutions for this arm? 3 unknowns 2 constraints Remember: Ax=b

40 Inverse Kinematics: 2D

41 Inverse Kinematics: 2D Configuration Transform from endeffector frame to world frame

42 Inverse Kinematics: 2D Configuration Transform from endeffector frame to world frame Inverse orientation Inverse position

43 Inverse Kinematics: 2D Configuration Transform from endeffector Closed form solution

44 Inverse Kinematics: 3D Configuration Transform from endeffector 6 DOF position and orientation of endeffector

45 Inverse Kinematics: 3D Configuration Transform from endeffector Closed form solution? 6 DOF position and orientation of endeffector

46 Stanford Manipulator assumes D-H frames

47 Recognize this robot?

48 RexArm (EECS 467 / ROB 550)

49 RexArm (EECS 467 / ROB 550) Find: configuration q = [θ1 θ2 θ3 θ4] as robot joint angles

50 Given: endeffector orientation ɸ as angle wrt. plane centered at o3 and parallel to ground plane link lengths (L4,L3,L2,L1) Find: configuration q = [θ1 θ2 θ3 θ4] as robot joint angles endeffector position [xg yg zg] wrt. base frame

51 overhead view solve for θ 1

52 solve for θ 1 Decoupling: separate endeffector from rest of the robot at last joint solve for θ 3 and joint 1 from rest of robot

53 solve for θ 1 o3 L3 θ 3 ɣ L2 solve for θ 3 Δz β Ψ θ 2 Δr o1

54 solve for θ 1 solve for θ 3 (Law of cosines with supplementary angle ɣ) Δz o3 L3 θ 3 ɣ β L2 Ψ θ 2 Δr o1

55 solve for θ 1 solve for θ 3 o3 L3 θ 3 ɣ L2 solve for θ 2 (Law of cosines with angle Ψ, arctan with angle β) Δz β Ψ θ 2 Δr o1

56 solve for θ 1 solve for θ 3 o3 L3 θ 3 ɣ L2 solve for θ 2 Δz β Ψ θ 2 two potential solutions depending on elbow angle Δr o1

57 solve for θ 1 θ 4 o3 θ 3 solve for θ 3 ɸ L4 θ 2 solve for θ 2 o1 solve for θ 4

58 solve for θ 1 solve for θ 3 θ 2 solve for θ 2 θ 3 solve for θ 4 (Equilvalence relation for adding angles from z0) ɸ θ 4 o1

59 solve for θ 1 solve for θ 3 ɸ θ 4 L4 o3 L3 θ 3 ɣ L2 solve for θ 2 Δz β Ψ θ 2 Δr o1 solve for θ 4 (Addition of angles in arm plane starting from z0)

60 Why Closed Form? Advantages Speed: IK solution computed in constant time Predictability: consistency in selecting satisfying IK solution Disadvantage Generality: general form for arbitrary kinematics difficult to express

61 Iterative Solutions to IK Minimize error between current endeffector and its desired position Start Transform desired endeffector velocity into configuration space Repeatedly step to convergence at desired endeffector position Goal 5-link planar arm

62 Next Class IK as an optimization problem Gradient descent optimization Manipulator Jacobian as the derivative of configuration Advanced: IK by Cyclic Coordinate Descent

63 autorob.github.io Inverse Kinematics: Manipulator Jacobian