Implemented CRobotArm Class as in Matlab code