Add Trajectory class

[sea-bass]

Adds a new Trajectory class that replaces the (t_pts, x_pts, y_pts, yaw_pts) tuple that was being used before.

Closes #135.

[github-actions]

Coverage Report

File	Stmts	Miss	Cover	Missing
__init__.py	0	0	100%
core
__init__.py	9	0	100%
dynamics.py	43	0	100%
gazebo.py	136	8	94%	55, 124, 230, 244–248
hallway.py	73	10	86%	59, 61, 63, 96, 158, 196–199, 205
locations.py	126	21	83%	53, 55, 69, 84–85, 87, 98–99, 101–102, 119, 177, 192, 217, 227, 273, 284–285, 287–288, 312
objects.py	69	4	94%	78, 106, 137, 197
robot.py	214	58	72%	140–141, 203, 216, 263, 277–278, 282, 286, 292–293, 295–296, 313–314, 342, 344–345, 366–367, 370–373, 376–378, 403, 407–409, 411, 413–416, 429–431, 435, 437–439, 441, 444–445, 448, 470–471, 479, 484–489, 492, 505
room.py	48	1	97%	113
world.py	571	138	75%	146–147, 183–184, 188, 241–242, 244–245, 273–274, 325, 339–340, 342–343, 372–376, 378–380, 382–383, 386, 390–394, 397–399, 402–409, 440, 476–477, 480, 496, 526–527, 538, 541, 563–567, 569–571, 574–575, 578–581, 583, 586, 588–590, 592–594, 596, 615, 625–629, 664–665, 668–671, 688–689, 695–696, 698–699, 701, 703–704, 706, 708–709, 719–720, 723, 725, 739, 741, 748, 795, 826–827, 846–847, 851–852, 887–888, 890, 907–908, 927–931, 943–946, 958–961, 986–987, 989, 1033–1034, 1166–1167
yaml_utils.py	106	11	89%	64, 68, 152–154, 158–159, 182–183, 207–208
gui
__init__.py	2	0	100%
main.py	152	29	80%	22, 24–26, 28, 49, 178–181, 188–191, 193, 196–202, 206–208, 212–213, 219, 230
world_canvas.py	226	52	76%	34–36, 110, 112, 114, 177, 181, 232–237, 243–244, 246–248, 253–254, 257, 260–262, 265, 270–271, 273, 300, 322–327, 330, 333–334, 349–351, 354, 371, 400, 418–419, 421–422, 464, 480, 487
manipulation
__init__.py	1	0	100%
grasping.py	236	61	74%	102, 110–112, 114–122, 124, 132, 462, 464–466, 470–472, 566–568, 571–575, 583, 586–588, 593–594, 600–604, 607–614, 617–619, 628, 630, 632–638
navigation
__init__.py	3	0	100%
a_star.py	78	10	87%	67–72, 74–76, 165
execution.py	34	4	88%	46–47, 49–50
occupancy_grid.py	139	14	89%	44–47, 49–53, 56, 192, 226, 234, 238
path_planner.py	26	6	76%	32, 35, 37, 41, 84, 89
planner_base.py	59	17	71%	29–35, 51, 57–59, 140–145
prm.py	72	56	22%	40–44, 46, 50–51, 54, 57–64, 66–67, 76–79, 85, 101–102, 104–109, 111–112, 115–118, 121–126, 135, 144, 154, 156, 158–159, 161, 174–178
rrt.py	172	7	95%	228, 353–354, 360–361, 363, 392
world_graph.py	70	3	95%	118, 121, 145
planning
__init__.py	0	0	100%
actions.py	66	43	34%	59–60, 62, 66–75, 77–80, 82–86, 88–91, 93–97, 99–100, 102–104, 150–151, 154–158
planning/pddlstream
__init__.py	7	1	85%	13
default_mappings.py	9	0	100%
planner.py	40	8	80%	119–126
primitives.py	57	2	96%	68, 208
utils.py	85	3	96%	66, 99–100
utils
__init__.py	0	0	100%
general.py	33	4	87%	23, 45, 51–52
knowledge.py	139	16	88%	90–91, 100, 135–138, 145, 149–150, 163, 178, 182, 224, 240, 295
motion.py	60	6	90%	56, 67–71
polygon.py	111	0	100%
pose.py	89	3	96%	79, 211, 227
search_graph.py	93	3	96%	241–243
trajectory.py	65	0	100%
TOTAL	3519	599	82%

Tests	Skipped	Failures	Errors	Time
168	0 💤	0 ❌	0 🔥	3m 19s ⏱️

[eholum]

Giving me work! Also check out that sweet, sweet cached docker build.

Took a quick skim and added some small thoughts. Nothing too important. I haven't checked anything locally... yet. This will encourage me to finally setup my new computer.

[eholum]

I think this is six of one, half dozen of the other, but if you stored a list of something more akin to StampedPose (ugh) it would simplify the code slightly. You could just add a time value to Pose and then only use it in trajectories. Then I think the only place that you would need to do conversions to/from np arrays is in interpolate, and you could delete a bunch of these kinds of statements and those on line 37. It's still the same amount of converting back and forth though...

Though idk how you feel about adding a time value to Pose. Also this is just fine so might not be worth changing.

[sea-bass]

this seems to work well, actually. I might have been overthinking how bad that was gonna be. Thanks!

As a huge bonus, storing as poses lets me do the interpolation for a generic SO(3) trajectory -- not that pyrobosim is using that right now, but if it ever does we're all set here 💪🏻

[eholum]

I can still bring my apple to the bathroom, and that's all that matters.