[Python] Determine the object distance from the ground given head pitch angle?

I want to determine the distance from a detected object (as spotted by a robot/camera) to the ground through the head pitch angle and the distance between the object and the robot/camera (which is a known fixed distance).

I want to do that because I want the robot to grab the object at the requested specific height using both arms used straight and perpendicular to the robot's TORSO. The head pitch angle range is as follows:



I consider the object to be "grabbable" only when the head pitch angle is positive. It goes without saying that is obvious that the object-ground distance is camera/robot-ground distance if the head pitch angle is 0°. The case I want to consider is when there's a certain positive head pitch angle at which the robot is looking at the detected object as explained by the following illustration:



my try to solve this is through the following in python:

distance_object_ground = self.DISTANCE_TORSO_OBJECT * math.tan(
head_pitch_angle)
DISTANCE_TORSO_OBJECT = is the distance between the robot to the object which is horizontal


is this an overall good approach to grabbing the object, if yes, is the distance from the object to the ground calculation accurate?

Continue reading...