2 Dof Robot Arm Matlab