uuv_trajectory_generator
uuv_trajectory_generator¶
Trajectory generation package.
uuv_trajectory_generator.trajectory_generator¶
TrajectoryGenerator¶
TrajectoryGenerator(self, full_dof=False, stamped_pose_only=False)
Input arguments
full_dof(type:bool, default:False): IfTrue, generate the trajectory in 6 DoFs, otherwiserollandpitchare set to zero.stamped_pose_only(type:bool, default:False): IfTruethe 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)
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)
set_waypoints¶
TrajectoryGenerator.set_waypoints(self, waypoints, init_rot=(0, 0, 0, 1))
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])
Input arguments
t(type:float, value:0): Timestamppos(type: list offloatornumpy.array, default:[0, 0, 0]): 3D position vector in metersquat(type: list offloatornumpy.array, default:[0, 0, 0, 1]): Quaternion in the form of(x, y, z, w).lin_vel(type: list offloatornumpy.array, default:[0, 0, 0]): 3D linear velocity vector in m/sang_vel(type: list offloatornumpy.array, default:[0, 0, 0]): 3D angular velocity vector as rad/slin_acc(type: list offloatornumpy.array, default:[0, 0, 0]): 3D linear acceleration vector as m/s^2ang_acc(type: list offloatornumpy.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)
Returns
Trajectory point message as uuv_control_msgs/TrajectoryPoint
from_message¶
TrajectoryPoint.from_message(self, msg)
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)
dict.
Input arguments
data(type:dict): Trajectory point as adict
to_dict¶
TrajectoryPoint.to_dict(self)
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)
Input arguments
full_dof(type:bool, default:False):Trueto generate 6 DoF trajectoriesuse_finite_diff(type:bool, default:True): Use finite differentiation ifTrue, otherwise use the motion regression algorithminterpolation_method(type:str, default:cubic): Name of the interpolation method, options arecubic,dubins,lipborlinearstamped_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)
get_max_time¶
WPTrajectoryGenerator.get_max_time(self)
set_duration¶
WPTrajectoryGenerator.set_duration(self, t)
is_finished¶
WPTrajectoryGenerator.is_finished(self)
reset¶
WPTrajectoryGenerator.reset(self)
init_waypoints¶
WPTrajectoryGenerator.init_waypoints(self, waypoint_set, init_rot=(0, 0, 0, 1))
add_waypoint¶
WPTrajectoryGenerator.add_waypoint(self, waypoint, add_to_beginning=False)
get_waypoints¶
WPTrajectoryGenerator.get_waypoints(self)
update_dt¶
WPTrajectoryGenerator.update_dt(self, t)
get_samples¶
WPTrajectoryGenerator.get_samples(self, step=0.005)
set_start_time¶
WPTrajectoryGenerator.set_start_time(self, t)
generate_pnt¶
WPTrajectoryGenerator.generate_pnt(self, t, pos, rot)
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 5tangents(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)
Input arguments
p1(type: list offloatornumpy.array): Point 1p2(type: list offloatornumpy.array): Point 2
Returns
Distance between points as a float
generate_cubic_curve¶
BezierCurve.generate_cubic_curve(pnts)
Input arguments
pnts(type: list offloator ofnumpy.array): List of points
Returns
List of BezierCurve segments
generate_quintic_curve¶
BezierCurve.generate_quintic_curve(pnts)
Input arguments
pnts(type: list offloator ofnumpy.array): List of points
Returns
List of BezierCurve segments
control_pnts¶
BezierCurve.control_pnts(self)
Returns
List of 3D points as list
interpolate¶
BezierCurve.interpolate(self, u)
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)
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)
Returns
float: Length of the curve
compute_polynomial¶
BezierCurve.compute_polynomial(self, n, i, u)
Input arguments
n(type:int): Degree of the Bezier curvei(type:int): Index of the control pointu(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)
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)
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)
get_samples¶
CSInterpolator.get_samples(self, max_time, step=0.001)
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)
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)
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)
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)
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)
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)
params input can be seen below:
params=dict( radius=0.0, max_pitch=0.0 )
radius(type:float): Turning radiusmax_pitch(type:float): Max. pitch angle allowed between two waypoints. If the pitch exceedsmax_pitch, a helical path is computed to perform steep climbs or dives.
Input arguments
params(type:dict):dictcontaining interpolator's configurable elements.
get_samples¶
DubinsInterpolator.get_samples(self, max_time, step=0.001)
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)
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)
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)
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)
Input arguments
center(type:list): Center of the helix in metersradius(type:float): Radius of the helix in metersn_turns(type:int): Number of turnsdelta_z(type:float): Length of the step in the Z direction between each turn of the helix in metersangle_offset(type:float): Angle offset to start the helixis_clockwise(type:bool, default:True): IfTrue, 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)
get_pitch¶
HelicalSegment.get_pitch(self)
interpolate¶
HelicalSegment.interpolate(self, u)
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)
Input arguments
p_init(type:listornumpy.array): Line's starting pointp_target(type:listornumpy.array): Line's ending point
interpolate¶
LineSegment.interpolate(self, u)
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)
Returns
numpy.array: 3D derivative value from the Bezier curve
get_length¶
LineSegment.get_length(self)
Returns
float: Length of the curve
get_tangent¶
LineSegment.get_tangent(self)
Returns
numpy.array: Tangent vector
uuv_trajectory_generator.path_generator.linear_interpolator¶
LinearInterpolator¶
LinearInterpolator(self)
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)
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)
get_samples¶
LinearInterpolator.get_samples(self, max_time, step=0.001)
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)
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)
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)
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)
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)
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):dictcontaining interpolator's configurable elements.
get_samples¶
LIPBInterpolator.get_samples(self, max_time, step=0.001)
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)
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)
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)
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)