Determine if a point(tcp) is inside a region(box)

General Application Help: How do I?
1

I am working on an error recovery procedure on how to get the robot to move out of a box safely. On an open with no obstacle application, I move up to a safe level and then move to my safe pose. In my current application, I have many holes/boxes that the tool/tcp is inside a hole. When I power up the system, I need to determine the box that I am in to move through a path back to my safe and ready pose. Currently, I am using a bunch of if-then to do this. My system is UR5e. Has anyone done something similar?

Thanks,

2

I always have a bunch of If statements, too, when making a safe home move. I don’t think there’s a better way to do it yet.

If you have a lot of boxes, then maybe you could use the pose_dist which check the distance between two poses. Then use the robot’s current TCP position compared to (maybe?) each box’ center, and determine which box like this?
Still requires some If statements, but I reckon it should be easier.