Renamed Angle to Orientation a constructor refactoring
updated ForwardKinematicsResult
fixed result for theta1
updated setValue for Matrix to be immutable
updated implementation with simple fixes