def test_captured_exception_in_thread():
def async_fn(callback):
def runner(cb):
raise ValueError('exception')
Thread(target=runner, args=(callback, )).start()
with pytest.raises(ValueError):
await_callback(async_fn)
def test_errback():
def async_fn(callback, errback):
def runner(cb, eb):
eb(ValueError('exception via errback'))
Thread(target=runner, args=(callback, errback)).start()
with pytest.raises(ValueError):
await_callback(async_fn, errback_name='errback')
def plan_motion(self,
robot,
goal_constraints,
start_configuration,
group,
path_constraints=None,
trajectory_constraints=None,
planner_id='',
num_planning_attempts=8,
allowed_planning_time=2.,
max_velocity_scaling_factor=1.,
max_acceleration_scaling_factor=1.,
attached_collision_meshes=None,
workspace_parameters=None):
kwargs = {}
kwargs['robot'] = robot
kwargs['goal_constraints'] = goal_constraints
kwargs['start_configuration'] = start_configuration
kwargs['group'] = group
kwargs['path_constraints'] = path_constraints
kwargs['trajectory_constraints'] = trajectory_constraints
kwargs['planner_id'] = planner_id
kwargs['num_planning_attempts'] = num_planning_attempts
kwargs['allowed_planning_time'] = allowed_planning_time
kwargs['max_velocity_scaling_factor'] = max_velocity_scaling_factor
kwargs[
'max_acceleration_scaling_factor'] = max_acceleration_scaling_factor
kwargs['attached_collision_meshes'] = attached_collision_meshes
kwargs['workspace_parameters'] = workspace_parameters
kwargs['errback_name'] = 'errback'
return await_callback(self.plan_motion_async, **kwargs)
def load_urdf(self, parameter_name='/robot_description'):
"""Loads a URDF model from the specified ROS parameter.
Parameters
----------
parameter_name : str, optional
Name of the ROS parameter containing the robot description.
Defaults to ``/robot_description``.
Returns
-------
str
URDF model of the robot currently loaded in ROS.
"""
if self.local_cache_enabled and self.robot_name:
filename = self._urdf_filename
if _cache_file_exists(filename):
return _read_file(filename)
param = roslibpy.Param(self.ros, parameter_name)
urdf = await_callback(param.get)
self.robot_name = self._read_robot_name(urdf)
if self.local_cache_enabled:
# Retrieve filename again, now that robot_name
# has been loaded from URDF
_write_file(self._urdf_filename, urdf)
return urdf
def load_srdf(self, parameter_name='/robot_description_semantic'):
"""Loads an SRDF model from the specified ROS parameter.
Parameters
----------
parameter_name : str, optional
Name of the ROS parameter containing the robot semantics.
Defaults to ``/robot_description_semantic``.
Returns
-------
str
SRDF model of the robot currently loaded in ROS.
"""
if self.local_cache_enabled and self.robot_name:
filename = self._srdf_filename
if _cache_file_exists(filename):
return _read_file(filename)
param = roslibpy.Param(self.ros, parameter_name)
srdf = await_callback(param.get)
if self.local_cache_enabled:
_write_file(self._srdf_filename, srdf)
return srdf
def inverse_kinematics(self,
robot,
frame_WCF,
start_configuration=None,
group=None,
options=None):
"""Calculate the robot's inverse kinematic for a given frame.
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which inverse kinematics is being calculated.
frame_WCF: :class:`compas.geometry.Frame`
The frame to calculate the inverse for.
start_configuration: :class:`compas_fab.robots.Configuration`, optional
If passed, the inverse will be calculated such that the calculated
joint positions differ the least from the start_configuration.
Defaults to the zero configuration.
group: str, optional
The planning group used for calculation. Defaults to the robot's
main planning group.
options: dict, optional
Dictionary containing the following key-value pairs:
- ``"base_link"``: (:obj:`str`) Name of the base link.
Defaults to the model's root link.
- ``"avoid_collisions"``: (:obj:`bool`, optional) Whether or not to avoid collisions.
Defaults to ``True``.
- ``"constraints"``: (:obj:`list` of :class:`compas_fab.robots.Constraint`, optional)
A set of constraints that the request must obey.
Defaults to ``None``.
- ``"timeout"``: (:obj:`int`, optional) Maximum allowed time for inverse kinematic calculation in seconds.
Defaults to ``2``. This value supersedes the ``"attempts"`` argument used before ROS Noetic.
- ``"attached_collision_meshes"``: (:obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh`, optional)
Defaults to ``None``.
- ``"attempts"``: (:obj:`int`, optional) The maximum number of inverse kinematic attempts.
Defaults to ``8``. This value is ignored on ROS Noetic and newer, use ``"timeout"`` instead.
Raises
------
compas_fab.backends.exceptions.BackendError
If no configuration can be found.
Returns
-------
:obj:`tuple` of :obj:`list`
A tuple of 2 elements containing a list of joint positions and a list of matching joint names.
"""
options = options or {}
kwargs = {}
kwargs['options'] = options
kwargs['frame_WCF'] = frame_WCF
kwargs['group'] = group
kwargs['start_configuration'] = start_configuration
kwargs['errback_name'] = 'errback'
# Use base_link or fallback to model's root link
options['base_link'] = options.get('base_link', robot.model.root.name)
return await_callback(self.inverse_kinematics_async, **kwargs)
def test_async_fn_with_more_params():
def async_fn(values, other_stuff, callback):
def runner(cb):
cb(200)
Thread(target=runner, args=(callback, )).start()
result = await_callback(async_fn, values=[1, 2, 3], other_stuff=None)
assert result == 200
def test_void_return_callback():
def async_fn(callback):
def runner(cb):
cb()
Thread(target=runner, args=(callback, )).start()
result = await_callback(async_fn)
assert result is None
def test_single_positional_arg_callback():
def async_fn(callback):
def runner(cb):
cb('only_return_value')
Thread(target=runner, args=(callback, )).start()
result = await_callback(async_fn)
assert result == 'only_return_value'
def test_one_positional_arg_and_kwargs_callback():
def async_fn(callback):
def runner(cb):
cb(1, retries=5)
Thread(target=runner, args=(callback, )).start()
result, kwargs = await_callback(async_fn)
assert result == 1
assert kwargs['retries'] == 5
def test_kwargs_callback():
def async_fn(callback):
def runner(cb):
cb(name='Austin', last_name='Powers')
Thread(target=runner, args=(callback, )).start()
result = await_callback(async_fn)
assert result['name'] == 'Austin'
assert result['last_name'] == 'Powers'
def forward_kinematics(self, robot, configuration, group, ee_link):
kwargs = {}
kwargs['robot'] = robot
kwargs['configuration'] = configuration
kwargs['group'] = group
kwargs['ee_link'] = ee_link
kwargs['errback_name'] = 'errback'
return await_callback(self.forward_kinematics_async, **kwargs)
def test_many_positional_args_and_kwargs_callback():
def async_fn(callback):
def runner(cb):
cb(4, 2, 3, retries=5)
Thread(target=runner, args=(callback, )).start()
a, b, c, kw = await_callback(async_fn)
assert a == 4
assert b == 2
assert c == 3
assert kw['retries'] == 5
def test_many_positional_args_callback():
def async_fn(callback):
def runner(cb):
cb(1, 2)
Thread(target=runner, args=(callback, )).start()
result = await_callback(async_fn)
assert result == (
1,
2,
)
def forward_kinematics(self, joint_positions, base_link, group,
joint_names, ee_link):
kwargs = {}
kwargs['joint_positions'] = joint_positions
kwargs['base_link'] = base_link
kwargs['group'] = group
kwargs['joint_names'] = joint_names
kwargs['ee_link'] = ee_link
kwargs['errback_name'] = 'errback'
return await_callback(self.forward_kinematics_async, **kwargs)
def forward_kinematics(self,
robot,
configuration,
group=None,
options=None):
"""Calculate the robot's forward kinematic.
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which inverse kinematics is being calculated.
configuration : :class:`compas_fab.robots.Configuration`
The full configuration to calculate the forward kinematic for. If no
full configuration is passed, the zero-joint state for the other
configurable joints is assumed.
group : str, optional
Unused parameter.
options : dict, optional
Dictionary containing the following key-value pairs:
- ``"base_link"``: (:obj:`str`) Name of the base link.
Defaults to the model's root link.
- ``"link"``: (:obj:`str`, optional) The name of the link to
calculate the forward kinematics for. Defaults to the group's end
effector link.
Backwards compatibility note: if there's no ``link`` option, the
planner will try also ``ee_link`` as fallback before defaulting
to the end effector's link.
Returns
-------
:class:`Frame`
The frame in the world's coordinate system (WCF).
"""
options = options or {}
kwargs = {}
kwargs['configuration'] = configuration
kwargs['options'] = options
kwargs['errback_name'] = 'errback'
# Use base_link or fallback to model's root link
options['base_link'] = options.get('base_link', robot.model.root.name)
# Use selected link or default to group's end effector
options['link'] = options.get(
'link',
options.get('ee_link')) or robot.get_end_effector_link_name(group)
if options['link'] not in robot.get_link_names(group):
raise ValueError(
'Link name {} does not exist in planning group'.format(
options['link']))
return await_callback(self.forward_kinematics_async, **kwargs)
def reset_planning_scene(self, options=None):
"""Resets the planning scene, removing all added collision meshes.
Parameters
----------
options : dict, optional
Unused parameter.
Returns
-------
``None``
"""
kwargs = {}
kwargs['errback_name'] = 'errback'
return await_callback(self.reset_planning_scene_async, **kwargs)
def get_planning_scene(self, options=None):
"""Retrieve the planning scene.
Parameters
----------
options : dict, optional
Unused parameter.
Returns
-------
:class:`compas_fab.backends.ros.messages.moveit_msgs.PlanningScene`
"""
kwargs = {}
kwargs['errback_name'] = 'errback'
return await_callback(self.get_planning_scene_async, **kwargs)
def plan_cartesian_motion(self, robot, frames, start_configuration, group,
max_step, jump_threshold, avoid_collisions,
path_constraints, attached_collision_meshes):
kwargs = {}
kwargs['robot'] = robot
kwargs['frames'] = frames
kwargs['start_configuration'] = start_configuration
kwargs['group'] = group
kwargs['max_step'] = max_step
kwargs['jump_threshold'] = jump_threshold
kwargs['avoid_collisions'] = avoid_collisions
kwargs['path_constraints'] = path_constraints
kwargs['attached_collision_meshes'] = attached_collision_meshes
kwargs['errback_name'] = 'errback'
return await_callback(self.plan_cartesian_motion_async, **kwargs)
def load_mesh(self, url):
"""Loads a mesh from local storage.
Parameters
----------
url : str
Mesh location
Returns
-------
:class:`Mesh`
Instance of a mesh.
"""
use_local_file = False
file_extension = _get_file_format(url)
if self.local_cache_enabled:
local_filename = self._local_mesh_filename(url)
use_local_file = _cache_file_exists(local_filename)
else:
_, local_filename = tempfile.mkstemp(suffix='.' + file_extension, prefix='ros_fileserver_')
if not use_local_file:
service = roslibpy.Service(self.ros, '/file_server/get_file', 'file_server/GetBinaryFile')
request = roslibpy.ServiceRequest(dict(name=url))
response = await_callback(service.call, 'callback', 'errback', request)
file_content = binascii.a2b_base64(response.data['value'])
# Just look away, we're about to do something nasty!
# namespaces are handled differently between the CLI and CPython
# XML parsers, so, we just get rid of it for DAE files
if file_extension == 'dae':
file_content = file_content.replace(b'xmlns="http://www.collada.org/2005/11/COLLADASchema"', b'')
# compas.files does not support file-like objects so we need to
# save the file to disk always. If local caching is enabled,
# we store it in the cache folder, otherwise, as a temp file.
_write_file(local_filename, file_content, 'wb')
else:
# Nothing to do here, the file will be read by the mesh importer
LOGGER.debug('Loading mesh file %s from local cache dir', local_filename)
return _fileserver_mesh_import(url, local_filename)
def add_attached_collision_mesh(self, attached_collision_mesh, options=None):
"""Add a collision mesh and attach it to the robot.
Parameters
----------
attached_collision_mesh : :class:`compas_fab.robots.AttachedCollisionMesh`
Object containing the collision mesh to be attached.
options : dict, optional
Unused parameter.
Returns
-------
``None``
"""
kwargs = {}
kwargs['attached_collision_mesh'] = attached_collision_mesh
kwargs['errback_name'] = 'errback'
return await_callback(self.add_attached_collision_mesh_async, **kwargs)
def remove_collision_mesh(self, id, options=None):
"""Remove a collision mesh from the planning scene.
Parameters
----------
id : str
Name of collision mesh to be removed.
options : dict, optional
Unused parameter.
Returns
-------
``None``
"""
kwargs = {}
kwargs['id'] = id
kwargs['errback_name'] = 'errback'
return await_callback(self.remove_collision_mesh_async, **kwargs)
def add_collision_mesh(self, collision_mesh, options=None):
"""Add a collision mesh to the planning scene.
Parameters
----------
collision_mesh : :class:`compas_fab.robots.CollisionMesh`
Object containing the collision mesh to be added.
options : dict, optional
Unused parameter.
Returns
-------
``None``
"""
kwargs = {}
kwargs['collision_mesh'] = collision_mesh
kwargs['errback_name'] = 'errback'
return await_callback(self.add_collision_mesh_async, **kwargs)
def compute_cartesian_path(self, frames, base_link, ee_link, group,
joint_names, joint_positions, max_step,
avoid_collisions, path_constraints,
attached_collision_object):
kwargs = {}
kwargs['frames'] = frames
kwargs['base_link'] = base_link
kwargs['ee_link'] = ee_link
kwargs['group'] = group
kwargs['joint_names'] = joint_names
kwargs['joint_positions'] = joint_positions
kwargs['max_step'] = max_step
kwargs['avoid_collisions'] = avoid_collisions
kwargs['path_constraints'] = path_constraints
kwargs['attached_collision_object'] = attached_collision_object
kwargs['errback_name'] = 'errback'
return await_callback(self.compute_cartesian_path_async, **kwargs)
def inverse_kinematics(self,
robot,
frame,
group,
start_configuration,
avoid_collisions=True,
constraints=None,
attempts=8,
attached_collision_meshes=None):
kwargs = {}
kwargs['robot'] = robot
kwargs['frame'] = frame
kwargs['group'] = group
kwargs['start_configuration'] = start_configuration
kwargs['avoid_collisions'] = avoid_collisions
kwargs['constraints'] = constraints
kwargs['attempts'] = attempts
kwargs['errback_name'] = 'errback'
return await_callback(self.inverse_kinematics_async, **kwargs)
def motion_plan_goal_frame(self,
frame,
base_link,
ee_link,
group,
joint_names,
joint_positions,
tolerance_position,
tolerance_angle,
path_constraints=None,
trajectory_constraints=None,
planner_id='',
num_planning_attempts=8,
allowed_planning_time=2.,
max_velocity_scaling_factor=1.,
max_acceleration_scaling_factor=1.,
attached_collision_object=None):
kwargs = {}
kwargs['frame'] = frame
kwargs['base_link'] = base_link
kwargs['ee_link'] = ee_link
kwargs['group'] = group
kwargs['joint_names'] = joint_names
kwargs['joint_positions'] = joint_positions
kwargs['tolerance_position'] = tolerance_position
kwargs['tolerance_angle'] = tolerance_angle
kwargs['path_constraints'] = path_constraints
kwargs['trajectory_constraints'] = trajectory_constraints
kwargs['planner_id'] = planner_id
kwargs['num_planning_attempts'] = num_planning_attempts
kwargs['allowed_planning_time'] = allowed_planning_time
kwargs['max_velocity_scaling_factor'] = max_velocity_scaling_factor
kwargs[
'max_acceleration_scaling_factor'] = max_acceleration_scaling_factor
kwargs['attached_collision_object'] = attached_collision_object
kwargs['errback_name'] = 'errback'
return await_callback(self.motion_plan_goal_frame_async, **kwargs)
def inverse_kinematics(self,
frame,
base_link,
group,
joint_names,
joint_positions,
avoid_collisions=True,
constraints=None,
attempts=8):
kwargs = {}
kwargs['frame'] = frame
kwargs['base_link'] = base_link
kwargs['group'] = group
kwargs['joint_names'] = joint_names
kwargs['joint_positions'] = joint_positions
kwargs['avoid_collisions'] = avoid_collisions
kwargs['constraints'] = constraints
kwargs['attempts'] = attempts
kwargs['errback_name'] = 'errback'
return await_callback(self.inverse_kinematics_async, **kwargs)
def get_planning_scene(self):
kwargs = {}
kwargs['errback_name'] = 'errback'
return await_callback(self.get_planning_scene_async, **kwargs)
def plan_motion(self,
robot,
goal_constraints,
start_configuration=None,
group=None,
options=None):
"""Calculates a motion path.
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which the motion plan is being calculated.
goal_constraints: list of :class:`compas_fab.robots.Constraint`
The goal to be achieved, defined in a set of constraints.
Constraints can be very specific, for example defining value domains
for each joint, such that the goal configuration is included,
or defining a volume in space, to which a specific robot link (e.g.
the end-effector) is required to move to.
start_configuration: :class:`compas_fab.robots.Configuration`, optional
The robot's full configuration, i.e. values for all configurable
joints of the entire robot, at the starting position. Defaults to
the all-zero configuration.
group: str, optional
The name of the group to plan for. Defaults to the robot's main
planning group.
options : dict, optional
Dictionary containing the following key-value pairs:
- ``"base_link"``: (:obj:`str`) Name of the base link.
- ``"path_constraints"``: (:obj:`list` of :class:`compas_fab.robots.Constraint`, optional)
Optional constraints that can be imposed along the solution path.
Note that path calculation won't work if the start_configuration
violates these constraints. Defaults to ``None``.
- ``"planner_id"``: (:obj:`str`)
The name of the algorithm used for path planning.
Defaults to ``'RRTConnect'``.
- ``"num_planning_attempts"``: (:obj:`int`, optional)
Normally, if one motion plan is needed, one motion plan is computed.
However, for algorithms that use randomization in their execution
(like 'RRT'), it is likely that different planner executions will
produce different solutions. Setting this parameter to a value above
``1`` will run many additional motion plans, and will report the
shortest solution as the final result. Defaults to ``1``.
- ``'allowed_planning_time'``: (:obj:`float`)
The number of seconds allowed to perform the planning. Defaults to ``2``.
- ``"max_velocity_scaling_factor"``: (:obj:`float`)
Defaults to ``1``.
- ``"max_acceleration_scaling_factor"``: (:obj:`float`)
Defaults to ``1``.
- ``"attached_collision_meshes"``: (:obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh`)
Defaults to ``None``.
Returns
-------
:class:`compas_fab.robots.JointTrajectory`
The calculated trajectory.
"""
options = options or {}
kwargs = {}
kwargs['goal_constraints'] = goal_constraints
kwargs['start_configuration'] = start_configuration
kwargs['group'] = group
kwargs['options'] = options
kwargs['errback_name'] = 'errback'
# Use base_link or fallback to model's root link
options['base_link'] = options.get('base_link', robot.model.root.name)
options['joints'] = {j.name: j.type for j in robot.model.joints}
return await_callback(self.plan_motion_async, **kwargs)
def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None):
"""Calculates a cartesian motion path (linear in tool space).
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which the cartesian motion plan is being calculated.
frames_WCF: list of :class:`compas.geometry.Frame`
The frames through which the path is defined.
start_configuration: :class:`Configuration`, optional
The robot's full configuration, i.e. values for all configurable
joints of the entire robot, at the starting position. Defaults to
the all-zero configuration.
group: str, optional
The planning group used for calculation. Defaults to the robot's
main planning group.
options: dict, optional
Dictionary containing the following key-value pairs:
- ``"base_link"``: (:obj:`str`) Name of the base link.
- ``"link"``: (:obj:`str`, optional) The name of the link to
calculate the forward kinematics for. Defaults to the group's end
effector link.
- ``"max_step"``: (:obj:`float`, optional) The approximate distance between the
calculated points. (Defined in the robot's units.) Defaults to ``0.01``.
- ``"jump_threshold"``: (:obj:`float`, optional)
The maximum allowed distance of joint positions between consecutive
points. If the distance is found to be above this threshold, the
path computation fails. It must be specified in relation to max_step.
If this threshold is ``0``, 'jumps' might occur, resulting in an invalid
cartesian path. Defaults to :math:`\\pi / 2`.
- ``"avoid_collisions"``: (:obj:`bool`, optional)
Whether or not to avoid collisions. Defaults to ``True``.
- ``"path_constraints"``: (:obj:`list` of :class:`compas_fab.robots.Constraint`, optional)
Optional constraints that can be imposed along the solution path.
Note that path calculation won't work if the start_configuration
violates these constraints. Defaults to ``None``.
- ``"attached_collision_meshes"``: (:obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh`)
Defaults to ``None``.
Returns
-------
:class:`compas_fab.robots.JointTrajectory`
The calculated trajectory.
"""
options = options or {}
kwargs = {}
kwargs['options'] = options
kwargs['frames_WCF'] = frames_WCF
kwargs['start_configuration'] = start_configuration
kwargs['group'] = group
kwargs['errback_name'] = 'errback'
# Use base_link or fallback to model's root link
options['base_link'] = options.get('base_link', robot.model.root.name)
options['joint_names'] = robot.get_configurable_joint_names()
options['joint_types'] = robot.get_configurable_joint_types()
options['link'] = options.get('link') or robot.get_end_effector_link_name(group)
if options['link'] not in robot.get_link_names(group):
raise ValueError('Link name {} does not exist in planning group'.format(options['link']))
return await_callback(self.plan_cartesian_motion_async, **kwargs)
