Shahriar,
For this application you should use Wolfram's SystemModeler. It is more appropriate for the task. You can also get an animation if you use the correct components. You can also integrate a controller written in C into a model. I have seen references to people who have called python from modelica (the language underneath SystemModeler) but I have never tried it.
Here is an example of a robot model in WSM: robot model example
Regards,
Neil