def __init__(self, full_dof=False): self._logger = get_logger() # Waypoint set self._waypoints = None # True if the path is generated for all degrees of freedom, otherwise # the path will be generated for (x, y, z, yaw) only self._is_full_dof = full_dof # The parametric variable to use as input for the interpolator self._s = list() self._segment_to_wp_map = list() self._cur_s = 0 self._s_step = 0.0001 self._start_time = None self._duration = None self._termination_by_time = True self._final_pos_tolerance = 0.1 self._init_rot = quaternion_about_axis(0.0, [0, 0, 1]) self._last_rot = quaternion_about_axis(0.0, [0, 0, 1]) self._markers_msg = MarkerArray() self._marker_id = 0
def __init__(self, full_dof=False, use_finite_diff=True, interpolation_method='cubic', stamped_pose_only=False): """Class constructor.""" self._logger = logging.getLogger('wp_trajectory_generator') out_hdlr = logging.StreamHandler(sys.stdout) out_hdlr.setFormatter( logging.Formatter( get_namespace().replace('/', '').upper() + ' -- %(asctime)s | %(levelname)s | %(module)s | %(message)s')) out_hdlr.setLevel(logging.INFO) self._logger.addHandler(out_hdlr) self._logger.setLevel(logging.INFO) self._path_generators = dict() self._logger.info('Waypoint interpolators available:') for gen in PathGenerator.get_all_generators(): self._logger.info('\t - ' + gen.get_label()) self._path_generators[gen.get_label()] = gen self._path_generators[gen.get_label()].set_full_dof(full_dof) # Time step between interpolated samples self._dt = None # Last time stamp self._last_t = None # Last interpolated point self._last_pnt = None self._this_pnt = None # Flag to generate only stamped pose, no velocity profiles self._stamped_pose_only = stamped_pose_only self._t_step = 0.001 # Interpolation method self._interp_method = interpolation_method # True if the path is generated for all degrees of freedom, otherwise # the path will be generated for (x, y, z, yaw) only self._is_full_dof = full_dof # Use finite differentiation if true, otherwise use motion regression # algorithm self._use_finite_diff = use_finite_diff # Time window used for the regression method self._regression_window = 0.5 # If the regression method is used, adjust the time step if not self._use_finite_diff: self._t_step = self._regression_window / 30 # Flags to indicate that the interpolation process has started and # ended self._has_started = False self._has_ended = False # The parametric variable to use as input for the interpolator self._cur_s = 0 self._init_rot = quaternion_about_axis(0.0, [0, 0, 1])
def _compute_rot_quat(self, dx, dy, dz): if np.isclose(dx, 0) and np.isclose(dy, 0): rotq = self._last_rot else: heading = np.arctan2(dy, dx) rotq = quaternion_about_axis(heading, [0, 0, 1]) if self._is_full_dof: rote = quaternion_about_axis( -1 * np.arctan2(dz, np.sqrt(dx**2 + dy**2)), [0, 1, 0]) rotq = quaternion_multiply(rotq, rote) # Certify that the next quaternion remains in the same half hemisphere d_prod = np.dot(self._last_rot, rotq) if d_prod < 0: rotq *= -1 return rotq
def 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)` """ s = max(0, s) s = min(s, 1) last_s = s - self._s_step if last_s == 0: last_s = 0 if s == 0: self._last_rot = deepcopy(self._init_rot) return self._init_rot this_pos = self.generate_pos(s) last_pos = self.generate_pos(last_s) dx = this_pos[0] - last_pos[0] dy = this_pos[1] - last_pos[1] dz = this_pos[2] - last_pos[2] rotq = self._compute_rot_quat(dx, dy, dz) self._last_rot = deepcopy(rotq) # Calculating the step for the heading offset q_step = quaternion_about_axis( self._interp_fcns['heading'](s), np.array([0, 0, 1])) # Adding the heading offset to the rotation quaternion rotq = quaternion_multiply(rotq, q_step) return rotq