Path Planning for Manipulator Arm

As part of my Ph.D. requirements, I worked as a TA for the Introduction to Robotics course at the University of Pennsylvania. I helped teach the class in the Fall 2018 semester and the Fall 2019 semester, both times under Dr. Cynthia Sung. The course introduces senior-level undergraduates and first-year graduate students to the basic fundamentals of robotics, starting with forward and inverse kinematics and moving through path-planning and dynamics. The course is taught with lab exercises performed on a Lynxmotion 5-DOF manipulator arm, though the ideas are applicable to mobile robots and more complex manipulators as well.

As the lead TA for the course I developed solution code for the various lab exercises we assigned the students, which gave me the chance to explore several algorithmic approaches to path planning for a manipulator arm. As the course is introductory, our assignments did not require efficient or fast code; nevertheless, the solution code needed to be both fast and efficient to give the students a sense of what their own algorithms could perform like. It also needed to be as robust as possible to stand up to abuse by the students, and to prevent unfair use.

A*

The A* implementation I developed was an adaptation of the algorithm I wrote for quadrotors. The Lynx is a 5-DOF manipulator, but its configuration can be simplified to 3D by making conservative assumptions about the dimensions of the robot and not actuating the last two joints, which control the orientation of the gripper. This means that the algorithm is no longer guaranteed to find a solution if one exists for the full 5-DOF manipulator, but allows an easy extension of existing code.

The challenge for a graph-search algorithm on a manipulator, however, is properly classifying the configuration space and generating an occupancy grid. We use a simple function that detects the intersection between a line and an axis-aligned bounding-box to check for the collision of any single link of the manipulator with an individual obstacle. This implementation is heavily optimized for MATLAB, which operates most efficiently on vectors, so instead of looping over the space of possible joint combinations we generate the set of angles and check all of them at once. This results in a fast and efficient algorithm.

Use of this A* solution code presented the following questions to the students:

1. By what logic can we simplify a 5-DOF manipulator to a 3-DOF manipulator for modeling purposes? (We did not enforce that the students’ code used 3-DOFs)

2. What assumptions/risks do you make by using the line/point model of the robot? Is this model too conservative or too liberal? (We did not prescribe a specific method of collision detection for the students)

RRT

I also developed an implementation of the Rapidly Exploring Random Trees algorithm [1] as an example of a probabilistic planner that doesn’t require processing of the full configuration space. Just like the A*, I aimed to make the algorithm as simple and efficient as possible to facilitate student interactions with it. By planning out my approach (pun intended), I was able to design the algorithm simply without using any complex tree representations. Defining a function:

allows the RRT algorithm to simply be written as follows:

Use of this RRT code presented the students with the following questions:

1. What advantages/disadvantages do you gain by using a probabilistic planner?

2. Why do the paths generated look so strange?

References

[1] S. M.Lavalle, “Rapidly-Exploring Random Trees: A New Tool for Path Planning,” 1998.