add joints to config file and increase wait time when moving to initial joint configuration