Added matlab function

Manipulator Dynamics, Etc.