Modeling and simulation for robots and mechanisms