Generate 3D robot path using target coordinates
Generate 3D robot trajectory avoiding obstacles
Plot 6-DOF robot trajectory avoiding obstacles
