Public Member Functions | |
def | __init__ (self) |
def | generate_pnt (self, s, t, args) |
def | generate_pos (self, s) |
def | generate_quat (self, s) |
def | get_samples (self, max_time, step=0.001) |
def | init_interpolator (self) |
def | set_parameters (self, params) |
Static Public Attributes | |
string | LABEL = 'linear' |
Private Attributes | |
_duration | |
_heading_spline | |
_interp_fcns | |
_last_rot | |
_marker_id | |
_markers_msg | |
_s | |
_segment_to_wp_map | |
_start_time | |
Simple interpolator that generates a parametric line connecting the input waypoints. > *Example* ```python from uuv_waypoints import Waypoint, WaypointSet from uuv_trajectory_generator import LinearInterpolator # Some sample 3D points q_x = [0, 1, 2, 4, 5, 6] q_y = [0, 2, 3, 3, 2, 0] q_z = [0, 1, 0, 0, 2, 2] q = np.vstack((q_x, q_y, q_z)).T # Create waypoint set waypoints = WaypointSet() for i in range(q.shape[0]): waypoints.add_waypoint(Waypoint(q[i, 0], q[i, 1], q[i, 2], max_forward_speed=0.5)) interpolator = LinearInterpolator() interpolator.init_waypoints(waypoints) interpolator.init_interpolator() # Use get_samples to retrieve points interpolated # using a fixed step, step being represented in the line's # parametric space pnts = interpolator.get_samples(max_time=None, step=0.01) # Or use the following to retrieve a position vector on the # set of lines pos = interpolator.generate_pos(s=0.2) ```
Definition at line 30 of file linear_interpolator.py.
def uuv_trajectory_generator.path_generator.linear_interpolator.LinearInterpolator.__init__ | ( | self | ) |
Definition at line 68 of file linear_interpolator.py.
def uuv_trajectory_generator.path_generator.linear_interpolator.LinearInterpolator.generate_pnt | ( | self, | |
s, | |||
t, | |||
args | |||
) |
Compute a point that belongs to the path on the interpolated space related to `s`, `s` being represented in the curve's parametric space. > *Input arguments* * `s` (*type:* `float`): Curve's parametric input expressed in the interval of [0, 1] * `t` (*type:* `float`): Trajectory point's timestamp > *Returns* `uuv_trajectory_generator.TrajectoryPoint` including position and quaternion vectors.
Definition at line 178 of file linear_interpolator.py.
def uuv_trajectory_generator.path_generator.linear_interpolator.LinearInterpolator.generate_pos | ( | self, | |
s | |||
) |
Generate a position vector for the path sampled point interpolated on the position related to `s`, `s` being represented in the curve's parametric space. > *Input arguments* * `s` (*type:* `float`): Curve's parametric input expressed in the interval of [0, 1] > *Returns* 3D position vector as a `numpy.array`.
Definition at line 153 of file linear_interpolator.py.
def uuv_trajectory_generator.path_generator.linear_interpolator.LinearInterpolator.generate_quat | ( | self, | |
s | |||
) |
Compute the quaternion of the path reference for a interpolated point related to `s`, `s` being represented in the curve's parametric space. The quaternion is computed assuming the heading follows the direction of the path towards the target. Roll and pitch can also be computed in case the `full_dof` is set to `True`. > *Input arguments* * `s` (*type:* `float`): Curve's parametric input expressed in the interval of [0, 1] > *Returns* Rotation quaternion as a `numpy.array` as `(x, y, z, w)`
Definition at line 203 of file linear_interpolator.py.
def uuv_trajectory_generator.path_generator.linear_interpolator.LinearInterpolator.get_samples | ( | self, | |
max_time, | |||
step = 0.001 |
|||
) |
Sample the full path for position and quaternion vectors. `step` is represented in the path's parametric space. > *Input arguments* * `step` (*type:* `float`, *default:* `0.001`): Parameter description > *Returns* List of `uuv_trajectory_generator.TrajectoryPoint`.
Definition at line 127 of file linear_interpolator.py.
def uuv_trajectory_generator.path_generator.linear_interpolator.LinearInterpolator.init_interpolator | ( | self | ) |
Initialize the interpolator. To have the path segments generated, `init_waypoints()` must be called beforehand by providing a set of waypoints as `uuv_waypoints.WaypointSet` type. > *Returns* `True` if the path segments were successfully generated.
Definition at line 78 of file linear_interpolator.py.
def uuv_trajectory_generator.path_generator.linear_interpolator.LinearInterpolator.set_parameters | ( | self, | |
params | |||
) |
Not implemented for this interpolator.
Definition at line 123 of file linear_interpolator.py.
|
private |
Definition at line 112 of file linear_interpolator.py.
|
private |
Definition at line 76 of file linear_interpolator.py.
|
private |
Definition at line 74 of file linear_interpolator.py.
|
private |
Definition at line 224 of file linear_interpolator.py.
|
private |
Definition at line 94 of file linear_interpolator.py.
|
private |
Definition at line 93 of file linear_interpolator.py.
|
private |
Definition at line 108 of file linear_interpolator.py.
|
private |
Definition at line 98 of file linear_interpolator.py.
|
private |
Definition at line 114 of file linear_interpolator.py.
|
static |
Definition at line 66 of file linear_interpolator.py.