Allow specifying relative poses

[sea-bass]

This PR implements a mechanism to express poses relative to other entities.

Closes #298

[sea-bass]

@kumar-sanjeeev Since I've been giving you a hard time with reviews, I'd like to give you a chance to do the same for this one, if you'd like.

There are still lots more cases to handle, but initially I'm curious what you think of this way to express relative poses, both in the programmatic and YAML interfaces.

The other open question is what the "reference pose" is defined as.

• For Locations, Objects, and Robots, I think it's okay.
• For Rooms and Object Spawns, it's currently just the centroid of the polygon... which probably needs to change.
• For Hallways ...? Maybe we don't support that now.

Perhaps the "offset" of the polygon footprint will need to be used more faithfully than it currently is, but TBD.

[github-actions]

Coverage Report

File	Stmts	Miss	Cover	Missing
__init__.py	0	0	100%
core
__init__.py	9	0	100%
dynamics.py	49	1	97%	142
gazebo.py	137	8	94%	55, 128, 234, 248–252
hallway.py	97	8	91%	206, 248–251, 295–297
locations.py	137	17	87%	76, 78, 111, 124–125, 127–128, 145, 204, 237, 262, 272, 329–330, 332–333, 362
objects.py	73	4	94%	113, 144, 191, 219
robot.py	497	86	82%	271, 274, 286, 289, 293–294, 299–300, 340–342, 358, 375–376, 415–417, 421–425, 450, 453, 457–459, 486, 488, 530–531, 533–534, 540–541, 550–553, 558, 560, 569–571, 578–580, 642–644, 669–671, 677–678, 686–690, 735–737, 748–749, 809–811, 816, 863–865, 870, 888, 894, 897, 899, 901, 904–906, 956–957, 990, 1008, 1011
room.py	65	2	96%	74, 140
world.py	833	192	76%	163, 166, 200–201, 205, 277–278, 280, 283, 313–314, 376, 390–391, 393, 396, 411–413, 433–437, 439–441, 443–444, 447, 449–452, 456–460, 463–464, 467, 470–481, 497–498, 516–518, 532, 534, 537–538, 543–545, 564–565, 567–568, 584, 586, 589–590, 595–597, 607–609, 626–627, 629–630, 644, 646, 649–650, 655–657, 680, 682, 685–686, 691–693, 729, 735, 771–772, 775, 798, 818, 821, 832, 835, 843, 859–863, 865, 868–869, 872–875, 877, 880, 883–884, 887–889, 891–893, 895–897, 899, 914, 921, 932–936, 971–972, 976–977, 995–996, 1002–1003, 1005–1006, 1008, 1010–1011, 1013, 1015–1016, 1026–1027, 1030, 1032, 1046, 1048, 1055, 1115, 1146–1147, 1170–1171, 1175–1176, 1195–1196, 1200–1201, 1226–1227, 1280–1281, 1305, 1370, 1377, 1487–1488, 1517, 1523, 1569, 1649–1650, 1716, 1726
yaml_utils.py	150	6	96%	90, 96, 194, 197, 217, 220
gui
__init__.py	2	0	100%
action_runners.py	91	48	47%	32–33, 35–39, 41, 74–75, 77–81, 83, 109–110, 112–116, 118, 144–145, 147–151, 153, 176–177, 179–183, 185, 208–209, 211–215, 217
main.py	277	74	73%	23–25, 46, 94, 98–101, 248–257, 269–279, 286–289, 291, 294–300, 304, 309–310, 314–315, 326, 369–370, 379–380, 384–385, 397, 400–403, 405–406, 408–409, 412–415, 417–419, 423–426
world_canvas.py	263	52	80%	134–137, 141–142, 147–148, 155, 157, 159, 216–218, 224, 226, 246–248, 293, 350–351, 378–379, 381, 383–384, 386–389, 391–392, 396–402, 405, 408–409, 422, 440–441, 443, 449, 451–454
manipulation
__init__.py	1	0	100%
grasping.py	239	64	73%	100, 108, 110–112, 114–122, 124, 132, 462, 464–466, 470–472, 566–567, 569–571, 574–578, 586, 589–591, 596–597, 603–607, 610–617, 620–622, 631, 633, 635–641
navigation
__init__.py	16	2	87%	37, 54
a_star.py	80	11	86%	82–84, 87–89, 91–93, 182, 191
execution.py	96	7	92%	81–83, 88–90, 125
occupancy_grid.py	139	14	89%	45–48, 50–54, 57, 193, 227, 235, 239
prm.py	58	4	93%	59–60, 139, 148
rrt.py	163	0	100%
visualization.py	32	6	81%	88–93
world_graph.py	61	6	90%	117, 120, 130, 139, 148, 150
planning
__init__.py	0	0	100%
actions.py	112	0	100%
planning/pddlstream
__init__.py	7	1	85%	13
default_mappings.py	9	0	100%
planner.py	40	8	80%	119–126
primitives.py	61	4	93%	68, 82, 94, 232
utils.py	99	8	91%	66, 93, 105, 115–116, 207–208, 211
utils
__init__.py	0	0	100%
general.py	47	2	95%	25, 47
knowledge.py	152	20	86%	91, 122, 163–168, 170–171, 174–178, 209–210, 252, 268, 331
logging.py	16	5	68%	24–27, 56
motion.py	70	3	95%	93–94, 96
polygon.py	111	0	100%
pose.py	117	0	100%
search_graph.py	93	3	96%	239–241
trajectory.py	67	2	97%	130, 167
TOTAL	4566	668	85%

Tests	Skipped	Failures	Errors	Time
205	0 💤	0 ❌	0 🔥	3m 56s ⏱️

[kumar-sanjeeev]

@kumar-sanjeeev Since I've been giving you a hard time with reviews, I'd like to give you a chance to do the same for this one, if you'd like.

There are still lots more cases to handle, but initially I'm curious what you think of this way to express relative poses, both in the programmatic and YAML interfaces.

The other open question is what the "reference pose" is defined as.

• For Locations, Objects, and Robots, I think it's okay.
• For Rooms and Object Spawns, it's currently just the centroid of the polygon... which probably needs to change.
• For Hallways ...? Maybe we don't support that now.

Perhaps the "offset" of the polygon footprint will need to be used more faithfully than it currently is, but TBD.

@sea-bass thank you for giving me this chance :). I will get back to you.

[kumar-sanjeeev]

@kumar-sanjeeev Since I've been giving you a hard time with reviews, I'd like to give you a chance to do the same for this one, if you'd like.

There are still lots more cases to handle, but initially I'm curious what you think of this way to express relative poses, both in the programmatic and YAML interfaces.

The other open question is what the "reference pose" is defined as.

• For Locations, Objects, and Robots, I think it's okay.
• For Rooms and Object Spawns, it's currently just the centroid of the polygon... which probably needs to change.
• For Hallways ...? Maybe we don't support that now.

Perhaps the "offset" of the polygon footprint will need to be used more faithfully than it currently is, but TBD.

I like the thought process behind relative poses. I have one question that comes to mind, after obtaining the absolute pose of an entity using world.get_pose_relative_to(), should we also need to verify if that pose is valid, as a safety check? Valid I mean to say collision checking with other entities.

[sea-bass]

I like the thought process behind relative poses. I have one question that comes to mind, after obtaining the absolute pose of an entity using world.get_pose_relative_to(), should we also need to verify if that pose is valid, as a safety check? Valid I mean to say collision checking with other entities.

Yes it should, but this already happens when you try to add the entity with the transformed pose -- not when you do the transformation.

[kumar-sanjeeev]

I like the thought process behind relative poses. I have one question that comes to mind, after obtaining the absolute pose of an entity using world.get_pose_relative_to(), should we also need to verify if that pose is valid, as a safety check? Valid I mean to say collision checking with other entities.

Yes it should, but this already happens when you try to add the entity with the transformed pose -- not when you do the transformation.

Okay, understood. At the moment, I don't see any significant challenges in implementing the suggested logic for relative pose.

[sea-bass]

@kumar-sanjeeev this is ready for review!

[kumar-sanjeeev]

Noice!

LGTM. Just a few basic comments for improving readability:

I was wondering if it would make sense to mention at the top of the World schema:
"Refer to the Specifying Poses section below to better understand how poses can be specified in PyRoboSim."

This addition could help users quickly locate the relevant section on poses when they are reading the YAML file.

[kumar-sanjeeev]

does it make sense to put this function in pose.py ?

[sea-bass]

I wanted to do this, but it felt strange to have to pass in a world in a function in pose.py. If we were using Python type hints, then this would actually cause a circular import of World.

Perhaps a better candidate might be pyrobosim/utils/knowledge.py? All the other functions in this file already accept worlds, so this file you definitely need to have World imported first.

---

It's the same reason World.get_pose_relative_to() wasn't just a free function in pose.py that accepted a World object as input. This one is even worse because it has many other imports in the body of this function (including Room, Location, etc.) and would cause several circular imports even without type hints.

I think at some point the world.py file (currently 1740 lines of code) will need to be split up into more utilities, and this would be one of them when that time comes.

[kumar-sanjeeev]

I understand. Let’s keep it as is for now. I remember when I was working on the type hints issue, I encountered a lot of circular import errors.

[kumar-sanjeeev]

Perfect !

I also tested the changes locally. The examples seem to be working fine, and the docs and test cases are running fine as well.