Motion Service¶
The class xamla_motion.motion_service.MotionService
implements the
low level service and action calls to communicate with the
xamlamoveit server part.
Warning
The class xamla_motion.motion_service.MotionService
is only documented
for completeness. The functionalities of this class are wrapped
by the specific clients to be more user-friendly. Please, only
use these specific client implementation and not directly the
motion services.
-
class
xamla_motion.motion_service.
MotionService
[source]¶ -
classmethod
create_plan_parameters
(move_group_name=None, joint_set=None, max_velocity=None, max_acceleration=None, **kwargs)[source]¶ Create PlanParameters from user defined and/or quried inputs
- Parameters
move_group_name (str (optional default query first)) – move group for which plan parameters should be created
joint_set (JointSet (optional default query similar)) – joint set for which plan parameters should be created
max_velocity (Iterable[float convertable] (optional default max query)) – Defines the maximal velocity for every joint
max_acceleration (Iterable[float convertable] (op default max query)) – Defines the maximal acceleration for every joint
kwargs (dict) –
- sample_resolutionfloat convertable (optional default 0.008/125Hz)
sample points frequency if value is create as 1.0 the value is interpreted as a value in seconds else the value is interpreted as a value in Hz
- collision_checkbool convertable (optional default True)
check for collision if True
- max_deviationfloat convertable (optional default 0.2)
max deviation from fly by points
- velocity_scalingfloat convertable (optional default 1.0)
scale query or user defined max velocity values between 0.0 and 1.0
- acceleration_scalingfloat convertable (optional default 1.0)
scale query or user defined max acceleration values between 0.0 and 1.0
- Returns
Instance of plan parameters with automatically queried and/or user defined values
- Return type
- Raises
ServiceException – If query services are not reachable or not finish successful
TypeError – If move group name is not of type None or str convertable If joint_set is not of type None or JointSet If an other parameter is not convertable
ValueError – If scaling parameters are not between 0.0 and 1.0
ArgumentError – If a argument is not set and also could not be quired automatically
-
classmethod
create_task_space_plan_parameters
(end_effector_name=None, max_xyz_velocity=None, max_xyz_acceleration=None, max_angular_velocity=None, max_angular_acceleration=None, **kwargs)[source]¶ Creates TakesSpacePlanParameters from user defined or queried inputs
- Parameters
move_group_name (str (optional default query first)) – move group for which plan parameters should be created
max_xyz_velocity (float convertable or None) – Defines the maximal xyz velocity [m/s]
max_xyz_acceleration (float convertable) – Defines the maximal xyz acceleration [m/s^2]
max_angular_velocity (float convertable or None) – Defines the maximal angular velocity [rad/s]
max_angular_acceleration (float convertable or None) – Defines the maximal angular acceleration [rad/s^2]
kwargs (dict) –
- sample_resolutionfloat convertable (optional default 0.008/125Hz)
sample points frequency if value is create as 1.0 the value is interpreted as a value in seconds else the value is interpreted as a value in Hz
- collision_checkbool convertable (optional default True)
check for collision if True
- ik_jump_thresholdfloat convertable (optional default 1.2)
maximal inverse kinematic jump
- max_deviationfloat convertable (optional default 0.2)
max deviation from fly by points
- velocity_scalingfloat convertable (optional default 1.0)
scale query or user defined max velocity values between 0.0 and 1.0
- acceleration_scalingfloat convertable (optional default 1.0)
scale query or user defined max acceleration values between 0.0 and 1.0
- Returns
Instance of TaskSpacePlanParameters with automatically queried and/or user defined values
- Return type
- Raises
ServiceException – If query services are not reachable or not finish successful
TypeError – If end_effector_name is not of type None or str convertable If an other parameter is not convertable
ValueError – If scaling parameters are not between 0.0 and 1.0
ArgumentError – If a argument is not set and also could not be quired automatically
-
classmethod
emergency_stop
(enable=True)[source]¶ Sets and resets emergency stop
- Parameters
enable (bool convertable (default True)) – If True activate emergency stop If False resets the emergency stop
- Returns
success (bool) – True if the service call was successful
message (str) – Response or error message
- Raises
TypeError – If enable is not convertable to bool
ServiceException – If query service is not available
-
execute_joint_trajectory
(trajectory, collision_check)[source]¶ Executes a joint trajectory
- Parameters
trajectory (JointTrajectory) – Joint trajectory which should be executed
collision_check (bool convertable) – If True check for collision while executing
- Raises
TypeError – If trajectory is not of type JointTrajectory or if collision_check is not convertable to bool
ServiceException – If execution ends not successful
-
execute_joint_trajectory_supervised
(trajectory, velocity_scaling, collision_check)[source]¶ Creates a instance of SteppedMotionClient for supervised trajectory execution :type trajectory:
JointTrajectory
:param trajectory: Joint trajectory which should be executed :type trajectory: JointTrajectory :type velocity_scaling:float
:param velocity_scaling: scaling of velocity range 0.0 to 1.0 :type velocity_scaling: float :type collision_check:bool
:param collision_check: If True check for collision while executing :type collision_check: bool convertable
-
classmethod
get_current_joint_values
(joint_set)[source]¶ Query the current joint positions of all joints in joint_set
-
move_gripper
(action_name, position, max_effort)[source]¶ Moves a gripper via the general ros interface GripperCommandAction
- Parameters
action_name (str convertable) – Name of the action to control a specific gripper
position (float convertable) – Requested position of the gripper in meter
max_effort (float convertable) – Force which should be applied
Results –
------- –
action_response (MoveGripperResult) – Result of the action execution
- Raises
TypeError – If inputs are not convertable to specified types
ServiceException – If action server is not available
RuntimeError – If action returns unexpected result
-
plan_cartesian_path
(path, parameters)[source]¶ Plan a joint trajectory from a cartesian path and plan parameters
- Parameters
path (CartesianPath) – Poses the planned trajectory must reach
parameters (PlanParameters) – Plan parameters which defines the limits, settings and move group name
- Returns
A joint trajectory which reach defined poses of path under the constrains in parameters
- Return type
- Raises
TypeError – If path is not of type CartesianPath or parameters is not of type PlanParameters
ValueError – If parameters joint set is not equal or sub set of the seed joint set and therefore reordering was not possible
ServiceException – If query services are not available or finish unsuccessfully or inverse kinematics ends with error
-
classmethod
plan_collision_free_joint_path
(path, parameters)[source]¶ Plans a collision free joint path by query it
- Parameters
path (JointPath) – joint path which should be replanned to be collision free
parameters (PlanParameters) – plan parameters which defines the limits and move group
- Returns
the replanned collision free joint path
- Return type
- Raises
TypeError – If path is not of type JointPath or if parameters is not of type PlanParameters
ValueError – If parameters joint set is not equal or sub set of the path joint set and therefore reordering was not possible
ServiceException – If query service is not available or finish unsuccessfully
-
classmethod
plan_move_joints
(path, parameters)[source]¶ Plans trajectory from a joint path
- Parameters
path (JointPath) – joint path with positions the trajectory must reach
parameters (PlanParameters) – plan parameters which defines the limits, settings and move group name
- Returns
Planned joint trajectory which reach the positions defined in path under the constraints of parameters
- Return type
- Raises
TypeError – If path is not of type JointPath or if parameters is not of type PlanParameters or
ValueError – If parameters joint set is not equal or sub set of the path joint set and therefore reordering was not possible
ServiceException – If query service is not available or finish unsuccessfully
-
classmethod
plan_move_pose_linear
(path, seed, parameters)[source]¶ Plans trajectory with linear movements from a cartesian path
- Parameters
path (CartesianPath) – cartesian path with poses the trajectory must reach
seed (JointValues) – numerical seed to control configuration
parameters (TaskSpacePlanParameters) – plan parameters which defines the limits, settings and end effector name
- Returns
Planned joint trajectory which reach the poses defined in path under the constraints of parameters
- Return type
- Raises
TypeError – If path is not of type CartesianPath or if parameters is not of type TaskSpacePlanParameters or if seed is not of type JointValues
ServiceException – If query service is not available or finish unsuccessfully
-
classmethod
query_available_end_effectors
()[source]¶ Query all currently available end effectors
To query the available end effectors the ros service with the string defined in _query_move_group_service is called an only the relevant information about the endeffector is filtered out
- Returns
end_effectors – Returns a dictionary key is the endeffector name, value is an instance of EndEffectorDescription
- Return type
Dict[str, EndEffectorDescription]
- Raises
xamla_motion.ServiceExeption – If Service not exist or is not callable
TypeError – If the service response joint_names are not of an iterable type
-
classmethod
query_available_move_groups
()[source]¶ Query all currently available move groups
To query the move groups the ros service with the string defined in query_move_group_service is called
- Returns
groups – Returns a list with instances of MoveGroupDescription. For further details please take a look into the documentation of MoveGroupDescription
- Return type
List[MoveGroupDescription]
- Raises
xamla_motion.ServiceNotAvailableException – If Service not exist or is not callable
TypeError – If the service response joint_names are not of an iterable type
-
classmethod
query_collision_free_joint_path
(move_group_name, joint_path)[source]¶ Query a collision free joint path from a user defined joint path
- Parameters
move_group_name (str convertable) – name of the move group for which the collision free joint is required
joint_path (JointPath) – joint path which may contains collisions
Results –
------- –
JointPath – New planned JointPath without collisions
:raises TypeError : type mismatch: If joint_path is not of expectef type JointPath or it move_group_name is not convertable to str :raises ServiceException: If query joint path service is not callable or is not successful
-
classmethod
query_endeffector_limits
(name)[source]¶ Query end effector limits from ros param
To query the end effector limits the ros param definied in end_effector_limits_param is read out
- Parameters
name (str convertable) – Name of the end effector for which the limits are queried
- Returns
Returns a instance of EndEffectorLimits
- Return type
- Raises
RuntimeError – If the the ros parameter or the requested end effector name no exists
KeyError – If not all necessary limits exists
-
query_inverse_kinematics
(pose, parameters, seed=[], end_effector_link='', timeout=None, attempts=1, const_seed=False)[source]¶ Query inverse kinematic solutions one pose
- Parameters
pose (pose) – Pose to transform to joint space
parameters (PlanParameters) – Plan parameters which defines the limits, settings and move group name
seed (JointValues (optional)) – Numerical seed to control joint configuration
end_effector_link (str convertable (optinal)) – necessary if poses are defined for end effector link
timeout (datatime.timedelta (optional)) – timeout
attempts (int convertable (optional default 1)) – Attempts to find a solution or each pose
const_seed (bool convertable (optional default False)) – use constant seed instead of consecutive seeds
- Returns
Instance of JointValues which is the joint space equivalent of the task space pose
- Return type
- Raises
TypeError – If pose is not of type Pose or parameters is not of type PlanParameters or seed is not empty list or type JointValues or the other parameters are not convertable to defined types
ValueError – If parameters joint set is not equal or sub set of the seed joint set if defined and therefore reordering was not possible
ServiceException – If query service is not available
-
query_inverse_kinematics_many
(poses, parameters, seed=[], timeout=None, attempts=1, const_seed=False)[source]¶ Query inverse kinematic solutions for a Iterable of Poses
- Parameters
poses (Iterable[EndEffectorPose]) – Iterable with EndEffectorPoses to transform to joint space
parameters (PlanParameters) – Plan parameters which defines the limits, settings and move group name
seed (JointValues (optional)) – Numerical seed to control joint configuration
timeout (datatime.timedelta) – timeout
attempts (int convertable (optional default 1)) – attempts to find a solution for each pose
const_seed (bool convertable (optional default False)) – use constant seed instead of consecutive seeds
- Returns
Instance of IkResult with all found solutions as a JointPath and error codes
- Return type
IkResult
- Raises
TypeError – If poses is not of type EndEffectorPose or parameters is not of type PlanParameters or seed is not empty list or type JointValues or the other parameters are not convertable to defined types
ValueError – If parameters joint set is not equal or sub set of the seed joint set if defined and therefore reordering was not possible
ServiceException – If query service is not available
-
classmethod
query_joint_limits
(joint_set)[source]¶ Query end joint limits from ros param
To query the joint limits the ros param definied in joint_limits_param + joint name + limit name is read out
- Parameters
join_set (JointSet) – Set of joints for which the limits are queried
- Returns
Returns a instance of JointLimits invalid limits are of type numpy.nan
- Return type
:raises TypeError : type mismatch: If joint_set is not of expected type JointSet :raises KeyError: If ros params not exists
-
classmethod
query_joint_path_collisions
(move_group_name, joint_path)[source]¶ Query collisions in joint path
- Parameters
move_group_name (str convertable) – Name of the move group for which a path should be check for collisions
joint_path (JointPath) – The path that should be check for collisions
- Returns
If collisions exists returns a list of JointValuesCollisions else returns None
- Return type
List[JointValuesCollision] or None
- Raises
TypeError – If move_group_name ist not str convertable or if joint_path is not of type JointPath
-
classmethod
query_joint_states
(joint_set)[source]¶ Query joint states by calling the providing ros service
To query the joint states the ros service with the name defined in quary_joint_states_service is called
- Parameters
joint_set (JointSet) – Set of joints for which the joint states are queried
- Returns
Returns a instance of JointStates which contains the joint states of all joints defined in joint_set
- Return type
:raises TypeError : type mismatch: If joint_set is not of expected type JointSet :raises xamla_motion.ServiceException: If ros service not exists or is not callable
-
classmethod
query_joint_trajectory
(joint_path, max_velocity, max_acceleration, max_deviation, delta_t)[source]¶ Query a joint trajectory from joint path / joint positions
- Parameters
joint_path (JointPath) – Defines the key joint positions the trajectory must reach
max_velocity (Iterable[float convertable]) – Defines the maximal velocity for every joint
max_acceleration (Iterable[float convertable]) – Defines the maximal acceleration for every joint
max_deviation (float convertable) – Defines the maximal deviation of the joints to the defined key points while executing the trajectory
delta_t (float convertable) – Sampling points frequency or time if value is create as 1.0 the value is interpreted as a value in seconds else the value is interpreted as a value in Hz
- Returns
An instance of JointTrajectory with dense JointTrajectoryPoints
- Return type
- Raises
TypeError – If joint_path is not of expected type JointPath or max_velocity or max_acceleration is not iterable or values of max_velocity, max_acceleration or delta_t are not convertable to float
ValueError – If max_velocity or max_acceleration have not the same number of values has joints are defined in joint_path
-
classmethod
query_pose
(move_group_name, joint_positions, end_effector_link='')[source]¶ Computes the pose by applying forward kinematics
- Parameters
move_group_name (str convertable) – name of the move group from which the pose is queried
joint_positions (JointValues) – joint values from which the pose is calculated
end_effector_link (str convertable (optional)) – end effector link is necessary if end effector is not part of the move group but pose should be computed for the end effector
- Returns
Pose of the kinematic chain defined by the input parameter
- Return type
- Raises
TypeError – If joint_positions is not of type JointValues
-
classmethod
query_pose_many
(move_group_name, joint_path, end_effector_link='')[source]¶ Query the poses from joint path points by applying forward kinematics
- Parameters
move_group_name (str convertable) – name of the move group from which the poses are queried
joint_path (JointPath) – joint path from which the poses are calculated
end_effector_link (str convertable (optional)) – end effector link is necessary if end effector is not part of the move group but pose should be computed for the end effector
- Returns
List of Poses
- Return type
List[Pose]
- Raises
TypeError – If joint_path is not of type JointPath
-
classmethod
query_task_space_trajectory
(end_effector_name, cartesian_path, seed, max_xyz_velocity, max_xyz_acceleration, max_angular_velocity, max_angular_acceleration, ik_jump_threshold, max_deviation, collision_check, delta_t)[source]¶ Query a joint trajectory from task space poses
- Parameters
end_effector_name (str convertable) – Name of the end effector for a trajectory should be quried
cartesian_path (CartesianPath) – Define the key poses the trajectory must reach
seed (JointValues) – numerical seed to control configuration of the robot
max_xyz_velocity (float convertable) – max velocity for translation in m/s
max_xyz_acceleration (float convertable) – max acceleration for translation in m/s^2
max_angular_velocity (float convertable) – max angular velocity in rad/s
max_angular_acceleration (float convertable) – max angular acceleration in rad/s^2
ik_jump_threshold (float convertable) – maximal inverse kinematic jump
max_deviation (float convertable) – Defines the maximal deviation of the joints to the defined key points while executing the trajectory
collision_check (bool convertable) – If true check the trajectory is collision free
delta_t (float convertable) – Sampling points frequency or time if value is create as 1.0 the value is interpreted as a value in seconds else the value is interpreted as a value in Hz
- Returns
An instance of JointTrajectory with dense JointTrajectoryPoints
- Return type
- Raises
TypeError – If cartesian_path is not of expected type CartesianPath or if seed is not of expected type JointValues max_* is not iterable or the values are not convertable as expected
ValueError – If max_velocity or max_acceleration have not the same number of values has joints are defined in joint_path
-
wsg_gripper_command
(action_name, command, width, speed, max_effort, stop_on_block=True)[source]¶ Controls a WeissWsg gripper via the the specific action WsgCommandAction
- Parameters
action_name (str convertable) – Name of the action to control a specific gripper
command (WsgCommand) – Specifies which kind of action should be performed
width (float convertable) – Requested position of the gripper in meter
speed (float convetable) – Requested speed in m/s
max_effort (float convertable) – Force which should be applied
Results –
------- –
action_response (MoveGripperResult) – Result of the action execution
- Raises
TypeError – If inputs are not convertable to specified types or if command is no of type WsgCommand
ServiceException – If action server is not available
RuntimeError – If action returns unexpected result
-
classmethod