-
Notifications
You must be signed in to change notification settings - Fork 3
Implement local fixed-frame Trajectory #41
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Draft
jtodev
wants to merge
11
commits into
main
Choose a base branch
from
trajectory-fixed-frame
base: main
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Draft
Changes from 4 commits
Commits
Show all changes
11 commits
Select commit
Hold shift + click to select a range
1f35c68
Implement fixed-frame trajectory
jtodev df6a669
Merge branch 'main' into trajectory-fixed-frame
jtodev 1dcd3ca
Fix trajectory property metadata
jtodev 3202220
Fix trajectory doctests
jtodev 82f7126
Apply suggestions from code review
jtodev a71ec6b
Validate trajectory spline inputs
jtodev fd4be0d
Separate trajectory interface from fixed implementation
jtodev 97948cb
Use offset shifts for static trajectory array
jtodev 01debff
Update src/scene_synthesis/trajectory.py
jtodev dcd2c6f
Use local trajectory implementation in fly-by example
jtodev 86b307c
Trim trajectory file docstring
jtodev File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Some comments aren't visible on the classic Files Changed page.
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,125 @@ | ||
| """Trajectory definition with a fixed frame of reference.""" | ||
|
|
||
| import numpy as np | ||
| from scipy.interpolate import splev, splprep | ||
| from traits.api import Dict, Float, HasStrictTraits, Property, Tuple, cached_property, property_depends_on | ||
|
|
||
|
|
||
| class Trajectory(HasStrictTraits): | ||
| """Represent a source trajectory in a fixed frame of reference. | ||
|
|
||
| The trajectory is specified by a mapping from time instants to sampled | ||
| ``(x, y, z)`` positions. A spline is fit through those samples and can | ||
| then be evaluated at arbitrary times to obtain positions or time | ||
| derivatives such as velocity. | ||
|
|
||
| Notes | ||
| ----- | ||
| - The spline order is chosen automatically based on the number of | ||
| available points, up to cubic interpolation. | ||
| - The frame of reference is fixed, i.e. the sampled positions are | ||
| interpreted directly as global coordinates. | ||
|
|
||
| Examples | ||
| -------- | ||
| >>> import scene_synthesis as ss | ||
| >>> trajectory = ss.Trajectory(points={0.0: (0.0, 0.0, 0.0), 1.0: (1.0, 0.0, 0.0)}) | ||
| >>> trajectory.location(0.5) | ||
| [array(0.5), array(0.), array(0.)] | ||
| """ | ||
|
|
||
| #: Dictionary mapping time instants to sampled ``(x, y, z)`` positions. | ||
| points = Dict( | ||
| key_trait=Float, | ||
| value_trait=Tuple(Float, Float, Float), | ||
| ) | ||
|
|
||
| #: Start and end time of the trajectory as ``(t_min, t_max)``. | ||
| interval = Property() | ||
|
|
||
| #: Internal spline representation returned by :func:`scipy.interpolate.splprep`. | ||
| tck = Property() | ||
|
|
||
| @property_depends_on(['points[]']) | ||
| def _get_interval(self): | ||
|
jtodev marked this conversation as resolved.
|
||
| return np.sort(list(self.points.keys()))[np.r_[0, -1]] | ||
|
|
||
| @cached_property | ||
| @property_depends_on(['points[]']) | ||
| def _get_tck(self): | ||
| t = np.sort(list(self.points.keys())) | ||
| xp = np.array([self.points[i] for i in t]).T | ||
| k = min(3, len(self.points) - 1) | ||
| tcku = splprep(xp, u=t, s=0, k=k) | ||
| return tcku[0] | ||
|
jtodev marked this conversation as resolved.
|
||
|
|
||
| def location(self, t, der=0): | ||
| """Evaluate the trajectory or one of its derivatives. | ||
|
|
||
| Parameters | ||
| ---------- | ||
| t : float or array-like of float | ||
| Time instant or time instants at which to evaluate the trajectory. | ||
| der : int, optional | ||
| Derivative order. Use ``0`` for position, ``1`` for velocity, | ||
| ``2`` for acceleration, and so on. Defaults to ``0``. | ||
|
|
||
| Returns | ||
| ------- | ||
| list[numpy.ndarray] | ||
| Three arrays representing the ``x``, ``y``, and ``z`` components | ||
| at the requested times. | ||
|
|
||
| Examples | ||
| -------- | ||
| >>> import scene_synthesis as ss | ||
| >>> trajectory = ss.Trajectory(points={0.0: (0.0, 0.0, 0.0), 1.0: (1.0, 0.0, 0.0)}) | ||
| >>> trajectory.location(0.5) | ||
| [array(0.5), array(0.), array(0.)] | ||
| """ | ||
| return splev(t, self.tck, der) | ||
|
|
||
| def traj(self, t_start, t_end=None, delta_t=None, der=0): | ||
| """Iterate through trajectory samples over a time range. | ||
|
|
||
| Parameters | ||
| ---------- | ||
| t_start : float | ||
| Start time of the iteration. If ``delta_t`` is omitted, this value | ||
| is interpreted as the step size and the full trajectory interval is | ||
| used. | ||
| t_end : float, optional | ||
| End time of the iteration. Defaults to the end of | ||
| :attr:`interval`. | ||
| delta_t : float, optional | ||
| Time step between yielded samples. If omitted, ``t_start`` is used | ||
| as the step size for traversing the full trajectory interval. | ||
| der : int, optional | ||
| Derivative order to evaluate. Defaults to ``0``. | ||
|
|
||
| Yields | ||
| ------ | ||
| tuple[numpy.float64, numpy.float64, numpy.float64] | ||
| The interpolated ``(x, y, z)`` values at each sampled time. | ||
|
|
||
| Examples | ||
| -------- | ||
| >>> import scene_synthesis as ss | ||
| >>> trajectory = ss.Trajectory(points={0.0: (0.0, 0.0, 0.0), 1.0: (1.0, 0.0, 0.0)}) | ||
| >>> samples = list(trajectory.traj(0.5)) | ||
| >>> samples[0] | ||
| (np.float64(0.0), np.float64(0.0), np.float64(0.0)) | ||
| >>> samples[1] | ||
| (np.float64(0.5), np.float64(0.0), np.float64(0.0)) | ||
| >>> samples = list(trajectory.traj(0.0, 1.0, 0.5)) | ||
| >>> samples[0] | ||
| (np.float64(0.0), np.float64(0.0), np.float64(0.0)) | ||
| >>> samples[1] | ||
| (np.float64(0.5), np.float64(0.0), np.float64(0.0)) | ||
| """ | ||
| if not delta_t: | ||
| delta_t = t_start | ||
| t_start, t_end = self.interval | ||
| if not t_end: | ||
|
jtodev marked this conversation as resolved.
Outdated
|
||
| t_end = self.interval[1] | ||
| yield from zip(*self.location(np.arange(t_start, t_end, delta_t), der), strict=True) | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,31 @@ | ||
| """Unit tests for the scene-synthesis trajectory class.""" | ||
|
|
||
| import numpy as np | ||
| import scene_synthesis as ss | ||
|
|
||
|
|
||
| def test_trajectory_interval_uses_point_bounds(): | ||
| """``Trajectory.interval`` should expose the first and last point times.""" | ||
| trajectory = ss.Trajectory(points={0.5: (0.0, 0.0, 0.0), 2.0: (1.0, 0.0, 0.0), 1.0: (0.5, 0.0, 0.0)}) | ||
|
|
||
| np.testing.assert_allclose(trajectory.interval, np.array([0.5, 2.0])) | ||
|
|
||
|
|
||
| def test_trajectory_location_matches_sampled_points(): | ||
| """``Trajectory.location`` should pass through the sampled points.""" | ||
| trajectory = ss.Trajectory(points={0.0: (0.0, 0.0, 0.0), 1.0: (1.0, 2.0, 0.0), 2.0: (2.0, 4.0, 0.0)}) | ||
|
|
||
| location = np.array(trajectory.location(1.0)) | ||
|
|
||
| np.testing.assert_allclose(location, np.array([1.0, 2.0, 0.0])) | ||
|
|
||
|
|
||
| def test_trajectory_traj_iterates_over_requested_range(): | ||
| """``Trajectory.traj`` should iterate over positions with the requested step size.""" | ||
| trajectory = ss.Trajectory(points={0.0: (0.0, 0.0, 0.0), 1.0: (1.0, 0.0, 0.0)}) | ||
|
|
||
| samples = list(trajectory.traj(0.0, 1.0, 0.25)) | ||
|
|
||
| assert len(samples) == 4 | ||
| np.testing.assert_allclose(samples[0], (0.0, 0.0, 0.0)) | ||
| np.testing.assert_allclose(samples[-1], (0.75, 0.0, 0.0)) |
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Uh oh!
There was an error while loading. Please reload this page.