Trajectory Optimization Using Sequential Convex Programming with Collision Avoidance