imporoved method for calculating robot_pose -Mac-I