def get_nonlinear_path(self,
position: List[float],
euler: List[float] = None,
quaternion: List[float] = None,
ignore_collisions=False,
trials=100,
max_configs=60,
trials_per_goal=6,
algorithm=Algos.SBL) -> ArmConfigurationPath:
"""Gets a non-linear (planned) configuration path given a target pose.
A path is generated by finding several configs for a pose, and ranking
them according to the distance in configuration space (smaller is
better).
Must specify either rotation in euler or quaternions, but not both!
:param position: The x, y, z position of the target.
:param euler: The x, y, z orientation of the target (in radians).
:param quaternion: A list containing the quaternion (x,y,z,w).
:param ignore_collisions: If collision checking should be disabled.
:param trials: The maximum number of attempts to reach max_configs
:param max_configs: The maximum number of configurations we want to
generate before ranking them.
:param trials_per_goal: The number of paths per config we want to trial.
:param algorithm: The algorithm for path planning to use.
:raises: ConfigurationPathError if no path could be created.
:return: A non-linear path in the arm configuration space.
"""
if not ((euler is None) ^ (quaternion is None)):
raise ConfigurationPathError(
'Specify either euler or quaternion values, but not both.')
prev_pose = self._ik_target.get_pose()
self._ik_target.set_position(position)
if euler is not None:
self._ik_target.set_orientation(euler)
elif quaternion is not None:
self._ik_target.set_quaternion(quaternion)
handles = [j.get_handle() for j in self.joints]
# Despite verbosity being set to 0, OMPL spits out a lot of text
with utils.suppress_std_out_and_err():
_, ret_floats, _, _ = utils.script_call(
'getNonlinearPath@PyRep',
PYREP_SCRIPT_TYPE,
ints=[
self._ik_group, self._collision_collection,
int(ignore_collisions), trials, max_configs,
trials_per_goal
] + handles,
strings=[algorithm.value])
self._ik_target.set_pose(prev_pose)
if len(ret_floats) == 0:
raise ConfigurationPathError('Could not create path.')
return ArmConfigurationPath(self, ret_floats)
def get_linear_path(self,
position: Union[List[float], np.ndarray],
euler: Union[List[float], np.ndarray] = None,
quaternion: Union[List[float], np.ndarray] = None,
steps=50,
ignore_collisions=False,
relative_to: Object = None) -> ArmConfigurationPath:
"""Gets a linear configuration path given a target pose.
Generates a path that drives a robot from its current configuration
to its target dummy in a straight line (i.e. shortest path in Cartesian
space).
Must specify either rotation in euler or quaternions, but not both!
:param position: The x, y, z position of the target.
:param euler: The x, y, z orientation of the target (in radians).
:param quaternion: A list containing the quaternion (x,y,z,w).
:param steps: The desired number of path points. Each path point
contains a robot configuration. A minimum of two path points is
required. If the target pose distance is large, a larger number
of steps leads to better results for this function.
:param ignore_collisions: If collision checking should be disabled.
:param relative_to: Indicates relative to which reference frame we want
the target pose. Specify None to retrieve the absolute pose,
or an Object relative to whose reference frame we want the pose.
:raises: ConfigurationPathError if no path could be created.
:return: A linear path in the arm configuration space.
"""
if not ((euler is None) ^ (quaternion is None)):
raise ConfigurationPathError(
'Specify either euler or quaternion values, but not both.')
prev_pose = self._ik_target.get_pose()
self._ik_target.set_position(position, relative_to)
if euler is not None:
self._ik_target.set_orientation(euler, relative_to)
elif quaternion is not None:
self._ik_target.set_quaternion(quaternion, relative_to)
handles = [j.get_handle() for j in self.joints]
# Despite verbosity being set to 0, OMPL spits out a lot of text
with utils.suppress_std_out_and_err():
_, ret_floats, _, _ = utils.script_call(
'getLinearPath@PyRep',
PYREP_SCRIPT_TYPE,
ints=[
steps, self._ik_group, self._collision_collection,
int(ignore_collisions)
] + handles)
self._ik_target.set_pose(prev_pose)
if len(ret_floats) == 0:
raise ConfigurationPathError('Could not create path.')
return ArmConfigurationPath(self, ret_floats)
def get_path_from_cartesian_path(
self, path: CartesianPath) -> ArmConfigurationPath:
"""Translate a path from cartesian space, to arm configuration space.
Note: It must be possible to reach the start of the path via a linear
path, otherwise an error will be raised.
:param path: A :py:class:`CartesianPath` instance to be translated to
a configuration-space path.
:raises: ConfigurationPathError if no path could be created.
:return: A path in the arm configuration space.
"""
handles = [j.get_handle() for j in self.joints]
_, ret_floats, _, _ = utils.script_call(
'getPathFromCartesianPath@PyRep',
PYREP_SCRIPT_TYPE,
ints=[
path.get_handle(), self._ik_group,
self._ik_target.get_handle()
] + handles)
if len(ret_floats) == 0:
raise ConfigurationPathError(
'Could not create a path from cartesian path.')
return ArmConfigurationPath(self, ret_floats)
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_configs_for_tip_pose(
self,
position: Union[List[float], np.ndarray],
euler: Union[List[float], np.ndarray] = None,
quaternion: Union[List[float], np.ndarray] = None,
ignore_collisions=False,
trials=300,
max_configs=60,
relative_to: Object = None) -> List[List[float]]:
"""Gets a valid joint configuration for a desired end effector pose.
Must specify either rotation in euler or quaternions, but not both!
:param position: The x, y, z position of the target.
:param euler: The x, y, z orientation of the target (in radians).
:param quaternion: A list containing the quaternion (x,y,z,w).
:param ignore_collisions: If collision checking should be disabled.
:param trials: The maximum number of attempts to reach max_configs
:param max_configs: The maximum number of configurations we want to
generate before ranking them.
:param relative_to: Indicates relative to which reference frame we want
the target pose. Specify None to retrieve the absolute pose,
or an Object relative to whose reference frame we want the pose.
:raises: ConfigurationError if no joint configuration could be found.
:return: A list of valid joint configurations for the desired
end effector pose.
"""
if not ((euler is None) ^ (quaternion is None)):
raise ConfigurationPathError(
'Specify either euler or quaternion values, but not both.')
prev_pose = self._ik_target.get_pose()
self._ik_target.set_position(position, relative_to)
if euler is not None:
self._ik_target.set_orientation(euler, relative_to)
elif quaternion is not None:
self._ik_target.set_quaternion(quaternion, relative_to)
handles = [j.get_handle() for j in self.joints]
# Despite verbosity being set to 0, OMPL spits out a lot of text
with utils.suppress_std_out_and_err():
_, ret_floats, _, _ = utils.script_call(
'findSeveralCollisionFreeConfigsAndCheckApproach@PyRep',
PYREP_SCRIPT_TYPE,
ints=[
self._ik_group, self._collision_collection,
int(ignore_collisions), trials, max_configs
] + handles)
self._ik_target.set_pose(prev_pose)
if len(ret_floats) == 0:
raise ConfigurationError(
'Could not find a valid joint configuration for desired end effector pose.'
)
num_configs = int(len(ret_floats) / len(handles))
return [[
ret_floats[len(handles) * i + j] for j in range(len(handles))
] for i in range(num_configs)]
def get_nonlinear_path_points(self, position: List[float],
angle=0,
boundaries=2,
path_pts=600,
ignore_collisions=False,
algorithm=Algos.RRTConnect) -> List[List[float]]:
"""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.
"""
# Base dummy required to be parent of the robot tree
# self.base_ref.set_parent(None)
# self.set_parent(self.base_ref)
# Missing the dist1 for intermediate target
self.target_base.set_position([position[0], position[1], self.target_z])
self.target_base.set_orientation([0, 0, angle])
handle_base = self.get_handle()
handle_target_base = self.target_base.get_handle()
# Despite verbosity being set to 0, OMPL spits out a lot of text
with utils.suppress_std_out_and_err():
_, ret_floats, _, _ = utils.script_call(
'getNonlinearPathMobile@PyRep', PYREP_SCRIPT_TYPE,
ints=[handle_base, handle_target_base,
self._collision_collection,
int(ignore_collisions), path_pts], floats=[boundaries],
strings=[algorithm.value])
if len(ret_floats) == 0:
raise ConfigurationPathError('Could not create path.')
path = []
for i in range(0, len(ret_floats) // 3):
inst = ret_floats[3 * i:3 * i + 3]
if i > 0:
dist_change = sqrt((inst[0] - prev_inst[0]) ** 2 + (
inst[1] - prev_inst[1]) ** 2)
else:
dist_change = 0
inst.append(dist_change)
path.append(inst)
prev_inst = inst
return path
def get_linear_path(self,
position: List[float],
angle=0) -> HolonomicConfigurationPath:
"""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])
handle_base = self.get_handle()
handle_target_base = self.target_base.get_handle()
_, ret_floats, _, _ = utils.script_call(
'getBoxAdjustedMatrixAndFacingAngle@PyRep',
PYREP_SCRIPT_TYPE,
ints=[handle_base, handle_target_base])
m = ret_floats[:-1]
angle = ret_floats[-1]
self.intermediate_target_base.set_position([
m[3] - m[0] * self.dist1, m[7] - m[4] * self.dist1, self.target_z
])
self.intermediate_target_base.set_orientation([0, 0, angle])
self.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 HolonomicConfigurationPath(self, path)
def get_nonlinear_path(self, position: Union[List[float], np.ndarray],
euler: Union[List[float], np.ndarray] = None,
quaternion: Union[List[float], np.ndarray] = None,
ignore_collisions=False,
trials=300,
max_configs=1,
distance_threshold: float = 0.65,
max_time_ms: int = 10,
trials_per_goal=1,
algorithm=Algos.RRTConnect,
relative_to: Object = None
) -> ArmConfigurationPath:
"""Gets a non-linear (planned) configuration path given a target pose.
A path is generated by finding several configs for a pose, and ranking
them according to the distance in configuration space (smaller is
better).
Must specify either rotation in euler or quaternions, but not both!
:param position: The x, y, z position of the target.
:param euler: The x, y, z orientation of the target (in radians).
:param quaternion: A list containing the quaternion (x,y,z,w).
:param ignore_collisions: If collision checking should be disabled.
:param trials: The maximum number of attempts to reach max_configs.
See 'solve_ik_via_sampling'.
:param max_configs: The maximum number of configurations we want to
generate before sorting them. See 'solve_ik_via_sampling'.
:param distance_threshold: Distance indicating when IK should be
computed in order to try to bring the tip onto the target.
See 'solve_ik_via_sampling'.
:param max_time_ms: Maximum time in ms spend searching for
each configuation. See 'solve_ik_via_sampling'.
:param trials_per_goal: The number of paths per config we want to trial.
:param algorithm: The algorithm for path planning to use.
:param relative_to: Indicates relative to which reference frame we want
the target pose. Specify None to retrieve the absolute pose,
or an Object relative to whose reference frame we want the pose.
:raises: ConfigurationPathError if no path could be created.
:return: A non-linear path in the arm configuration space.
"""
handles = [j.get_handle() for j in self.joints]
try:
configs = self.solve_ik_via_sampling(
position, euler, quaternion, ignore_collisions, trials,
max_configs, distance_threshold, max_time_ms, relative_to)
except ConfigurationError as e:
raise ConfigurationPathError('Could not create path.') from e
_, ret_floats, _, _ = utils.script_call(
'getNonlinearPath@PyRep', PYREP_SCRIPT_TYPE,
ints=[self._collision_collection, int(ignore_collisions),
trials_per_goal] + handles,
floats=configs.flatten().tolist(),
strings=[algorithm.value])
if len(ret_floats) == 0:
raise ConfigurationPathError('Could not create path.')
return ArmConfigurationPath(self, ret_floats)
