Smooth Point-to-Point Trajectory Planning in $SE$ (3)with Self-Collision and Joint Constraints Avoidance.