def __init__(self, manipulator, interface=None, base_link=None, end_effector_link=None, control_type='position',
gripper=None, bounding_box=None, use_orientation=True, hand='right', position='relative',
priority=None, verbose=False):
"""
Args:
manipulator (Manipulator): manipulator robot instance.
interface (None, LeapMotionInterface): LeapMotion interface. If None, it will create one.
base_link (int, str): base link id or name.
end_effector_link (int, str): end effector link id or name.
control_type (str): type of control to use, select between {'position', 'impedance'}.
gripper (None, Gripper): gripper robot instance.
bounding_box (None, np.array[float[2,3]]): bounding box limits.
use_orientation (bool): if we should account for the orientation of the end-effector as well using the
interface.
hand (str): the hand that we will use to move the end-effector, select between {'right', 'left'}.
position (str): if the position of the hand should be absolute or relative when moving the end-effector.
If relative, by moving the hand from the center it will compute the distance from it and move the
end-effector accordingly. Note that the orientation is always absolute here.
priority (int): priority of the bridge.
verbose (bool): If True, print information on the standard output.
"""
# set manipulator
self.manipulator = manipulator
self.gripper = gripper
self.base_link = -1 if base_link is None else base_link
self.distal_link = manipulator.get_end_effector_ids(end_effector=0) if end_effector_link is None \
else end_effector_link
self.use_orientation = use_orientation
self.hand_type = Hand.RIGHT if hand == 'right' else Hand.LEFT
self.position_type = Position.RELATIVE if position == 'relative' else Position.ABSOLUTE
self.grasp_strength = 5
# if interface not defined, create one.
if interface is None:
interface = LeapMotionInterface(bounding_box=bounding_box, verbose=verbose)
if not isinstance(interface, LeapMotionInterface):
raise TypeError("Expecting the given 'interface' to be an instance of `LeapMotionInterface`, but got "
"instead: {}".format(type(interface)))
# create controller
model = RobotModelInterface(manipulator)
x_des = self.manipulator.get_link_positions(link_ids=end_effector_link, wrt_link_id=base_link)
if control_type == 'position':
task = CartesianTask(model, distal_link=end_effector_link, base_link=base_link, desired_position=x_des,
kp_position=50.)
control_type = ControlType.POSITION
elif control_type == 'impedance':
task = CartesianImpedanceControlTask(model, distal_link=end_effector_link, base_link=base_link,
desired_position=x_des, kp_position=100, kd_linear=60)
control_type = ControlType.IMPEDANCE
else:
raise NotImplementedError("Please select between {'position', 'impedance'} for the control_type.")
self._task = task
self._control_type = control_type
self._solver = QPTaskSolver(task=task)
# call superclass
super(BridgeLeapMotionManipulator, self).__init__(interface, priority, verbose=verbose)
def __init__(self, manipulator, interface, base_link=None, end_effector_link=None, control_type='position',
gripper=None, translation_scale=1., rotation_step=0.001, use_orientation=False,
priority=None, verbose=False):
"""
Initialize the bridge between the Controller and a manipulator's end-effector.
Args:
manipulator (Manipulator): manipulator robot instance.
interface (GameControllerInterface): Game controller interface.
base_link (int, str): base link id or name. If None, it will be set to -1 (the base)
end_effector_link (int, str): end effector link id or name. If None, it will take the first end-effector.
control_type (str): type of control to use, select between {'position', 'impedance'}.
gripper (None, Gripper): gripper robot instance.
translation_scale (float): translation scale coefficient. It will multiply the value by translation offset
by that value when moving the joysticks.
rotation_step (float): rotation step value. As long as we keep pushing on the corresponding buttons, it
will increment the angles by that step value.
use_orientation (bool): if we should account for the orientation of the end-effector as well using the
interface.
priority (int): priority of the bridge.
verbose (bool): If True, print information on the standard output.
"""
# set manipulator
self.manipulator = manipulator
self.gripper = gripper
self.base_link = -1 if base_link is None else base_link
self.distal_link = manipulator.get_end_effector_ids(end_effector=0) if end_effector_link is None \
else end_effector_link
self.use_orientation = use_orientation
self.grasp_strength = 0
self.translation_scale = translation_scale
self.rotation_step = rotation_step
# check the game controller interface
if not isinstance(interface, GameControllerInterface):
raise TypeError("Expecting the given 'interface' to be an instance of `GameControllerInterface`, but got "
"instead: {}".format(type(interface)))
# create controller
model = RobotModelInterface(manipulator)
x_des = self.manipulator.get_link_positions(link_ids=end_effector_link, wrt_link_id=base_link)
if control_type == 'position':
task = CartesianTask(model, distal_link=end_effector_link, base_link=base_link, desired_position=x_des,
kp_position=50.)
control_type = ControlType.POSITION
elif control_type == 'impedance':
task = CartesianImpedanceControlTask(model, distal_link=end_effector_link, base_link=base_link,
desired_position=x_des, kp_position=100, kd_linear=60)
control_type = ControlType.IMPEDANCE
else:
raise NotImplementedError("Please select between {'position', 'impedance'} for the control_type.")
self._task = task
self._control_type = control_type
self._solver = QPTaskSolver(task=task)
# call superclass
super(BridgeControllerManipulator, self).__init__(interface, priority, verbose=verbose)
def __init__(self, manipulator, interface=None, base_link=None, end_effector_link=None, control_type='position',
gripper=None, translation_range=(-1, 1), rotation_range=(-1, 1), use_orientation=False,
priority=None, verbose=False):
"""
Initialize the bridge between the SpaceMouse and a manipulator's end-effector.
Args:
manipulator (Manipulator): manipulator robot instance.
interface (None, SpaceMouseInterface): SpaceMouse interface. If None, it will create one.
base_link (int, str): base link id or name. If None, it will be set to -1 (the base)
end_effector_link (int, str): end effector link id or name. If None, it will take the first end-effector.
control_type (str): type of control to use, select between {'position', 'impedance'}.
gripper (None, Gripper): gripper robot instance.
translation_range (np.array[float[2]], np.array[float[2,3]], tuple[float[2]],
tuple[np.array[float[3]][2]]): the lower and higher bounds for the (x, y, z). This is used to normalize
the translation range to be between [-1, 1]. The frame (x,y,z) is defined with x pointing forward, y to
the left, and z up.
rotation_range (np.array[float[2]], np.array[float[2,3]], tuple[float[2]], tuple[np.array[3][2]]): the
lower and higher bounds for the roll-pitch-yaw angles. This is used to normalize the rotation range to
be between [-1, 1]. The frame (x,y,z) is defined with x pointing forward, y to the left, and z up.
use_orientation (bool): if we should account for the orientation of the end-effector as well using the
interface.
priority (int): priority of the bridge.
verbose (bool): If True, print information on the standard output.
"""
# set manipulator
self.manipulator = manipulator
self.gripper = gripper
self.base_link = -1 if base_link is None else base_link
self.distal_link = manipulator.get_end_effector_ids(end_effector=0) if end_effector_link is None \
else end_effector_link
self.use_orientation = use_orientation
self.grasp_strength = 0
# if interface not defined, create one.
if interface is None:
interface = SpaceMouseInterface(verbose=verbose, translation_range=translation_range,
rotation_range=rotation_range)
if not isinstance(interface, SpaceMouseInterface):
raise TypeError("Expecting the given 'interface' to be an instance of `SpaceMouseInterface`, but got "
"instead: {}".format(type(interface)))
# create controller
model = RobotModelInterface(manipulator)
x_des = self.manipulator.get_link_positions(link_ids=end_effector_link, wrt_link_id=base_link)
if control_type == 'position':
task = CartesianTask(model, distal_link=end_effector_link, base_link=base_link, desired_position=x_des,
kp_position=50.)
control_type = ControlType.POSITION
elif control_type == 'impedance':
task = CartesianImpedanceControlTask(model, distal_link=end_effector_link, base_link=base_link,
desired_position=x_des, kp_position=100, kd_linear=60)
control_type = ControlType.IMPEDANCE
else:
raise NotImplementedError("Please select between {'position', 'impedance'} for the control_type.")
self._task = task
self._control_type = control_type
self._solver = QPTaskSolver(task=task)
# call superclass
super(BridgeSpaceMouseManipulator, self).__init__(interface, priority, verbose=verbose)
def __init__(self,
manipulator,
interface=None,
base_link=None,
end_effector_link=None,
control_type='position',
gripper=None,
translation_step=0.001,
rotation_step=0.001,
use_orientation=False,
priority=None,
verbose=False):
"""
Initialize the bridge between the mouse+keyboard interface and a manipulator's end-effector.
Args:
manipulator (Manipulator): manipulator robot instance.
interface (None, SpaceMouseInterface): SpaceMouse interface. If None, it will create one.
base_link (int, str): base link id or name. If None, it will be set to -1 (the base)
end_effector_link (int, str): end effector link id or name. If None, it will take the first end-effector.
control_type (str): type of control to use, select between {'position', 'impedance'}.
gripper (None, Gripper): gripper robot instance.
translation_step (float): translation step value. As long as we keep pushing on the corresponding buttons,
it will increment the positions by that step value.
rotation_step (float): rotation step value. As long as we keep pushing on the corresponding buttons, it
will increment the angles by that step value.
use_orientation (bool): if we should account for the orientation of the end-effector as well using the
interface.
priority (int): priority of the bridge.
verbose (bool): If True, print information on the standard output.
"""
# set manipulator
self.manipulator = manipulator
self.gripper = gripper
self.base_link = -1 if base_link is None else base_link
self.distal_link = manipulator.get_end_effector_ids(end_effector=0) if end_effector_link is None \
else end_effector_link
self.use_orientation = use_orientation
self.grasp_strength = 0
self.translation_step = translation_step
self.rotation_step = rotation_step
# if interface not defined, create one.
if not isinstance(interface, MouseKeyboardInterface):
interface = MouseKeyboardInterface(self.simulator)
# create controller
model = RobotModelInterface(manipulator)
x_des = self.manipulator.get_link_positions(link_ids=end_effector_link,
wrt_link_id=base_link)
if control_type == 'position':
task = CartesianTask(model,
distal_link=end_effector_link,
base_link=base_link,
desired_position=x_des,
kp_position=50.)
control_type = ControlType.POSITION
elif control_type == 'impedance':
task = CartesianImpedanceControlTask(model,
distal_link=end_effector_link,
base_link=base_link,
desired_position=x_des,
kp_position=100,
kd_linear=60)
control_type = ControlType.IMPEDANCE
else:
raise NotImplementedError(
"Please select between {'position', 'impedance'} for the control_type."
)
self._task = task
self._control_type = control_type
self._solver = QPTaskSolver(task=task)
# call superclass
super(BridgeMouseKeyboardManipulator, self).__init__(interface,
priority,
verbose=verbose)