Hi,
The code looks fine. The only thing I noticed was that the Z value of your home position is very high, 569.5
I'm thinking that your model isn't in the correct units. If you would like me to take a look at it you could email the model to
lashkari@worldviz.com