Kinematics

The diagram represents a robot leg. I have added this picture just to help visualise the robot leg. Section wise explanation has been added in the Code. The code is modular, you can add more DOF to expand it.