A Time-Optimal Intersection Search Algorithm for Robot Grasping

Algorithm 1

The pyramid-shaped time optimal search algorithm.

	In step 1, set k = 1, and select . The arrival time t1 is obtained according to inverse kinematics, and is calculated via the equation,  = c1 + c2 (c1, c2 R).
	In step 2, if , the algorithm is stopped and the intersection point does not exist; if , go to step 3.
	In step 3, calculate .
	In step 4, if , set , and stop the algorithm; otherwise skip to step 5.
	In step 5, set , and is calculated; then go to step 3.