Jane Li. Assistant Professor Mechanical Engineering Department, Robotic Engineering Program Worcester Polytechnic Institute

Transcription

1 Jane Li Assistant Professor Mechanical Engineering Department, Robotic Engineering Program Worcester Polytechnic Institute

2 (3 pts) Compare the testing methods for testing path segment and finding first collision Compare the non-holonomic RRT with holonomic RRT: given a new node to connect to, (3 pts) how to extend toward this node? (3 pts) how to connect to this node for the last step? RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/2018 2

3 PRM planning Detect collision as quickly as possible Bisection strategy Physical simulation, haptic interaction Find first collision Sequential strategy RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/2018 3

4 Apply motion primitives (i.e. simple actions) at q near q f ( q, u) --- use action u from q to arrive at q arg min( d( q, q )) chose u * rand Holonomic RRT q new Non-Holonomic RRT q 1 q 2 q new = q 3 q init q near q rand You probably won t reach q rand by doing this Key point: No problem, you re still exploring! u* q near q rand RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/2018 4

5 q rand q near How to bridge between the two points? RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/2018 5

6 Shoot out trajectories in different directions until a trajectory of the desired boundary value is found. System Boundary condition RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/2018 6

7

8 We have learned the planning algorithms that can generalize across many types of robots Discrete planning Sampling-based planning Theoretically, we should be all set. However When it comes to manipulator robots, we may have to handle an application-specific problem

9 RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/2018 9

10 RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

11 RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

12 How to resolve the kinematic redundancy? How to coordinate macro- and micro-structures? Arm-hand structure Body-arm structure How to handle bimanual coordination? RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

13 How to resolve the kinematic redundancy? Solution to Inverse kinematics Pseudo-inverse Additional constraints and optimization criteria RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

14 Robot Joint Angles Forward Kinematics Inverse Kinematics End Effector Pose RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

15 If N>M, FK maps a continuum of configurations to one end-effector pose: If N=M, FK C-space Task space FK maps a finite number of configurations to one end-effector pose: If N<M, Target pose not reachable C-space FK Task space RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

16 Direct kinematics First-order differential kinematics Jacobian Second-order differential kinematics RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

17 Our primary concern is the end-effector pose in task space IK solver needs to compute a C-space motion that does the right thing in task space RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

18 Direct kinematics IK solution Analytical solution robot geometry Algebraic solution homogeneous transformation matrices RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

19 RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

20 RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

21 RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

22 Do not care about the redundant DOFs motion Standard IK solvers, using pseudo-inverse Utilize redundant DOFs to handle additional constraints Obstacle Utilize redundant DOFs to optimize performance What are the performance indices? RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

23 First-order differential kinematics IK solution Inverse the Jacobian (non-redundant manipulator) Pseudo-inverse of Jacobian RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

24 Start with Forward Kinematics function x FK(q) Take the derivative with respect to time: dx d[ FK( q)] dfk( q) dt dt dq Now we get the standard Jacobian equation: dx dt RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/ dq dt dq dfk( q) J( q) J ( q) dt dq

25 RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

26 If N=M, Jacobian is square Standard matrix inverse If N>M, Pseudo-Inverse Weighted Pseudo-Inverse Damped least squares RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

27 Pseudo-inverse matrix The unique matrix satisfying the Moore Penrose conditions For redundant manipulator RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

28 Pseudo-Inverse specifies a unique solution for inverse kinematics Implicitly, it performs the following optimization Minimize, given RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

29 Multiply weighting coefficient matrix to Pseudo-Inverse Jacobian where Optimization? Minimize, given RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

30 How to choose the weighting coefficient matrix? W>0 and symmetric Large weight small joint velocity Weights ~ inverse of the joint angle range RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

31 Singular Value Decomposition (SVD) RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

32 RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

33 where where RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

34 Manipulability index Jacobian matrix determinant Which is indeed Is it a good measurement? RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

35 Manipulability index condition number Alternatively, can use isotropy Range? Is it good enough? RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

36 Manipulability index the smallest singular value Direction of velocity disadvantage Is it good enough? RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

37 Manipulability index What does it imply? Manipulability of every sub-manipulator (non-redundant) RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

38 To render robust behavior when crossing the singularity, we can add a small constant along the diagonal of (J(q) T J(q)) to make it invertible when it is singular RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

39 The matrix will be invertible but this technique introduces a small inaccuracy RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

40 Induced error by damped least squares Choice of the damping factor As a function the minimum singular value measure of distance to singularity Induce the damping only/mostly in the non-feasible direction of the task RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

41 Project a task space velocity vector into the null-space Primary task Additional constraint Secondary task Full rank square Jacobian Invertible! where RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

42 Secondary tasks is satisfied in the null-space of the Jacobian pseudo-inverse In linear algebra, the null-space of a matrix A is the set of vectors V such that, for any v in V, 0 = A T v. V is orthogonal to the range of A range of A y V x V range of A 2D example 3D example RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

43 Given the null space of Jacobian, the secondary task will not disturb the primary task The null-space projection matrix for the Jacobian pseudoinverse is: RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

44 Project a task space velocity vector into the null-space range of J(q) + secondary task v range of N(q) x Primary task Secondary task N ( q) v RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

45 The null-space is often used to push IK solvers away from Joint limits, obstacles How to define the secondary task for the constraints in both task and joint space? Primary task Secondary task RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

46 For non-linear systems, magnitude differences in primary and secondary can cause numerical problems One can overwhelm the other when you normalize later Introduce a normalization factor Conflicts with primary? Primary task Secondary task RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

47 What if you have three or more tasks? The i-th task is: The i-th null-space is: The recursive null-space formula is then: RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

48 Second-order differential kinematics IK solution is an arbitrary joint-space acceleration RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

49 Choose We have Minimum-norm acceleration solution RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

50 Chapter 10 Redundant Robots in Handbook of Robotics, 2 nd ed RBE 550 Motion Planning Instructor: Jane Li, Mechanical Engineering Department & Robotic Engineering Program - WPI 3/22/

51