Source code for xamla_motion.data_types.task_space_plan_parameters

# task_space_plan_parameters.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 copy import deepcopy

from .end_effector_limits import EndEffectorLimits

import numpy as np

from ..xamla_motion_exceptions import ArgumentError


[docs]class TaskSpacePlanParameters(object): """ TaskSpace holds all constrains for task space trajectory planning Methods ------- from_arguments(end_effector_name, max_xyz_velocity, max_xyz_acceleration, max_angular_velocity, max_angular_acceleration, **kwargs) Creates a instance or TaskSpacePlanParameters from single limit arguments """ def __init__(self, end_effector_name, end_effector_limits, **kwargs): """ Initialization of TaskSpacePlanParameters class Parameters ---------- end_effector_name : str convertable Name of the end effector end_effector_limits : EndEffectorLimits Limits of the end effector in task space kwargs : dict The argv dict is used to set parameters which have default values this are sample_resolution (default = 0.008 / 125 Hz), collision_check (default = True), max_deviation (default = 0.2), ik_jump_threshold (default = 1.2) Returns ------ Instance of TaskSpacePlanParameters Raises ------ TypeError : type mismatch If end_effector_limits is not of expected type EndEffectorLimits Examples -------- >>> end_effector_limits = EndeffectorLimits(1.0, 1.0, 1.0, 1.0) >>> p = TaskSpacePlanParameters('tool1', end_effector_limits, max_deviation=0.1) """ # if you change defaults here please also edit documentation in # motion_service.py create_task_space_plan_parameters() try: self.__sample_resolution = float(kwargs.get("sample_resolution", 0.08)) except TypeError as exc: raise TypeError('sample_resolution can not be converted to float') try: self.__collision_check = bool(kwargs.get("collision_check", True)) except TypeError as exc: raise TypeError('collision_check can not be converted to bool') try: self.__ik_jump_threshold = float(kwargs.get("ik_jump_threshold", 1.2)) except TypeError as exc: raise TypeError('ik_jump_threshold can not be converted to float') try: self.__max_deviation = float(kwargs.get("max_deviation", 0.2)) except TypeError as exc: raise TypeError('max_deviation can not be converted to float') if isinstance(end_effector_limits, EndEffectorLimits): self.__end_effector_limits = end_effector_limits else: raise TypeError('end_effector_limits is not of' ' expected type EndEffectorLimits') self.__end_effector_name = str(end_effector_name)
[docs] @classmethod def from_arguments(cls, end_effector_name, max_xyz_velocity, max_xyz_acceleration, max_angular_velocity, max_angular_acceleration, **kwargs): """ Initialization of TaskSpacePlanParameters by single limits Parameters ---------- end_effector_name : str Name of the end effector max_xyz_velocity : float convertable Defines the maximal xyz velocity [m/s] max_xyz_acceleration : float convertable (read only) Defines the maximal xyz acceleration [m/s^2] max_angular_velocity : float convertable (read only) Defines the maximal angular velocity [rad/s] max_angular_acceleration : float convertable (read only) Defines the maximal angular acceleration [rad/s^2] kwargs : dict The argv dict is used to set parameters which have default values this are sample_resolution (default = 0.008), collision_check (default = True), max_deviation (default = 0.2), ik_jump_threshold (default = 1.2) Returns ------- TaskSpacePlanParameters Instance of TaskSpacePlanParameters Raises ------ ArgumentError If instance of EndeffectorLimits could not be created due to wrong type or format of the arguments Examples -------- >>> p = TaskSpacePlanParameters('tool1', 1.0, 1.0, 1.0, 1.0, max_deviation=0.1) """ try: end_effector_limits = EndEffectorLimits(max_xyz_velocity, max_xyz_acceleration, max_angular_velocity, max_angular_acceleration) except (ValueError, TypeError) as exc: raise ArgumentError('It was not possible to create' ' an instance of Endeffectorlimits' ' due to limit type or size') from exc return cls(end_effector_name, end_effector_limits, **kwargs)
[docs] @classmethod def from_builder(cls, builder): """ Initilization of TaskSpacePlanParameters from TaskSpacePlanParameters Builder Parameters ---------- builder Instance of TaskSpacePlanParameter Builder Returns ------ Instance of TaskSpacePlanParameters Raises ------ TypeError If builder is not of expected type TaskSpacePlanParameters Builder """ if not isinstance(builder, cls._Builder): raise TypeError('builder is not of expected type ' 'TaskSpacePlanParameters builder') return cls(builder.end_effector_name, builder.e_e_limits, sample_resolution=builder.sample_resolution, collision_check=builder.collision_check, max_deviation=builder.max_deviation, ik_jump_threshold=builder.ik_jump_threshold)
class _Builder(object): def __init__(self, instance): self.__end_effector_name = instance.end_effector_name self.__e_e_limits = instance.end_effector_limits self.__sample_resolution = instance.sample_resolution self.__max_deviation = instance.max_deviation self.__collision_check = instance.collision_check self.__ik_jump_threshold = instance.ik_jump_threshold @property def end_effector_name(self): return self.__end_effector_name @property def end_effector_limits(self): return self.__e_e_limits @property def sample_resolution(self): return self.__sample_resolution @sample_resolution.setter def sample_resolution(self, value): value = float(value) self.__sample_resolution = value @property def collision_check(self): return self.__collision_check @collision_check.setter def collision_check(self, value): value = bool(value) self.__collision_check = value @property def max_deviation(self): return self.__max_deviation @max_deviation.setter def max_deviation(self, value): value = float(value) self.__max_deviation = value @property def ik_jump_threshold(self): return self.__ik_jump_threshold @ik_jump_threshold.setter def ik_jump_threshold(self, value): value = float(value) self.__ik_jump_threshold = value def scale_velocity(self, value): value = float(value) if value > 1.0 or value < 0.0: raise ValueError('velocity_scaling is not' ' between 0.0 and 1.0') max_xyz_velocity = self.__e_e_limits.max_xyz_velocity max_xyz_velocity *= value max_angular_velocity = self.__e_e_limits.max_angular_velocity max_angular_velocity *= value limits = EndEffectorLimits(max_xyz_velocity, self.__e_e_limits.max_xyz_acceleration, max_angular_velocity, self.__e_e_limits.max_angular_acceleration) self.__e_e_limits = limits def scale_acceleration(self, value): value = float(value) if value > 1.0 or value < 0.0: raise ValueError('acceleration_scaling is not' ' between 0.0 and 1.0') max_xyz_acc = self.__e_e_limits.max_xyz_acceleration max_xyz_acc *= value max_angular_acc = self.__e_e_limits.max_angular_velocity max_angular_acc *= value limits = EndEffectorLimits(self.__e_e_limits.max_xyz_velocity, max_xyz_acc, self.__e_e_limits.max_angular_velocity, max_angular_acc) self.__e_e_limits = limits def build(self): return TaskSpacePlanParameters(self.__end_effector_name, self.__e_e_limits, sample_resolution=self.__sample_resolution, collision_check=self.__collision_check, max_deviation=self.__max_deviation, ik_jump_threshold=self.__ik_jump_threshold) @property def end_effector_name(self): """ end_effector_name : str (read only) Name of the move group for which plan parameters are applied """ return self.__end_effector_name @property def end_effector_limits(self): """ end_effector_limits: EndeffectorLimits(read only) define the task space constraints """ return self.__end_effector_limits @property def max_xyz_velocity(self): """ max_xyz_velocity: float(read only) max translational velocity """ return self.__end_effector_limits.max_xyz_velocity @property def max_xyz_acceleration(self): """ max_xyz_acceleration: float(read only) max translational acceleration """ return self.__end_effector_limits.max_xyz_acceleration @property def max_angular_velocity(self): """ max_angular_velocity: float(read only) max angular velocity """ return self.__end_effector_limits.max_angular_velocity @property def max_angular_acceleration(self): """ max_angular_acceleration: float(read only) max angular acceleration """ return self.__end_effector_limits.max_angular_acceleration @property def sample_resolution(self): """ sample_resolution: float sampling resolution in Hz """ return self.__sample_resolution @sample_resolution.setter def sample_resolution(self, value): value = float(value) self.__sample_resolution = value @property def collision_check(self): """ collision_check: bool defines if collision check should be performed """ return self.__collision_check @property def max_deviation(self): """ max_deviation: float defines the maximal deviation from trajectory points when fly-by-points in joint space """ return self.__max_deviation @property def ik_jump_threshold(self): """ ik_jump_threshold: float defines the inverse kinematic jump threshold """ return self.__ik_jump_threshold
[docs] def with_collision_check(self, value): """ Create an instance of TaskSpacePlanParameters with collision check value Parameters ---------- value : bool New value of collision check Returns ------- TaskSpacePlanParameters Instance of TaskSpacePlanParameters with new collision check value Raises ------ TypeError If value is not of type bool """ if not isinstance(value, bool): raise TypeError('value is not of expected type bool') if self.__collision_check == value: return self else: builder = self._Builder(self) builder.collision_check = value return builder.build()
[docs] def with_sample_resolution(self, value): """ Create an instance of TaskSpacePlanParameters with sample resolution value Parameters ---------- value : bool New value of sample resolution Returns ------- TaskSpacePlanParameters Instance of TaskSpacePlanParameters with new sample resolution value Raises ------ TypeError If value is not convertable to float """ value = float(value) if np.isclose(self.__sample_resolution, value): return self else: builder = self._Builder(self) builder.sample_resolution = value return builder.build()
[docs] def with_max_deviation(self, value): """ Create an instance of TaskSpacePlanParameters with max deviation value Parameters ---------- value : bool New value of max deviation Returns ------- TaskSpacePlanParameters Instance of TaskSpacePlanParameters with new max deviation value Raises ------ TypeError If value is not convertable to float """ value = float(value) if np.isclose(self.__max_deviation, value): return self else: builder = self._Builder(self) builder.max_deviation = value return builder.build()
[docs] def with_ik_jump_threshold(self, value): """ Create an instance of TaskSpacePlanParameters with ik jump threshold value Parameters ---------- value : bool New value of ik jump threshold Returns ------- TaskSpacePlanParameters Instance of TaskSpacePlanParameters with new ik jump threshold value Raises ------ TypeError If value is not convertable to float """ value = float(value) if np.isclose(self.__max_deviation, value): return self else: builder = self._Builder(self) builder.max_deviation = value return builder.build()
[docs] def to_builder(self): """ Create an instance of TaskSpacePlanParameters Builder Returns ------- TaskSpacePlanParameters Builder Instance of TaskSpacePlanParameters Builder """ return self._Builder(self)
def __str__(self): j_str = '\n'.join(['sampling_resolution = '+str(self.__sample_resolution), 'collision_check = '+str(self.__collision_check), 'max_devitation = '+str(self.__max_deviation), 'ik_jump_threshold ='+str(self.__ik_jump_threshold)]) end_effector_limits_str = '\n' + self.__end_effector_limits.__str__() end_effector_limits_str = end_effector_limits_str.replace('\n', '\n ') return 'TaskSpacePlanParameters:\n' + j_str + end_effector_limits_str def __repr__(self): return self.__str__() def __eq__(self, other): r_tol = 1.0e-13 a_tol = 1.0e-14 if not isinstance(other, self.__class__): return False if id(other) == id(self): return True if other.end_effector_limits != self.__end_effector_limits: return False if not np.isclose(self.__sample_resolution, other.sampling_resolution, rtol=r_tol, atol=a_tol): return False if not np.isclose(self.__max_deviation, other.max_deviation, rtol=r_tol, atol=a_tol): return False if not np.isclose(self.__ik_jump_threshold, other.ik_jump_threshold, rtol=r_tol, atol=a_tol): return False if self.__collision_check != other.collision_check: return False return True def __ne__(self, other): return not self.__eq__(other)