Skip to content

uuv_trajectory_generator

uuv_trajectory_generator

Trajectory generation package.

uuv_trajectory_generator.trajectory_generator

TrajectoryGenerator

TrajectoryGenerator(self, full_dof=False, stamped_pose_only=False)
Trajectory generator based on waypoint and trajectory interpolation.

Input arguments

  • full_dof (type: bool, default: False): If True, generate the trajectory in 6 DoFs, otherwise roll and pitch are set to zero.
  • stamped_pose_only (type: bool, default: False): If True the output trajectory will set velocity and acceleration references as zero.
points

List of uuv_trajectory_generator.TrajectoryPoint: List of trajectory points

time

List of float: List of timestamps

set_stamped_pose_only

TrajectoryGenerator.set_stamped_pose_only(self, flag)
Set flag to enable or disable computation of trajectory points

Input arguments

  • flag (type: bool): Parameter description

Returns

Description of return values

get_trajectory_as_message
TrajectoryGenerator.get_trajectory_as_message(self)

Return the trajectory points as a Trajectory type message. If waypoints are currently in use, then sample the interpolated path and return the poses only.

is_using_waypoints

TrajectoryGenerator.is_using_waypoints(self)
Return true if the waypoint interpolation is being used.

set_waypoints

TrajectoryGenerator.set_waypoints(self, waypoints, init_rot=(0, 0, 0, 1))
Initializes the waypoint interpolator with a set of waypoints.

get_waypoints
TrajectoryGenerator.get_waypoints(self)

Return the waypoints used by the waypoint interpolator, if any exist.

add_waypoint
TrajectoryGenerator.add_waypoint(self, waypoint, add_to_beginning=False)

Add waypoint to the current waypoint set, if one has been initialized.

add_trajectory_point
TrajectoryGenerator.add_trajectory_point(self, pnt)

If a trajectory set is currently being used in the interpolation process, add a trajectory point to the set.

uuv_trajectory_generator.trajectory_point

TrajectoryPoint

TrajectoryPoint(self, t=0.0, pos=[0, 0, 0], quat=[0, 0, 0, 1], lin_vel=[0, 0, 0], ang_vel=[0, 0, 0], lin_acc=[0, 0, 0], ang_acc=[0, 0, 0])
Trajectory point data structure.

Input arguments

  • t (type: float, value: 0): Timestamp
  • pos (type: list of float or numpy.array, default: [0, 0, 0]): 3D position vector in meters
  • quat (type: list of float or numpy.array, default: [0, 0, 0, 1]): Quaternion in the form of (x, y, z, w).
  • lin_vel (type: list of float or numpy.array, default: [0, 0, 0]): 3D linear velocity vector in m/s
  • ang_vel (type: list of float or numpy.array, default: [0, 0, 0]): 3D angular velocity vector as rad/s
  • lin_acc (type: list of float or numpy.array, default: [0, 0, 0]): 3D linear acceleration vector as m/s^2
  • ang_acc (type: list of float or numpy.array, default: [0, 0, 0]): 3D angular acceleration vector as rad/s^2
a

numpy.array: Linear acceleration vector

acc

numpy.array: Linear acceleration vector

alpha

numpy.array: Angular acceleartion vector

p

numpy.array: Position vector

pos

numpy.array: Position vector

q

numpy.array: Quaternion vector as (x, y, z, w)

rot

numpy.array: roll, pitch and yaw angles

rot_matrix

numpy.array: Rotation matrix

rotq

numpy.array: Quaternion vector as (x, y, z, w)

t

float: Time stamp

v

numpy.array: Linear velocity vector

vel

numpy.array: Linear velocity vector

w

numpy.array: Angular velocity vector

x

float: X coordinate of position vector

y

float: Y coordinate of position vector

z

float: Z coordinate of position vector

to_message

TrajectoryPoint.to_message(self)
Convert current data to a trajectory point message.

Returns

Trajectory point message as uuv_control_msgs/TrajectoryPoint

from_message

TrajectoryPoint.from_message(self, msg)
Parse a trajectory point message of type uuv_control_msgs/TrajectoryPoint into the uuv_trajectory_generator/TrajectoryPoint.

Input arguments

  • msg (type: uuv_control_msgs/TrajectoryPoint): Input trajectory message
from_dict

TrajectoryPoint.from_dict(self, data)
Initialize the trajectory point attributes from a dict.

Input arguments

  • data (type: dict): Trajectory point as a dict
to_dict

TrajectoryPoint.to_dict(self)
Convert trajectory point to dict.

Returns

Trajectory points data as a dict

uuv_trajectory_generator.wp_trajectory_generator

WPTrajectoryGenerator

WPTrajectoryGenerator(self, full_dof=False, use_finite_diff=True, interpolation_method='cubic', stamped_pose_only=False)
Class that generates a trajectory from the interpolated path generated from a set of waypoints. It uses the information given for the waypoint's maximum forward speed to estimate the velocity between waypoint and parametrize the interpolated curve. The velocity and acceleration profiles are the generated through finite discretization. These profiles are not optimized, this class is a simple solution for quick trajectory generation for waypoint navigation.

Input arguments

  • full_dof (type: bool, default: False): True to generate 6 DoF trajectories
  • use_finite_diff (type: bool, default: True): Use finite differentiation if True, otherwise use the motion regression algorithm
  • interpolation_method (type: str, default: cubic): Name of the interpolation method, options are cubic, dubins, lipb or linear
  • stamped_pose_only (type: bool, default: False): Generate only position and quaternion vectors, velocities and accelerations are set to zero
closest_waypoint

Return the closest waypoint to the current position on the path.

closest_waypoint_idx

int: Index of the closest waypoint to the current position on the path.

interpolator

str: Name of the interpolation method

interpolator_tags

List of str: List of all interpolation method

stamped_pose_only

bool: Flag to enable computation of stamped poses

started

bool: Flag set to true if the interpolation has started.

use_finite_diff

bool: Use finite differentiation for computation of trajectory points

is_full_dof

WPTrajectoryGenerator.is_full_dof(self)
Return true if the trajectory is generated for all 6 degrees of freedom.

get_max_time

WPTrajectoryGenerator.get_max_time(self)
Return maximum trajectory time.

set_duration

WPTrajectoryGenerator.set_duration(self, t)
Set a new maximum trajectory time.

is_finished

WPTrajectoryGenerator.is_finished(self)
Return true if the trajectory has finished.

reset

WPTrajectoryGenerator.reset(self)
Reset all class attributes to allow a new trajectory to be computed.

init_waypoints

WPTrajectoryGenerator.init_waypoints(self, waypoint_set, init_rot=(0, 0, 0, 1))
Initialize the waypoint set.

add_waypoint

WPTrajectoryGenerator.add_waypoint(self, waypoint, add_to_beginning=False)
Add waypoint to the existing waypoint set. If no waypoint set has been initialized, create new waypoint set structure and add the given waypoint.

get_waypoints

WPTrajectoryGenerator.get_waypoints(self)
Return waypoint set.

update_dt

WPTrajectoryGenerator.update_dt(self, t)
Update the time stamp.

get_samples

WPTrajectoryGenerator.get_samples(self, step=0.005)
Return pose samples from the interpolated path.

set_start_time

WPTrajectoryGenerator.set_start_time(self, t)
Set a custom starting time to the interpolated trajectory.

generate_pnt

WPTrajectoryGenerator.generate_pnt(self, t, pos, rot)
Return trajectory sample for the current parameter s.

uuv_trajectory_generator.path_generator.bezier_curve

BezierCurve
BezierCurve(self, pnts, order, tangents=None, normals=None)

Implementation of Bezier curves of orders 3, 4 and 5 based on [1].

Input arguments

  • pnts (type: list): List of 3D points as a vector (example: [[0, 0, 0], [0, 1, 2]])
  • order (type: int): Order of the Bezier curve, options are 3, 4 or 5
  • tangents (type: list, default: None): Optional input of the tangent vectors for each of the input points. In case only two points are provided, the tangents have to be provided, too. Otherwise, the tangents will be calculated.
  • normals (type: list, default: None): Optional input of the normal vectors for each of the input points. In case only two points are provided, the normals have to be provided, too. Otherwise, the normals will be calculated.

Note

[1] Biagiotti, Luigi, and Claudio Melchiorri. Trajectory planning for automatic machines and robots. Springer Science & Business Media, 2008.

distance

BezierCurve.distance(p1, p2)
Compute the distance between two 3D points.

Input arguments

  • p1 (type: list of float or numpy.array): Point 1
  • p2 (type: list of float or numpy.array): Point 2

Returns

Distance between points as a float

generate_cubic_curve

BezierCurve.generate_cubic_curve(pnts)
Generate cubic Bezier curve segments from a list of points.

Input arguments

  • pnts (type: list of float or of numpy.array): List of points

Returns

List of BezierCurve segments

generate_quintic_curve

BezierCurve.generate_quintic_curve(pnts)
Generate quintic Bezier curve segments from a list of points.

Input arguments

  • pnts (type: list of float or of numpy.array): List of points

Returns

List of BezierCurve segments

control_pnts

BezierCurve.control_pnts(self)
Return the list of control points of the Bezier curve.

Returns

List of 3D points as list

interpolate

BezierCurve.interpolate(self, u)
Interpolate the Bezier curve using the input parametric variable u.

Input arguments

  • u (type: float): Curve parametric input in the interval [0, 1]

Returns

3D point from the Bezier curve as numpy.array

get_derivative

BezierCurve.get_derivative(self, u, order=1)
Compute the derivative of the Bezier curve using the input parametric variable u.

Input arguments

  • u (type: float): Curve parametric input in the interval [0, 1]
  • order (type: int, default: 1): Order of the derivative

Returns

numpy.array: 3D derivative value from the Bezier curve

get_length

BezierCurve.get_length(self)
Get length of the Bezier curve segment.

Returns

float: Length of the curve

compute_polynomial

BezierCurve.compute_polynomial(self, n, i, u)
Compute the Bernstein polynomial

\mathbf{B} = {n\choose i} (1 - u)^{(n - i)} u^{i}

Input arguments

  • n (type: int): Degree of the Bezier curve
  • i (type: int): Index of the control point
  • u (type: float): Parametric input of the curve in interval [0, 1]

Returns

float: Bernstein polynomial result

uuv_trajectory_generator.path_generator.cs_interpolator

CSInterpolator

CSInterpolator(self)
Interpolator that will generate cubic Bezier curve segments for a set of waypoints. The full algorithm can be seen in Biagiotti and Melchiorri, 2008.

Note

Biagiotti, Luigi, and Claudio Melchiorri. Trajectory planning for automatic machines and robots. Springer Science & Business Media, 2008.

LABEL

str(object='') -> string

Return a nice string representation of the object. If the argument is a string, the return value is the same object.

init_interpolator

CSInterpolator.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.

set_parameters

CSInterpolator.set_parameters(self, params)
Not implemented for this interpolator.

get_samples

CSInterpolator.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.

generate_pos

CSInterpolator.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.

generate_pnt

CSInterpolator.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.

generate_quat

CSInterpolator.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)

uuv_trajectory_generator.path_generator.dubins_interpolator

DubinsInterpolator

DubinsInterpolator(self)
3D Dubins path interpolator implemented based on the sources below.

Note

Owen, Mark, Randal W. Beard, and Timothy W. McLain. "Implementing Dubins Airplane Paths on Fixed-Wing UAVs." Handbook of Unmanned Aerial Vehicles (2014): 1677-1701.

Cai, Wenyu, Meiyan Zhang, and Yahong Zheng. "Task Assignment and Path Planning for Multiple Autonomous Underwater Vehicles Using 3D Dubins Curves." Sensors 17.7 (2017):1607.

Hansen, Karl D., and Anders La Cour-Harbo. "Waypoint Planning with Dubins Curves Using Genetic Algorithms." 2016 European Control Conference (ECC) (2016).

Lin, Yucong, and Srikanth Saripalli. "Path Planning Using 3D Dubins Curve for Unmanned Aerial Vehicles." 2014 International Conference on Unmanned Aircraft Systems (ICUAS) (2014).

LABEL

str(object='') -> string

Return a nice string representation of the object. If the argument is a string, the return value is the same object.

init_interpolator

DubinsInterpolator.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

bool: True if the path segments were successfully generated.

set_parameters

DubinsInterpolator.set_parameters(self, params)
Set interpolator's parameters. All the options for the params input can be seen below:

params=dict(
    radius=0.0,
    max_pitch=0.0
    )
  • radius (type: float): Turning radius
  • max_pitch (type: float): Max. pitch angle allowed between two waypoints. If the pitch exceeds max_pitch, a helical path is computed to perform steep climbs or dives.

Input arguments

  • params (type: dict): dict containing interpolator's configurable elements.
get_samples

DubinsInterpolator.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.

generate_pos

DubinsInterpolator.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.

generate_pnt

DubinsInterpolator.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.

generate_quat

DubinsInterpolator.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)

uuv_trajectory_generator.path_generator.helical_segment

HelicalSegment

HelicalSegment(self, center, radius, n_turns, delta_z, angle_offset, is_clockwise=True)
Generator of helical segments.

Input arguments

  • center (type: list): Center of the helix in meters
  • radius (type: float): Radius of the helix in meters
  • n_turns (type: int): Number of turns
  • delta_z (type: float): Length of the step in the Z direction between each turn of the helix in meters
  • angle_offset (type: float): Angle offset to start the helix
  • is_clockwise (type: bool, default: True): If True, the helix is generated clockwise.

Example

radius = 3
center = [2, 2, 2]
n_turns = 2
delta_z = 1
angle_offset = 0.0
is_clockwise = True

helix = HelicalSegment(center, radius, n_turns, delta_z, angle_offset, is_clockwise)

u = numpy.linspace(0, 1, 100)
pnts = numpy.array([helix.interpolate(i) for i in u])
get_length

HelicalSegment.get_length(self)
Return the length of the helix in meters

get_pitch

HelicalSegment.get_pitch(self)
Return the pitch angle of the helical path in radians

interpolate

HelicalSegment.interpolate(self, u)
Compute the 3D point on the helical path

Input arguments

  • param (type: data_type, default: data): Parameter description

Returns

Description of return values

uuv_trajectory_generator.path_generator.line_segment

LineSegment

LineSegment(self, p_init, p_target)
Line segment class.

Input arguments

  • p_init (type: list or numpy.array): Line's starting point
  • p_target (type: list or numpy.array): Line's ending point
interpolate

LineSegment.interpolate(self, u)
Interpolate the Bezier curve using the input parametric variable u.

Input arguments

  • u (type: float): Curve parametric input in the interval [0, 1]

Returns

numpy.array: 3D point from the Bezier curve

get_derivative

LineSegment.get_derivative(self, *args)
Compute the derivative of the line segment.

Returns

numpy.array: 3D derivative value from the Bezier curve

get_length

LineSegment.get_length(self)
Get length of the Bezier curve segment.

Returns

float: Length of the curve

get_tangent

LineSegment.get_tangent(self)
Compute tangent vector.

Returns

numpy.array: Tangent vector

uuv_trajectory_generator.path_generator.linear_interpolator

LinearInterpolator

LinearInterpolator(self)
Simple interpolator that generates a parametric line connecting the input waypoints.

Example

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)
LABEL

str(object='') -> string

Return a nice string representation of the object. If the argument is a string, the return value is the same object.

init_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.

set_parameters

LinearInterpolator.set_parameters(self, params)
Not implemented for this interpolator.

get_samples

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.

generate_pos

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.

generate_pnt

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.

generate_quat

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)

uuv_trajectory_generator.path_generator.lipb_interpolator

LIPBInterpolator
LIPBInterpolator(self)

Linear interpolator with polynomial blends.

Note

Biagiotti, Luigi, and Claudio Melchiorri. Trajectory planning for automatic machines and robots. Springer Science & Business Media, 2008.

LABEL

str(object='') -> string

Return a nice string representation of the object. If the argument is a string, the return value is the same object.

init_interpolator

LIPBInterpolator.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.

set_parameters

LIPBInterpolator.set_parameters(self, params)
Set interpolator's parameters. All the options for the params input can be seen below:

params=dict(
    radius=0.0
    )
  • radius (type: float): Radius of the corners modeled as fifth-order Bezier curves.

Input arguments

  • params (type: dict): dict containing interpolator's configurable elements.
get_samples

LIPBInterpolator.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.

generate_pos

LIPBInterpolator.generate_pos(self, s, *args)
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.

generate_pnt

LIPBInterpolator.generate_pnt(self, s, t=0.0, *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.

generate_quat

LIPBInterpolator.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)

uuv_trajectory_generator.path_generator.path_generator