# motion_client.py
#
# Copyright (c) 2018, Xamla and/or its affiliates. All rights reserved.
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
#!/usr/bin/env python3
from datetime import timedelta
from typing import Union, Tuple
import numpy as np
from xamla_motion.data_types import (Pose,
CartesianPath,
EndEffectorPose,
IkResults,
JointPath,
JointSet,
JointValues,
JointStates,
JointTrajectory,
PlanParameters,
TaskSpacePlanParameters)
from xamla_motion.motion_operations import (MoveCartesianArgs,
MoveCartesianCollisionFreeOperation,
MoveCartesianLinearOperation,
MoveCartesianOperation, MoveJointsArgs,
MoveJointsCollisionFreeOperation,
MoveJointsOperation)
from xamla_motion.motion_service import MotionService, SteppedMotionClient
[docs]class MoveGroup(object):
"""
Class with encapsulate move functionality for a specific move group
"""
# Methods
# -------
# set_default_end_effector(end_effector_name: str)
# Set one of the end effector from the list of available ones as default
# get_end_effector(name: Union[None, str]) -> xamla_motion.v2.EndEffector
# Get the end effector specified by name or the default end effector
# get_current_joint_states() -> JointStates
# Returns the current joint states of the move group joints
# get_current_joint_positions() -> JointValues
# Returns the current joint positions of the move group joints
# move_joints(target: Union[JointValues, JointPath], velocity_scaling: Union[None, float]=None,
# collision_check: Union[None, bool]=None, max_deviation: Union[None, float]=None,
# acceleration_scaling: Union[None, float]=None) -> MoveJointsOperation
# Create MoveJointsOperation for target joint positions
# move_joints_collision_free(target: Union[JointValues, JointPath], velocity_scaling: Union[None, float]=None,
# collision_check: Union[None, bool]=None, max_deviation: Union[None, float]=None,
# acceleration_scaling: Union[None, float]=None) -> MoveJointsCollisionFreeOperation
# Create MoveJointsCollisionFreeOperation for target joint positions
def __init__(self, move_group_name: Union[None, str] = None,
end_effector_name: Union[None, str] = None, motion_service=None):
"""
Initialize MoveGroup class
The initialization process is possible with different settings:
move_group_name and end_effector_name are None:
In this case the first available move group and its
first end effector is selected automatically. If no
move group or end_effector is available exception is
raised.
move_group_name is defined and end_effector_name is none:
In this case the initialization routine tries to find
the move group with requested name. If it is available
create a instance for this move group and the first
available end effector as default end effector. Else
an exceptions is raised.
move_group is None and end_effector_name is defined:
In this case the initialization routine tries to find
the move group with requested end effector. If it is
available create a instance for this move group and the
requested end effector as default end effector. Else
an exceptions is raised.
move_group is defined and also end_effector_name is defined:
In this case the initialization routine tries to find
the requested move group with requested end effector.
If it is available create a instance for this move group
and the requested end effector as default end effector. Else
an exceptions is raised.
Parameters
----------
move_group_name : Union[None, str]
If defined name of the move group which this
instance should represent
end_effector_name : Union[None, str]
If defined name of the end_effector which should
be used as default end_effector
motion_service : Union[None, MotionService]
An instance of MotionService which is used to
communicate with the motion server if None a
new instance of MotionService is created
Raises
------
TypeError
If motion_service is not of type MotionService
or If the other inputs are not str convertable
ServiceException
If necessary services to query available move
groups and end effectors are not available
RuntimeError
If requested move group or end effector not
exist or are not available
"""
if not motion_service:
motion_service = MotionService()
if not isinstance(motion_service, MotionService):
raise TypeError('motion_service is not of'
' expected type MotionService')
self.__m_service = motion_service
groups = self.__m_service.query_available_move_groups()
if not groups:
raise RuntimeError('no move groups available')
if end_effector_name:
end_effector_name = str(end_effector_name)
if move_group_name:
move_group_name = str(move_group_name)
if not end_effector_name and not move_group_name:
move_group_name = groups[0].name
try:
end_effector_name = groups[0].end_effector_names[0]
except IndexError:
end_effector_name = None
details = groups[0]
elif end_effector_name and not move_group_name:
try:
details = next(g for g in groups
if any([end_effector_name in
g.end_effector_names]))
except StopIteration:
raise RuntimeError('it exist no move group with'
' end effector: '
+ end_effector_name)
move_group_name = details.name
elif not end_effector_name and move_group_name:
try:
details = next(g for g in groups
if g.name == move_group_name)
except StopIteration:
raise RuntimeError('it exist no move group with'
' name: ' + move_group_name)
try:
end_effector_name = details.end_effector_names[0]
except IndexError:
end_effector_name = None
else:
try:
details = next(g for g in groups
if any([end_effector_name in
g.end_effector_names]))
except StopIteration:
raise RuntimeError('move group: ' + move_group_name +
' with end effector: '
+ end_effector_name +
' not exists')
self.__name = move_group_name
self.__details = details
self.__joint_set = self.__details.joint_set
p = self.__m_service.create_plan_parameters(move_group_name,
self.__joint_set,
sample_resolution=0.05,
collision_check=True)
self.__plan_parameters = p
names = self.__details.end_effector_names
if names:
link_names = self.__details.end_effector_link_names
self.__end_effectors = {name: EndEffector(self,
name,
link_names[i])
for i, name in enumerate(names)}
self.set_default_end_effector(end_effector_name)
else:
self.__end_effectors = {}
self.__selected_end_effector = None
self.__task_space_plan_parameters = None
self.__velocity_scaling = 1.0
self.__acceleration_scaling = 1.0
@property
def name(self):
"""
name : str (read only)
move group name
"""
return self.__name
@property
def motion_service(self):
"""
motion_service : MotionService (read only)
return the instance of motion service which is used
to communicate via ros which the motion server
"""
return self.__m_service
@property
def end_effector_names(self):
"""
end_effector_names : List[str] (read only)
List of end effectors the move groups contains
"""
return self.__details.end_effector_names
@property
def selected_end_effector(self):
"""
selected_end_effector : str (read only)
Name of the currently selected end effector
"""
return self.__selected_end_effector
@property
def joint_set(self):
"""
joint_set : JointSet (read only)
joint set of the move group
"""
return self.__details.joint_set
@property
def default_plan_parameters(self):
"""
plan_parameters : PlanParameters (read only)
Instance of PlanParameters from which the
limits and settings are used when no specific
user input is given
"""
return self.__plan_parameters
@property
def default_task_space_plan_parameters(self):
"""
task_space_plan_parameters : TaskSpacePlanParameters (read only)
Instance of TaskSpacePlanParameters from which the
limits and settings are used when no specific
user input is given
"""
return self.__task_space_plan_parameters
@property
def collision_check(self):
"""
collision_check : bool
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
"""
return self.__plan_parameters.collision_check
@collision_check.setter
def collision_check(self, value: bool):
"""
Set collision check on or of
Parameters
----------
value : bool
activate collision check or deactivate it
"""
self.__plan_parameters.with_collision_check(value)
if self.__task_space_plan_parameters:
self.__task_space_plan_parameters.with_collision_check(value)
@property
def sample_resolution(self):
"""
sample_resolution : float
Trajectory point sampling frequency
If value smaller one in seconds else in Hz
"""
return self.__plan_parameters.sample_resolution
@sample_resolution.setter
def sample_resolution(self, value: float):
"""
Set trajectory point sampling frequency
Parameters
----------
value : float convertable
If value smaller one interpreted as in
seconds else in Hz
"""
self.__plan_parameters.with_sample_resolution(value)
if self.__task_space_plan_parameters:
self.__task_space_plan_parameters.with_sample_resolution(value)
@property
def max_deviation(self):
"""
max_deviation : Union[None, float]Defines the maximal deviation from trajectory points
when it is a fly-by-point in joint space
"""
return self.__plan_parameters.max_deviation
@max_deviation.setter
def max_deviation(self, value: float):
"""
Set maximal devitation
Parameters
----------
value : float convertable
maximal deviation
"""
self.__plan_parameters.with_max_deviation(value)
if self.__task_space_plan_parameters:
self.__task_space_plan_parameters.with_max_deviation(value)
@property
def ik_jump_threshold(self):
"""
ik_jump_threshold : float
maximal allowed inverse kinematics jump threshold
"""
if self.__task_space_plan_parameters:
return self.__task_space_plan_parameters.ik_jump_threshold
else:
raise RuntimeError('task space plan parameters not defined'
' because move group has no end effector')
@ik_jump_threshold.setter
def ik_jump_threshold(self, value: float):
"""
Set maximal allowed inverse kinematics jump threshold
Parameters
----------
value : float convertable
new maximal allowed inverse kinematics jump threshold
"""
if not self.__task_space_plan_parameters:
raise RuntimeError('task space plan parameters not defined'
' because move group has no end effector')
self.__task_space_plan_parameters.with_ik_jump_threshold(value)
@property
def velocity_scaling(self):
"""
Default velocity scaling which is applied if no custom value is provided
"""
return self.__velocity_scaling
@velocity_scaling.setter
def velocity_scaling(self, value: float):
"""
Set default velocity scaling which is applied if no custom value is provided
Parameters
----------
value: float convertable
new default velocity scaling value
"""
if value > 1.0 or value < 0.0:
raise ValueError('value is not'
' between 0.0 and 1.0')
self.__velocity_scaling = value
@property
def acceleration_scaling(self):
"""
Default acceleration scaling which is applied if no custom value is provided
"""
return self.__acceleration_scaling
@acceleration_scaling.setter
def acceleration_scaling(self, value: float):
"""
Set default acceleration scaling which is applied if no custom value is provided
Parameters
----------
value: float convertable
new default acceleration scaling value
"""
if value > 1.0 or value < 0.0:
raise ValueError('value is not'
' between 0.0 and 1.0')
self.__acceleration_scaling = value
[docs] def set_default_end_effector(self, end_effector_name: str):
"""
Set one of the end effector from the list of available ones as default
Parameters
----------
end_effector_name : str convertable
Name of the end effector which should be now the
default end effector of this move group
Raises
------
TypeError
If end_effector_name is not str convertable
RuntimeError
If the input end effector name is not available
"""
end_effector_name = str(end_effector_name)
try:
self.__end_effectors[end_effector_name]
except KeyError as exc:
raise RuntimeError('move group' + self.__name +
' has no end effector with name: ' +
end_effector_name) from exc
self.__selected_end_effector = end_effector_name
p = self.__m_service.create_task_space_plan_parameters(
end_effector_name,
sample_resolution=0.05,
collision_check=False)
self.__task_space_plan_parameters = p
[docs] def get_end_effector(self, name: Union[None, str] = None) -> 'xamla_motion.v2.EndEffector':
"""
Get the end effector specified by name or
the default end effector if name is not provided
Parameters
---------
name : Union[None, str]
Name of the end effector which should be returned
Returns
-------
end_effector : xamla_motion.v2.EndEffector
Instance of Class EndEffector with requested name
Raises
------
RuntimeError
If no end effector exist with the requested name
"""
if name:
try:
return self.__end_effectors[name]
except KeyError as exc:
raise RuntimeError('end effector with name' + name +
'is not available for move group: '
+ self.__name) from exc
else:
try:
return self.__end_effectors[self.__selected_end_effector]
except KeyError as exc:
raise RuntimeError('move group: ' + self.__name +
' does not have any end effectors') from exc
[docs] def get_current_joint_states(self) -> JointStates:
"""
Returns the current joint states of the move group joints
Returns
-------
joint_states : JointStates
current joint states of the move group joints
Raises
------
ServiceException
If Service is not available or finish
not successful
"""
return self.__m_service.query_joint_states(self.joint_set)
[docs] def get_current_joint_positions(self) -> JointValues:
"""
Returns the current joint positions of the move group joints
Returns
-------
joint_positions : JointValues
current joint position of the move group joints
Raises
------
ServiceException
If Service is not available or finish
not successful
"""
return self.get_current_joint_states().positions
def _build_plan_parameters(self,
velocity_scaling: Union[None, float] = None,
collision_check: Union[None, bool] = None,
max_deviation: Union[None, float] = None,
acceleration_scaling: Union[None, float] = None,
sample_resolution: Union[None, float] = None
) -> PlanParameters:
"""
Build an instance of PlanParameters from input and default values
All attributes with value None are ignored and instead the default
values are used defined in default_plan_parameters are used
Parameters
----------
velocity_scaling : Union[None, float]
Scaling factor which is applied on the maximal
possible joint velocities
collision_check : Union[None, bool]
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
max_deviation : Union[None, float]
Defines the maximal deviation from trajectory points
when it is a fly-by-point in joint space
acceleration_scaling : Union[None, float]
Scaling factor which is applied on the maximal
possible joint accelerations
sample_resolution : Union[None, float]
target sample frequency in Hz
Returns
-------
PlanParameters
New instance of PlanParameters with requested changes
from the default instance
Raises
------
TypeError
If inpute values are not convertable to specified types
ValueError
If scaling inputs are not between 0.0 and 1.0
"""
builder = self.__plan_parameters.to_builder()
if velocity_scaling:
builder.scale_velocity(velocity_scaling)
if collision_check:
builder.collision_check = collision_check
if max_deviation:
builder.max_deviation = max_deviation
if acceleration_scaling:
builder.scale_acceleration(acceleration_scaling)
if sample_resolution:
builder.sample_resolution = sample_resolution
return builder.build()
def _build_task_space_plan_parameters(self,
velocity_scaling: Union[None, float] = None,
collision_check: Union[None, bool] = None,
max_deviation: Union[None, float] = None,
acceleration_scaling: Union[None, float] = None,
sample_resolution: Union[None, float] = None,
ik_jump_threshold: Union[None, float] = None,
end_effector_name: Union[None,
str] = None
) -> TaskSpacePlanParameters:
"""
Build an instance of TaskSpacePlanParameters from input
and default values
All attributes with value None are ignored and instead the default
values are used defined in default_task_space_plan_parameters
are used
Parameters
----------
velocity_scaling : Union[None, float]
Scaling factor which is applied on the maximal
possible joint velocities
collision_check : Union[None, bool]
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
max_deviation : Union[None, float]
Defines the maximal deviation from trajectory points
when it is a fly-by-point in joint space
acceleration_scaling : Union[None, float]
Scaling factor which is applied on the maximal
possible joint accelerations
sample_resolution : Union[None, float]
target sample frequency in Hz
ik_jump_threshold : Union[None, float]
Maximal joint value jump between two consecutively
following trajectory points
end_effector_name : Union[None, str]
Name of end effector which should be used
Returns
-------
TaskSpacePlanParameters
New instance of TaskSpacePlanParameters with requested changes
from the default instance
Raises
------
TypeError
If input values are not convertable to specified types
ValueError
If scaling inputs are not between 0.0 and 1.0
"""
end_effector_name = str(end_effector_name)
if (end_effector_name is not None and
end_effector_name != self.__selected_end_effector):
try:
self.__end_effectors[end_effector_name]
except KeyError as exc:
raise RuntimeError('move group' + self.__name +
' has no end effector with name: ' +
end_effector_name) from exc
p = self.__m_service.create_task_space_plan_parameters(
end_effector_name)
else:
p = self.__task_space_plan_parameters
builder = p.to_builder()
if velocity_scaling:
builder.scale_velocity(velocity_scaling)
if collision_check:
builder.collision_check = collision_check
if max_deviation:
builder.max_deviation = max_deviation
if acceleration_scaling:
builder.scale_acceleration(acceleration_scaling)
if sample_resolution:
builder.sample_resolution = sample_resolution
if ik_jump_threshold:
builder.ik_jump_threshold = ik_jump_threshold
return builder.build()
[docs] def move_joints(self,
target: Union[JointValues, JointPath],
velocity_scaling: Union[None, float] = None,
collision_check: Union[None, bool] = None,
max_deviation: Union[None, float] = None,
acceleration_scaling: Union[None, float] = None
) -> MoveJointsOperation:
"""
Create MoveJointsOperation for target joint positions
Parameters
----------
target : Union[JointValues, JointPath]
Target joint positions or target JointPath
velocity_scaling : Union[None, float] (default None)
Scaling factor which is applied on the maximal
possible joint velocities
If None the MoveGroup default is used
collision_check : Union[None, bool] (default None)
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
If None the MoveGroup default is used
max_deviation : Union[None, float] (default None)
Defines the maximal deviation from trajectory points
when it is a fly-by-point in joint space
If None the MoveGroup default is used
acceleration_scaling : Union[None, float] (default None)
Scaling factor which is applied on the maximal
possible joint accelerations
If None the MoveGroup default is used
Returns
-------
move_joint_operations : MoveJointsOperation
Instance of MoveJointsOperation
Raises
------
TypeError
If target is not one of types JointValues, JointPath
If all other inputs are not convertable to specified types
"""
if not isinstance(target, (JointValues, JointPath)):
raise TypeError('target is not one of expected types'
' JointValues, JointPath')
args = MoveJointsArgs()
args.move_group = self
args.target = target
args.velocity_scaling = float(velocity_scaling or
self.__velocity_scaling)
args.acceleration_scaling = float(acceleration_scaling or
self.__acceleration_scaling)
args.collision_check = bool(collision_check or
self.__plan_parameters.collision_check)
args.max_deviation = float(max_deviation or
self.__plan_parameters.max_deviation)
args.sample_resolution = self.__plan_parameters.sample_resolution
return MoveJointsOperation(args)
[docs] def move_joints_collision_free(self, target: Union[JointValues, JointPath],
velocity_scaling: Union[None, float] = None,
collision_check: Union[None, bool] = None,
max_deviation: Union[None, float] = None,
acceleration_scaling: Union[None, float] = None):
"""
Create MoveJointsCollisionFreeOperation for target joint positions
Parameters
----------
target : Union[JointValues, JointPath]
Target joint positions or target JointPath
velocity_scaling : Union[None, float] (default None)
Scaling factor which is applied on the maximal
possible joint velocities
If None the MoveGroup default is used
collision_check : Union[None, bool] (default None)
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
If None the MoveGroup default is used
max_deviation : Union[None, float] (default None)
Defines the maximal deviation from trajectory points
when it is a fly-by-point in joint space
If None the MoveGroup default is used
acceleration_scaling : Union[None, float] (default None)
Scaling factor which is applied on the maximal
possible joint accelerations
If None the MoveGroup default is used
Returns
-------
MoveJointsCollisionFreeOperation
Instance of MoveJointsCollisionFreeOperation
Raises
------
TypeError
If target is not one of types JointValues, JointPath
If all other inputs are not convertable to specified types
"""
if not isinstance(target, (JointValues, JointPath)):
raise TypeError('target is not one of expected types'
' JointValues, JointPath')
args = MoveJointsArgs()
args.move_group = self
args.target = target
args.velocity_scaling = float(velocity_scaling or
self.__velocity_scaling)
args.acceleration_scaling = float(acceleration_scaling or
self.__acceleration_scaling)
args.collision_check = bool(collision_check or
self.__plan_parameters.collision_check)
args.max_deviation = float(max_deviation or
self.__plan_parameters.max_deviation)
args.sample_resolution = self.__plan_parameters.sample_resolution
return MoveJointsCollisionFreeOperation(args)
def __str__(self):
return 'MoveGroup: {})'.format(self.__name)
[docs]class EndEffector(object):
"""
Class with encapsulate move functionality for a specific end effector
"""
# Methods
# -------
# from_end_effector_name(end_effector_name: str) -> EndEffector
# Creates an instance of MoveGroup and select the correct instance of EndEffector
# get_current_pose() -> Pose
# Returns the current pose of the end effector
# compute_pose(joint_values: JointValues) -> Pose
# compute pose from joint values / configuration
# inverse_kinematics(
# poses: Union[Pose, CartesianPath], collision_check: Union[None, bool], seed: Union[None, JointValues],
# timeout: Union[None, datatime.timedelta], const_seed: bool, attempts: int
# ) -> JointValues:
# inverse kinematic solutions for one pose
# inverse_kinematics_many(
# poses: Union[Pose, CartesianPath], collision_check: Union[None, bool], seed: Union[None, JointValues],
# timeout: Union[None, datatime.timedelta], const_seed: bool, attempts: int
# ) -> IkResults:
# inverse kinematic solutions for many poses
# move_cartesian(
# target: Union[Pose, CartesianPath], seed: Union[None, JointValues]=None,
# velocity_scaling: Union[None, float]=None, collision_check: Union[None, bool]=None,
# max_deviation: Union[None, float]=None, acceleration_scaling: Union[None, float]=None
# ) -> MoveCartesianOperation
# Create MoveCartesianOperation for target joint positions
# move_cartesian_collision_free(
# target: Union[Pose, CartesianPath], seed=Union[None, JointValues], velocity_scaling: Union[None, float]=None,
# max_deviation: Union[None, float]=None, acceleration_scaling: Union[None, float]=None
# ) -> MoveCartesianCollisionFreeOperation
# Create MoveCartesianCollisionFreeOperation for target joint positions
# move_poses_linear(
# target: Union[Pose, CartesianPath], velocity_scaling: Union[None, float]=None,
# collision_check: Union[None, bool]=None, max_deviation: Union[None, float]=None,
# acceleration_scaling: Union[None, float]=None
# ) -> MoveCartesianLinearOperation
# Create MoveCartesianLinearOperation for target joint positions
def __init__(self, move_group: MoveGroup, end_effector_name: str,
end_effector_link_name: str):
"""
Initialize EndEffector class
Parameters
----------
move_group : xamla_motion.v2.MoveGroup
Instance of move group where the endeffector belongs
to
end_effector_name : str_convertable
If defined name of the end_effector which should
for which a instance of EndEffector is created
and which is selected as default for the move
group
end_effector_link_name : str convertable
name of the end effector link
Raises
------
TypeError
If move_group is not of type MoveGroup
or If the other inputs are not str convertable
ServiceException
If necessary services to query available move
groups and end effectors are not available
RuntimeError
If requested move group or end effector not
exist or are not available
"""
if not isinstance(move_group, MoveGroup):
raise TypeError('move_group is not of expected'
' type MoveGroup')
self.__name = str(end_effector_name)
self.__move_group = move_group
self.__link_name = str(end_effector_link_name)
self.__m_service = move_group.motion_service
[docs] @staticmethod
def from_end_effector_name(end_effector_name: str) -> 'xamla_motion.v2.EndEffector':
"""
Creates an instance of MoveGroup and select the correct instance of EndEffector
Parameters
----------
end_effector_name : str convertable
Name of the end_effector for which a instance of EndEffector
should be created
Returns
-------
xamla_motion.v2.EndEffector
Instance of EndEffector for end effector with the name
specified by end_effector name (instance is also hold
by the created move_group)
Raises
------
TypeError
If end_effector_name ist not convertable to str
RuntimeError
If it is not possible to create an instance of
EndEffector because the end effector specified
by end_effector_name is not available
"""
end_effector_name = str(end_effector_name)
try:
move_group = MoveGroup(None, end_effector_name)
except RuntimeError as exc:
raise RuntimeError('end effector with name: ' +
end_effector_name +
' is not available') from exc
return move_group.get_end_effector()
@property
def name(self):
"""
name : str (read only)
end effector name
"""
return self.__name
@property
def move_group(self):
"""
move_group : xamla_motion.v2.MoveGroup
Instance of MoveGroup which manages the
move group where the end effector belongs to
"""
return self.__move_group
@property
def motion_service(self):
"""
motion_service : MotionService
Instance of MotionService which the move group
and all end effectors use to communicate with
the motion server
"""
return self.__m_service
@property
def link_name(self):
"""
link_name : str
end effector link name
"""
return self.__link_name
[docs] def get_current_pose(self) -> Pose:
"""
Returns the current pose of the end effector
Returns
-------
pose : Pose
An instance of Pose which represents the current
end effector pose
Raises
------
ServiceException
If query services from motion server are not
available or finish unsuccessfully
"""
positions = self.__move_group.get_current_joint_positions()
p = self.__m_service.query_pose(self.__move_group.name,
positions,
self.__link_name)
return p
def _build_task_space_plan_parameters(self,
velocity_scaling: Union[None, float] = None,
collision_check: Union[None, bool] = None,
max_deviation: Union[None, float] = None,
acceleration_scaling: Union[None, float] = None,
sample_resolution: Union[None, float] = None,
ik_jump_threshold: Union[None,
float] = None
) -> TaskSpacePlanParameters:
"""
Build an instance of TaskSpacePlanParameters from input
and default values
All attributes with value None are ignored and instead the default
values are used defined in default_task_space_plan_parameters
are used
Parameters
----------
velocity_scaling : Union[None, float]
Scaling factor which is applied on the maximal
possible joint velocities
collision_check : Union[None, bool]
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
max_deviation : Union[None, float]
Defines the maximal deviation from trajectory points
when it is a fly-by-point in joint space
acceleration_scaling : Union[None, float]
Scaling factor which is applied on the maximal
possible joint accelerations
sample_resolution : Union[None, float]
target sample frequency in Hz
ik_jump_threshold : Union[None, float]
Maximal joint value jump between two consecutively
following trajectory points
Returns
-------
task_space_plan_parameters: TaskSpacePlanParameters
New instance of TaskSpacePlanParameters with requested changes
from the default instance
Raises
------
TypeError
If input values are not convertable to specified types
ValueError
If scaling inputs are not between 0.0 and 1.0
"""
return self.__move_group._build_task_space_plan_parameters(
velocity_scaling=velocity_scaling, collision_check=collision_check,
max_deviation=max_deviation, acceleration_scaling=acceleration_scaling,
sample_resolution=sample_resolution, ik_jump_threshold=ik_jump_threshold,
end_effector_name=self.__name)
[docs] def compute_pose(self, joint_values: JointValues) -> Pose:
"""
compute pose from joint values / configuration
Parameters
----------
joint_values : JointValues
Joint configuration of the robot which should be
transformed to a cartesian pose
Returns
-------
pose : Pose
An instance of Pose which represents the joint
configuration of joint_values as a pose in
cartesian space
Raises
------
ServiceException
If query services from motion server are not
available or finish unsuccessfully
TypeError
If joint_values is not of expected type JointValues
"""
pose = self.__m_service.query_pose(self.__move_group.name,
joint_values,
self.__link_name)
return pose
[docs] def inverse_kinematics(self, pose: Pose,
collision_check: bool,
seed: JointValues = None,
timeout: timedelta = None,
const_seed: bool = False,
attempts: int = 1) -> JointValues:
"""
inverse kinematic solutions for one pose
Parameters
----------
poses : Union[Pose, CartesianPath]
Poses to transform to joint space
collision_check : Union[None, bool]
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
seed : Union[None, JointValues] (optional)
Numerical seed to control joint configuration
timeout : Union[None, datatime.timedelta] (optional)
timeout
const_seed : bool (optional default False)
Determines if for each pose in poses the same seed should be used
attempts : int (optional default 1)
number of attempts to find solution
Returns
-------
JointValues
Instance of JointValues with a found solution
Raises
------
TypeError
If poses is not of correct type
ServiceException
If query service is not available
"""
if not isinstance(pose, Pose):
raise TypeError('target is not one of expected '
'types Pose')
if not seed:
seed = self.__move_group.get_current_joint_positions()
parameters = self.__move_group._build_plan_parameters(1.0,
collision_check)
joint_values = self.__m_service.query_inverse_kinematics(pose,
parameters,
seed,
self.__link_name,
timeout)
return joint_values
[docs] def inverse_kinematics_many(self, poses: Union[Pose, CartesianPath],
collision_check: Union[None, bool],
seed: Union[None, JointValues] = None,
timeout: Union[None, timedelta] = None,
const_seed: bool = False,
attempts: int = 1) -> IkResults:
"""
inverse kinematic solutions for many poses
Parameters
----------
poses : Union[Pose, CartesianPath]
Poses to transform to joint space
collision_check : Union[None, bool]
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
seed : Union[None, JointValues] (optional)
Numerical seed to control joint configuration
timeout : Union[None, datatime.timedelta] (optional)
timeout
const_seed : bool (optional default False)
Determines if for each pose in poses the same seed should be used
attempts : int (optional default 1)
number of attempts to find solution
Returns
-------
IkResult
Instance of IkResult with all found solutions as
a JointPath and error codes
Raises
------
TypeError
If poses is not of correct type
ServiceException
If query service is not available
"""
if isinstance(poses, Pose):
poses = [EndEffectorPose(poses, self.__link_name)]
elif isinstance(poses, CartesianPath):
poses = [EndEffectorPose(p, self.__link_name) for p in poses]
else:
raise TypeError('target is not one of expected '
'types Union[Pose, CartesianPath]')
if not seed:
seed = self.__move_group.get_current_joint_positions()
parameters = self.__move_group._build_plan_parameters(1.0,
collision_check)
ik = self.__m_service.query_inverse_kinematics_many(poses,
parameters,
seed,
timeout)
if not ik.succeeded:
print('computation of inverse kinematic fails'
' for one or more request in batch: ' + str(ik))
return ik
[docs] def move_cartesian(self, target: Union[Pose, CartesianPath],
seed: Union[None, JointValues] = None,
velocity_scaling: Union[None, float] = None,
collision_check: Union[None, bool] = None,
max_deviation: Union[None, float] = None,
acceleration_scaling: Union[None, float] = None) -> MoveCartesianOperation:
"""
Create MoveCartesianOperation for target joint positions
Parameters
----------
target : Union[Pose, CartesianPath]
Target Pose or target CartesianPath
seed : Union[None, JointValues] (default None)
Numerical seed to control joint configuration
velocity_scaling : Union[None, float] (default None)
Scaling factor which is applied on the maximal
possible joint and task velocities
If None the MoveGroup default is used
collision_check : Union[None, bool] (default None)
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
If None the MoveGroup default is used
max_deviation : Union[None, float] (default None)
Defines the maximal deviation from trajectory points
when it is a fly-by-point in joint space
If None the MoveGroup default is used
acceleration_scaling : Union[None, float] (default None)
Scaling factor which is applied on the maximal
possible joint and task accelerations
If None the MoveGroup default is used
Returns
-------
move_joint_operations : MoveCartesianOperation
Instance of MoveCartesianOperation
Raises
------
TypeError
If target is not one of types Pose, CartesianPath
If all other inputs are not convertable to specified types
"""
if not isinstance(target, (Pose, CartesianPath)):
raise TypeError('target is not one of expected types'
' Pose, CartesianPath')
if not isinstance(seed, (type(None), JointValues)):
raise TypeError('seed is not of expected type JointValues')
args = MoveCartesianArgs()
args.end_effector = self
args.move_group = self.__move_group
args.seed = seed
args.target = target
args.velocity_scaling = float(velocity_scaling or
self.__move_group.velocity_scaling)
args.acceleration_scaling = float(acceleration_scaling or
self.__move_group.acceleration_scaling)
args.collision_check = bool(collision_check or
self.__move_group.collision_check)
args.max_deviation = float(max_deviation or
self.__move_group.max_deviation)
args.sample_resolution = self.__move_group.sample_resolution
args.ik_jump_threshold = self.__move_group.ik_jump_threshold
return MoveCartesianOperation(args)
[docs] def move_cartesian_collision_free(self, target: Union[Pose, CartesianPath],
seed: Union[None, JointValues] = None,
velocity_scaling: Union[None, float] = None,
collision_check: Union[None, bool] = None,
max_deviation: Union[None, float] = None,
acceleration_scaling: Union[None,
float] = None
) -> MoveCartesianCollisionFreeOperation:
"""
Create MoveCartesianCollisionFreeOperation for target joint positions
Parameters
----------
target : Union[Pose, CartesianPath]
Target Pose or target CartesianPath
seed : Union[None, JointValues] (default None)
Numerical seed to control joint configuration
velocity_scaling : Union[None, float] (default None)
Scaling factor which is applied on the maximal
possible joint and task velocities
If None the MoveGroup default is used
collision_check : Union[None, bool] (default None)
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
If None the MoveGroup default is used
max_deviation : Union[None, float] (default None)
Defines the maximal deviation from trajectory points
when it is a fly-by-point in joint space
If None the MoveGroup default is used
acceleration_scaling : Union[None, float] (default None)
Scaling factor which is applied on the maximal
possible joint and task accelerations
If None the MoveGroup default is used
Returns
-------
move_joint_operations : MoveCartesianCollisionFreeOperation
Instance of MoveCartesianCollisionFreeOperation
Raises
------
TypeError
If target is not one of types Pose, CartesianPath
If all other inputs are not convertable to specified types
"""
if not isinstance(target, (Pose, CartesianPath)):
raise TypeError('target is not one of expected types'
' Pose, CartesianPath')
if not isinstance(seed, (type(None), JointValues)):
raise TypeError('seed is not of expected type JointValues')
args = MoveCartesianArgs()
args.end_effector = self
args.move_group = self.__move_group
args.seed = seed
args.target = target
args.velocity_scaling = float(velocity_scaling or
self.__move_group.velocity_scaling)
args.acceleration_scaling = float(acceleration_scaling or
self.__move_group.acceleration_scaling)
args.collision_check = bool(collision_check or
self.__move_group.collision_check)
args.max_deviation = float(max_deviation or
self.__move_group.max_deviation)
args.sample_resolution = self.__move_group.sample_resolution
args.ik_jump_threshold = self.__move_group.ik_jump_threshold
return MoveCartesianCollisionFreeOperation(args)
[docs] def move_cartesian_linear(self, target: Union[Pose, CartesianPath],
seed: Union[None, JointValues]=None,
velocity_scaling: Union[None, float]=None,
collision_check: Union[None, bool]=None,
max_deviation: Union[None, float]=None,
acceleration_scaling: Union[None, float]=None):
"""
Create MoveCartesianLinearOperation for target joint positions
Parameters
----------
target : Union[Pose, CartesianPath]
Target Pose or target CartesianPath
seed : Union[None, JointValues] (default None)
Numerical seed to control joint configuration
velocity_scaling : Union[None, float] (default None)
Scaling factor which is applied on the maximal
possible joint and task velocities
If None the MoveGroup default is used
collision_check : Union[None, bool] (default None)
If true the trajectory planing try to plan a
collision free trajectory and before executing
a trajectory a collision check is performed
If None the MoveGroup default is used
max_deviation : Union[None, float] (default None)
Defines the maximal deviation from trajectory points
when it is a fly-by-point in joint space
If None the MoveGroup default is used
acceleration_scaling : Union[None, float] (default None)
Scaling factor which is applied on the maximal
possible joint and task accelerations
If None the MoveGroup default is used
Returns
-------
move_joint_operations : MoveCartesianLinearOperation
Instance of MoveCartesianLinearOperation
Raises
------
TypeError
If target is not one of types Pose, CartesianPath
If all other inputs are not convertable to specified types
"""
if not isinstance(target, (Pose, CartesianPath)):
raise TypeError('target is not one of expected types'
' Pose, CartesianPath')
if not isinstance(seed, (type(None), JointValues)):
raise TypeError('seed is not of expected type JointValues')
args = MoveCartesianArgs()
args.end_effector = self
args.move_group = self.__move_group
args.seed = seed
args.target = target
args.velocity_scaling = float(velocity_scaling or
self.__move_group.velocity_scaling)
args.acceleration_scaling = float(acceleration_scaling or
self.__move_group.acceleration_scaling)
args.collision_check = bool(collision_check or
self.__move_group.collision_check)
args.max_deviation = float(max_deviation or
self.__move_group.max_deviation)
return MoveCartesianLinearOperation(args)
def __str__(self):
return 'Endeffector {}, (Link: {}, MoveGroup: {})'.format(self.__name,
self.__link_name,
self.__move_group.name)