Search code examples
pythonopencvimage-processingcomputer-visionrobotics

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:

HeadPitch Angle

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:

head pitch angle

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?

Solution

  • If your distances and angles are being reliably measured, then yes, this does seem like a solid approach to grabbing the object. Your math is almost correct, however it seems the provided code finds the distance from the object to the head of the robot rather than the ground. To find the distance to the ground, subtract the distance you just got from the object to the head of the robot from the height of the head on the robot. I've included a diagram to better articulate what I mean. The formula that you used calculates the Head-Object Distance in blue, and if your goal is the Object_Ground_Distance in green, then you have to subtract the HeadObject Distance from the Camara/Robot-Ground Distance in orange.