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, otherwiseroll
andpitch
are set to zero.stamped_pose_only
(type:bool
, default:False
): IfTrue
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)
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 offloat
ornumpy.array
, default:[0, 0, 0]
): 3D position vector in metersquat
(type: list offloat
ornumpy.array
, default:[0, 0, 0, 1]
): Quaternion in the form of(x, y, z, w)
.lin_vel
(type: list offloat
ornumpy.array
, default:[0, 0, 0]
): 3D linear velocity vector in m/sang_vel
(type: list offloat
ornumpy.array
, default:[0, 0, 0]
): 3D angular velocity vector as rad/slin_acc
(type: list offloat
ornumpy.array
, default:[0, 0, 0]
): 3D linear acceleration vector as m/s^2ang_acc
(type: list offloat
ornumpy.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
):True
to 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
,lipb
orlinear
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)
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 offloat
ornumpy.array
): Point 1p2
(type: list offloat
ornumpy.array
): Point 2
Returns
Distance between points as a float
generate_cubic_curve¶
BezierCurve.generate_cubic_curve(pnts)
Input arguments
pnts
(type: list offloat
or ofnumpy.array
): List of points
Returns
List of BezierCurve
segments
generate_quintic_curve¶
BezierCurve.generate_quintic_curve(pnts)
Input arguments
pnts
(type: list offloat
or 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
):dict
containing 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:list
ornumpy.array
): Line's starting pointp_target
(type:list
ornumpy.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
):dict
containing 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)