Motion Model Working

It works for a single body, need to add support for multiple bodies.