I have written the same code at least four times now April 10, 2017 code dh parameters forward kinematics matlab +