def get_linear_path(self,
position: List[float],
angle=0) -> NonHolonomicConfigurationPath:
"""Initialize linear path and check for collision along it.
Must specify either rotation in euler or quaternions, but not both!
:param position: The x, y position of the target.
:param angle: The z orientation of the target (in radians).
:raises: ConfigurationPathError if no path could be created.
:return: A linear path in the 2d space.
"""
position_base = self.get_position()
angle_base = self.get_orientation()[-1]
self.target_base.set_position(
[position[0], position[1], self.target_z])
self.target_base.set_orientation([0, 0, angle])
self.intermediate_target_base.set_position(
[position[0], position[1], self.target_z])
self.intermediate_target_base.set_orientation([0, 0, angle])
path = [[position_base[0], position_base[1], angle_base],
[position[0], position[1], angle]]
if self._check_collision_linear_path(path):
raise ConfigurationPathError(
'Could not create path. '
'An object was detected on the linear path.')
return NonHolonomicConfigurationPath(self, path)
def get_nonlinear_path(
self,
position: List[float],
angle=0,
boundaries=2,
path_pts=600,
ignore_collisions=False,
algorithm=Algos.RRTConnect) -> NonHolonomicConfigurationPath:
"""Gets a non-linear (planned) configuration path given a target pose.
:param position: The x, y, z position of the target.
:param angle: The z orientation of the target (in radians).
:param boundaries: A float defining the path search in x and y direction
[[-boundaries,boundaries],[-boundaries,boundaries]].
:param path_pts: number of sampled points returned from the computed path
:param ignore_collisions: If collision checking should be disabled.
:param algorithm: Algorithm used to compute path
:raises: ConfigurationPathError if no path could be created.
:return: A non-linear path (x,y,angle) in the xy configuration space.
"""
path = self._get_nonlinear_path_points(position, angle, boundaries,
path_pts, ignore_collisions,
algorithm)
return NonHolonomicConfigurationPath(self, path)
def step(self, path:NonHolonomicConfigurationPath) -> bool:
"""Make a step along the trajectory.
Step forward by calling _get_base_actuation to get the velocity needed
to be applied at the wheels.
NOTE: This does not step the physics engine. This is left to the user.
:return: If the end of the trajectory has been reached.
"""
if self._path_done:
raise RuntimeError('This path has already been completed. '
'If you want to re-run, then call set_to_start.')
pos_inter = self.intermediate_target_base.get_position(
relative_to=self)
if len(path._path_points) > 2: # Non-linear path
if path.inter_done:
path._next_i_path()
path._set_inter_target(path.i_path)
path.inter_done = False
if sqrt((pos_inter[0]) ** 2 + (pos_inter[1]) ** 2) < 0.1:
path.inter_done = True
[vl, vr], _ = self.get_base_actuation()
else:
[vl, vr], _ = self.get_base_actuation()
self.set_velocity(vl,vr,vl,vr)
if path.i_path == len(path._path_points) - 1:
self._path_done = True
else:
[vl, vr], self._path_done = self.get_base_actuation()
self.set_velocity(vl,vr,vl,vr)
return self._path_done