Add new insert function which can specify the initial joint angle