modified forward kinematics algorithm
added kinematic check in main and fixed bug with forward kinematics