Done smooth movement (Robot & Arm, not gripper yet)