Motion Planning with RRT Connect

Problem

Consider a planar robot $R$ manipulating objects in a 2D workspace. The robot has four links $l_0, l_1, l_2, l_3$ ($l_0$ is the base link) with the assumption that each joint is in the range of $[-\pi/2,\pi/2]$. Each pose in the workspace is represented with the format $(x,y,\alpha)$. Based on the assumptions, the robot is tasked to find a motion sequence $\Pi$ moving a target object $T$ from a start pose $S_T$ to a goal pose $S_G$. Each motion in $\Pi$ is required to be collision-free with other boxes in the environment.

Get Started

First, install necessary libraries: (1) nlohmann-json for reading configuration files; (2) opencv4 for visualization.

$ sudo apt-get update
$ sudo apt-get install libopencv-dev
$ sudo apt-get install nlohmann-json3-dev

Second, change the parameters in the ``config.json'' file as needed. Current supported parameters include (1) the start and goal pose of the target object; (2) Poses of obstacles in the environment, (3) the length and width of a link; (4) the width and height of an obstacle/target object; (5) Clearance to obstacles.

Third, build the project with "make" command.

$ cd /path/to/repo/root/
$ make

Fourth, run the executable with

$ ./motion_planning

Fifth, when the planner finds a path, it plots an image of the initial state.

Sixth, press any key to start the animation. Some demo videos are attached in the zip file.

Technical Details

Basic Version

The motion plan is computed by RRT-connect. IK is chosen to be the analytical solution with $q_2\geq 0$.

Clearance Version (lastest)

In the basic version, RRT state feasibility is evaluated with polygon intersections. In this version, I evaluate polygon distance instead. A state is feasible if and only if all the robot links and the target object is at least $d$ away from obstacles, where $d$ is the defined clearance.

Video Explanation

1. "basic-version-easy.mp4". The basic version planner in an easy test case.
2. "basic-version-narrow.mp4". The basic version planner in a challenging test case.
3. "clearance-version-5cm.mp4" Clearance version implementation with clearance=5cm.
4. "clearance-version-10cm.mp4" Clearance version implementation with clearance=10cm.