class MoveItRemoveAttachedCollisionMesh(RemoveAttachedCollisionMesh):
"""Callable to remove an attached collision mesh from the robot."""
APPLY_PLANNING_SCENE = ServiceDescription(
'/apply_planning_scene',
'ApplyPlanningScene',
ApplyPlanningSceneRequest,
ApplyPlanningSceneResponse,
)
def __init__(self, ros_client):
self.ros_client = ros_client
def remove_attached_collision_mesh(self, id, options=None):
"""Remove an attached collision mesh from the robot.
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_attached_collision_mesh_async,
**kwargs)
def remove_attached_collision_mesh_async(self, callback, errback, id):
aco = AttachedCollisionObject()
aco.object.id = id
aco.object.operation = CollisionObject.REMOVE
robot_state = RobotState(attached_collision_objects=[aco],
is_diff=True)
scene = PlanningScene(robot_state=robot_state, is_diff=True)
request = scene.to_request(self.ros_client.ros_distro)
self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)
class MoveItRemoveCollisionMesh(RemoveCollisionMesh):
"""Callable to remove a collision mesh from the planning scene."""
APPLY_PLANNING_SCENE = ServiceDescription(
'/apply_planning_scene',
'ApplyPlanningScene',
ApplyPlanningSceneRequest,
ApplyPlanningSceneResponse,
)
def __init__(self, ros_client):
self.ros_client = ros_client
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 remove_collision_mesh_async(self, callback, errback, id):
co = CollisionObject()
co.id = id
co.operation = CollisionObject.REMOVE
world = PlanningSceneWorld(collision_objects=[co])
scene = PlanningScene(world=world, is_diff=True)
request = dict(scene=scene)
self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)
class MoveItAddAttachedCollisionMesh(AddAttachedCollisionMesh):
"""Callable to add a collision mesh and attach it to the robot."""
APPLY_PLANNING_SCENE = ServiceDescription('/apply_planning_scene',
'ApplyPlanningScene',
ApplyPlanningSceneRequest,
ApplyPlanningSceneResponse,
)
def __init__(self, ros_client):
self.ros_client = ros_client
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 add_attached_collision_mesh_async(self, callback, errback, attached_collision_mesh):
aco = AttachedCollisionObject.from_attached_collision_mesh(
attached_collision_mesh)
aco.object.operation = CollisionObject.ADD
world = PlanningSceneWorld(collision_objects=[aco.object])
scene = PlanningScene(world=world, is_diff=True)
request = dict(scene=scene)
self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)
class MoveItResetPlanningScene(ResetPlanningScene):
"""Callable to add a collision mesh to the planning scene."""
APPLY_PLANNING_SCENE = ServiceDescription(
'/apply_planning_scene',
'ApplyPlanningScene',
ApplyPlanningSceneRequest,
ApplyPlanningSceneResponse,
)
def __init__(self, ros_client):
self.ros_client = ros_client
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 reset_planning_scene_async(self, callback, errback):
scene = self.ros_client.get_planning_scene()
for collision_object in scene.world.collision_objects:
collision_object.operation = CollisionObject.REMOVE
for collision_object in scene.robot_state.attached_collision_objects:
collision_object.object['operation'] = CollisionObject.REMOVE
scene.is_diff = True
scene.robot_state.is_diff = True
request = scene.to_request(self.ros_client.ros_distro)
self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)
class MoveItAppendCollisionMesh(AppendCollisionMesh):
"""Callable to append a collision mesh to the planning scene."""
APPLY_PLANNING_SCENE = ServiceDescription(
'/apply_planning_scene',
'ApplyPlanningScene',
ApplyPlanningSceneRequest,
ApplyPlanningSceneResponse,
)
def __init__(self, ros_client):
self.ros_client = ros_client
def append_collision_mesh(self, collision_mesh, options=None):
"""Append a collision mesh to the planning scene.
Parameters
----------
collision_mesh : :class:`compas_fab.robots.CollisionMesh`
Object containing the collision mesh to be appended.
options : dict, optional
Unused parameter.
Returns
-------
``None``
"""
kwargs = {}
kwargs['collision_mesh'] = collision_mesh
kwargs['errback_name'] = 'errback'
return await_callback(self.append_collision_mesh_async, **kwargs)
def append_collision_mesh_async(self, callback, errback, collision_mesh):
co = CollisionObject.from_collision_mesh(collision_mesh)
co.operation = CollisionObject.APPEND
world = PlanningSceneWorld(collision_objects=[co])
scene = PlanningScene(world=world, is_diff=True)
request = scene.to_request(self.ros_client.ros_distro)
self.APPLY_PLANNING_SCENE(self.ros_client, request, callback, errback)
class MoveItPlanningScene(GetPlanningScene):
"""Callable to retrieve the planning scene."""
GET_PLANNING_SCENE = ServiceDescription('/get_planning_scene',
'GetPlanningScene',
GetPlanningSceneRequest,
GetPlanningSceneResponse)
def __init__(self, ros_client):
self.ros_client = ros_client
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 get_planning_scene_async(self, callback, errback):
request = dict(components=PlanningSceneComponents(
PlanningSceneComponents.SCENE_SETTINGS
| PlanningSceneComponents.ROBOT_STATE
| PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS
| PlanningSceneComponents.WORLD_OBJECT_NAMES
| PlanningSceneComponents.WORLD_OBJECT_GEOMETRY
| PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.OBJECT_COLORS))
self.GET_PLANNING_SCENE(self.ros_client, request, callback, errback)
class MoveItPlanMotion(PlanMotion):
"""Callable to find a path plan to move the selected robot from its current position within the `goal_constraints`."""
GET_MOTION_PLAN = ServiceDescription('/plan_kinematic_path',
'GetMotionPlan', MotionPlanRequest,
MotionPlanResponse, validate_response)
def __init__(self, ros_client):
self.ros_client = ros_client
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_motion_async(self,
callback,
errback,
goal_constraints,
start_configuration=None,
group=None,
options=None):
"""Asynchronous handler of MoveIt motion planner service."""
# http://docs.ros.org/jade/api/moveit_core/html/utils_8cpp_source.html
# TODO: if list of frames (goals) => receive multiple solutions?
joints = options['joints']
header = Header(frame_id=options['base_link'])
joint_state = JointState(header=header,
name=start_configuration.joint_names,
position=start_configuration.joint_values)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header),
is_diff=True)
if options.get('attached_collision_meshes'):
for acm in options['attached_collision_meshes']:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
# convert constraints
goal_constraints = [
convert_constraints_to_rosmsg(goal_constraints, header)
]
path_constraints = convert_constraints_to_rosmsg(
options.get('path_constraints'), header)
trajectory_constraints = options.get('trajectory_constraints')
if trajectory_constraints is not None:
trajectory_constraints = TrajectoryConstraints(
constraints=convert_constraints_to_rosmsg(
options['trajectory_constraints'], header))
request = dict(
start_state=start_state,
goal_constraints=goal_constraints,
path_constraints=path_constraints,
trajectory_constraints=trajectory_constraints,
planner_id=options.get('planner_id', 'RRTConnect'),
group_name=group,
num_planning_attempts=options.get('num_planning_attempts', 1),
allowed_planning_time=options.get('allowed_planning_time', 2.),
max_velocity_scaling_factor=options.get(
'max_velocity_scaling_factor', 1.),
max_acceleration_scaling_factor=options.get(
'max_acceleration_scaling_factor', 1.))
# workspace_parameters=options.get('workspace_parameters')
def response_handler(response):
try:
trajectory = convert_trajectory(joints, response.trajectory,
response.trajectory_start, 1.,
response.planning_time,
response)
callback(trajectory)
except Exception as e:
errback(e)
self.GET_MOTION_PLAN(self.ros_client, request, response_handler,
errback)
class MoveItForwardKinematics(ForwardKinematics):
"""Callable to calculate the robot's forward kinematic."""
GET_POSITION_FK = ServiceDescription('/compute_fk', 'GetPositionFK',
GetPositionFKRequest,
GetPositionFKResponse,
validate_response)
def __init__(self, ros_client):
self.ros_client = ros_client
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 forward_kinematics_async(self, callback, errback, configuration,
options):
"""Asynchronous handler of MoveIt FK service."""
base_link = options['base_link']
fk_link_names = [options['link']]
header = Header(frame_id=base_link)
joint_state = JointState(name=configuration.joint_names,
position=configuration.values,
header=header)
robot_state = RobotState(joint_state,
MultiDOFJointState(header=header))
def convert_to_frame(response):
callback(response.pose_stamped[0].pose.frame)
self.GET_POSITION_FK(self.ros_client,
(header, fk_link_names, robot_state),
convert_to_frame, errback)
class MoveItPlanCartesianMotion(PlanCartesianMotion):
"""Callable to calculate a cartesian motion path (linear in tool space)."""
GET_CARTESIAN_PATH = ServiceDescription('/compute_cartesian_path',
'GetCartesianPath',
GetCartesianPathRequest,
GetCartesianPathResponse,
validate_response)
def __init__(self, ros_client):
self.ros_client = ros_client
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)
def plan_cartesian_motion_async(self, callback, errback,
frames_WCF, start_configuration=None, group=None, options=None):
"""Asynchronous handler of MoveIt cartesian motion planner service."""
joint_names = options['joint_names']
joint_types = options['joint_types']
joint_type_by_name = dict(zip(joint_names, joint_types))
header = Header(frame_id=options['base_link'])
waypoints = [Pose.from_frame(frame) for frame in frames_WCF]
joint_state = JointState(header=header,
name=start_configuration.joint_names,
position=start_configuration.values)
start_state = RobotState(joint_state, MultiDOFJointState(header=header))
if options.get('attached_collision_meshes'):
for acm in options['attached_collision_meshes']:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
path_constraints = convert_constraints_to_rosmsg(options.get('path_constraints'), header)
request = dict(header=header,
start_state=start_state,
group_name=group,
link_name=options['link'],
waypoints=waypoints,
max_step=float(options.get('max_step', 0.01)),
jump_threshold=float(options.get('jump_threshold', 1.57)),
avoid_collisions=bool(options.get('avoid_collisions', True)),
path_constraints=path_constraints)
def convert_to_trajectory(response):
try:
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = response.fraction
trajectory.joint_names = response.solution.joint_trajectory.joint_names
joint_types = [joint_type_by_name[name] for name in trajectory.joint_names]
trajectory.points = convert_trajectory_points(
response.solution.joint_trajectory.points, joint_types)
start_state = response.start_state.joint_state
start_state_types = [joint_type_by_name[name] for name in start_state.name]
trajectory.start_configuration = Configuration(start_state.position, start_state_types, start_state.name)
callback(trajectory)
except Exception as e:
errback(e)
self.GET_CARTESIAN_PATH(self.ros_client, request, convert_to_trajectory, errback)
class MoveItInverseKinematics(InverseKinematics):
"""Callable to calculate the robot's inverse kinematics for a given frame."""
GET_POSITION_IK = ServiceDescription('/compute_ik', 'GetPositionIK',
GetPositionIKRequest,
GetPositionIKResponse,
validate_response)
def __init__(self, ros_client):
self.ros_client = ros_client
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``.
- ``"attempts"``: (:obj:`int`, optional) The maximum number of inverse kinematic attempts.
Defaults to ``8``.
- ``"attached_collision_meshes"``: (:obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh`, optional)
Defaults to ``None``.
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 inverse_kinematics_async(self,
callback,
errback,
frame_WCF,
start_configuration=None,
group=None,
options=None):
"""Asynchronous handler of MoveIt IK service."""
base_link = options['base_link']
header = Header(frame_id=base_link)
pose_stamped = PoseStamped(header, Pose.from_frame(frame_WCF))
joint_state = JointState(name=start_configuration.joint_names,
position=start_configuration.values,
header=header)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if options.get('attached_collision_meshes'):
for acm in options['attached_collision_meshes']:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
constraints = convert_constraints_to_rosmsg(options.get('constraints'),
header)
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=options.get(
'avoid_collisions', True),
attempts=options.get('attempts', 8))
def convert_to_positions(response):
callback((response.solution.joint_state.position,
response.solution.joint_state.name))
self.GET_POSITION_IK(self.ros_client, (ik_request, ),
convert_to_positions, errback)
class MoveItInverseKinematics(InverseKinematics):
"""Callable to calculate the robot's inverse kinematics for a given frame."""
GET_POSITION_IK = ServiceDescription('/compute_ik', 'GetPositionIK',
GetPositionIKRequest,
GetPositionIKResponse,
validate_response)
def __init__(self, ros_client):
self.ros_client = ros_client
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.
- ``"max_results"``: (:obj:`int`) Maximum number of results to return.
Defaults to ``100``.
Raises
------
compas_fab.backends.exceptions.BackendError
If no configuration can be found.
Yields
-------
: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'
max_results = options.get('max_results', 100)
# Use base_link or fallback to model's root link
options['base_link'] = options.get('base_link', robot.model.root.name)
for _ in range(max_results):
yield await_callback(self.inverse_kinematics_async, **kwargs)
def inverse_kinematics_async(self,
callback,
errback,
frame_WCF,
start_configuration=None,
group=None,
options=None):
"""Asynchronous handler of MoveIt IK service."""
base_link = options['base_link']
header = Header(frame_id=base_link)
pose_stamped = PoseStamped(header, Pose.from_frame(frame_WCF))
joint_state = JointState(name=start_configuration.joint_names,
position=start_configuration.joint_values,
header=header)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if options.get('attached_collision_meshes'):
for acm in options['attached_collision_meshes']:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
constraints = convert_constraints_to_rosmsg(options.get('constraints'),
header)
timeout_in_secs = options.get('timeout', 2)
timeout_duration = Duration(timeout_in_secs, 0).to_data()
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=options.get(
'avoid_collisions', True),
attempts=options.get('attempts', 8),
timeout=timeout_duration)
# The field `attempts` was removed in Noetic (and higher)
# so it needs to be removed from the message otherwise it causes a serialization error
# https://github.com/ros-planning/moveit/pull/1288
if self.ros_client.ros_distro not in (RosDistro.KINETIC,
RosDistro.MELODIC):
del ik_request.attempts
def convert_to_positions(response):
callback((response.solution.joint_state.position,
response.solution.joint_state.name))
self.GET_POSITION_IK(self.ros_client, (ik_request, ),
convert_to_positions, errback)
