def handle_explicitly(self) -> None:
"""Handle spherical vision sensor explicitly.
This enables capturing image (e.g., capture_rgb())
without PyRep.step().
"""
utils.script_call('handleSpherical@PyRep', PYREP_SCRIPT_TYPE,
[self._sensor_depth._handle, self._sensor_rgb._handle] + self._six_sensor_handles, [], [], [])
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_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 script_call(
self,
function_name_at_script_name: str,
script_handle_or_type: int,
ints=(),
floats=(),
strings=(),
bytes='') -> (Tuple[List[int], List[float], List[str], str]):
"""Calls a script function (from a plugin, the main client application,
or from another script). This represents a callback inside of a script.
:param function_name_at_script_name: A string representing the function
name and script name, e.g. myFunctionName@theScriptName. When the
script is not associated with an object, then just specify the
function name.
:param script_handle_or_type: The handle of the script, otherwise the
type of the script.
:param ints: The input ints to the script.
:param floats: The input floats to the script.
:param strings: The input strings to the script.
:param bytes: The input bytes to the script (as a string).
:return: Any number of return values from the called Lua function.
"""
return utils.script_call(function_name_at_script_name,
script_handle_or_type, ints, floats, strings,
bytes)
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 step(self) -> 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._mobile.intermediate_target_base.get_position(
relative_to=self._mobile)
if len(self._path_points) > 2: # Non-linear path
if self.inter_done:
self._next_i_path()
self._set_inter_target(self.i_path)
self.inter_done = False
handleBase = self._mobile.get_handle()
handleInterTargetBase = self._mobile.intermediate_target_base.get_handle(
)
__, ret_floats, _, _ = utils.script_call(
'getBoxAdjustedMatrixAndFacingAngle@PyRep',
PYREP_SCRIPT_TYPE,
ints=[handleBase, handleInterTargetBase])
m = ret_floats[:-1]
angle = ret_floats[-1]
self._mobile.intermediate_target_base.set_position(
[m[3], m[7], self._mobile.target_z])
self._mobile.intermediate_target_base.set_orientation(
[0, 0, angle])
if sqrt((pos_inter[0])**2 + (pos_inter[1])**2) < 0.1:
self.inter_done = True
actuation = [0, 0, 0]
else:
actuation, _ = self._mobile.get_base_actuation()
self._mobile.set_base_angular_velocites(actuation)
if self.i_path == len(self._path_points) - 1:
self._path_done = True
else:
actuation, self._path_done = self._mobile.get_base_actuation()
self._mobile.set_base_angular_velocites(actuation)
return self._path_done
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_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)
