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,
model,
distal_link,
base_link=None,
desired_wrench=0.,
kp=1.,
weight=1.,
constraints=[]):
"""
Initialize the task.
Args:
model (ModelInterface): model interface.
distal_link (int, str): distal link id or name.
base_link (int, str, None): base link id or name. If None, it will be the base root link.
desired_wrench (float, np.array[float[6]]): desired wrench (force and torque) in the base link of reference
frame.
kp (float, np.array[float[6,6]]): proportional gain = compliance matrix.
weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task.
constraints (list[Constraint]): list of constraints associated with the task.
"""
super(InteractionTask, self).__init__(model=model,
weight=weight,
constraints=constraints)
# set variables
self.distal_link = self.model.get_link_id(distal_link)
self.base_link = self.model.get_link_id(
base_link) if base_link is not None else base_link
self.desired_wrench = desired_wrench
self.wrench = None # measured wrench
self.kp = kp
# create sub-task
self._task = CartesianTask(model,
distal_link=distal_link,
base_link=base_link,
weight=weight)
class InteractionTask(JointVelocityTask):
r"""Interaction Task
From the documentation of the framework of [1]: "the `InteractionTask` class implements an admittance based force
control using the admittance law:
.. math::
dx = K_p (w_d - w) \\
x_d = x + dx
where :math:`w_d \in \mathbb{R}^6` is the desired wrench in some base_link frame, :math:`w` is the measured wrench
transformed from the Force/Torque sensor frame to the base_link frame. The displacement :math:`dx` is integrated
using the previous position :math:`x`, and a new desired position :math:`x_d` is computed. The references
:math:`x_d` and :math:`dx` are then used inside a Cartesian task (see ``CartesianTask``).
Warnings: the :math:`w_d` is the desired wrench that the robot has to exert on the environment, so the measured
wrench :math:`w` is the wrench produced by the robot on the environment (and not the opposite)!"
The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
References:
- [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
"""
def __init__(self,
model,
distal_link,
base_link=None,
desired_wrench=0.,
kp=1.,
weight=1.,
constraints=[]):
"""
Initialize the task.
Args:
model (ModelInterface): model interface.
distal_link (int, str): distal link id or name.
base_link (int, str, None): base link id or name. If None, it will be the base root link.
desired_wrench (float, np.array[float[6]]): desired wrench (force and torque) in the base link of reference
frame.
kp (float, np.array[float[6,6]]): proportional gain = compliance matrix.
weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task.
constraints (list[Constraint]): list of constraints associated with the task.
"""
super(InteractionTask, self).__init__(model=model,
weight=weight,
constraints=constraints)
# set variables
self.distal_link = self.model.get_link_id(distal_link)
self.base_link = self.model.get_link_id(
base_link) if base_link is not None else base_link
self.desired_wrench = desired_wrench
self.wrench = None # measured wrench
self.kp = kp
# create sub-task
self._task = CartesianTask(model,
distal_link=distal_link,
base_link=base_link,
weight=weight)
##############
# Properties #
##############
@property
def desired_wrench(self):
"""Get the desired wrench."""
return self._desired_wrench
@desired_wrench.setter
def desired_wrench(self, wrench):
"""Set the desired wrench."""
if wrench is None:
wrench = np.zeros(6)
elif isinstance(wrench, (float, int)):
wrench = wrench * np.ones(6)
if not isinstance(wrench, (list, tuple, np.ndarray)):
raise TypeError(
"Expecting the given 'desired_wrench' to be a tuple/list of np.array, or a np.array, "
"but got instead: {}".format(type(wrench)))
self._desired_wrench = np.asarray(wrench).reshape(
-1) # (N*6,) or (N*3,)
@property
def wrench(self):
"""Get the current wrench."""
return self._wrench
@wrench.setter
def wrench(self, wrench):
"""Set the current wrench expressed in the base link of reference frame."""
if wrench is not None:
if not isinstance(wrench, (list, tuple, np.ndarray)):
raise TypeError(
"Expecting the given 'desired_wrench' to be a tuple/list of np.array, or a np.array, "
"but got instead: {}".format(type(wrench)))
wrench = np.asarray(wrench).reshape(-1) # (6,) or (3,)
self._wrench = wrench
# enable / disable the tasks based on if the wrench was provided or not
if self._wrench is None:
self.disable()
else:
self.enable()
@property
def kp(self):
"""Return the proportional gain / compliance matrix."""
return self._kp
@kp.setter
def kp(self, kp):
"""Set the proportional gain / compliance matrix."""
if kp is None:
kp = 1.
if not isinstance(kp, (float, int, np.ndarray)):
raise TypeError(
"Expecting the given compliance matrix gain kp to be an int, float, np.array, instead "
"got: {}".format(type(kp)))
if isinstance(kp, np.ndarray) and kp.shape != (6, 6):
raise ValueError(
"Expecting the given compliance matrix gain kp to be of shape {}, but instead "
"got shape: {}".format((6, 6), kp.shape))
self._kp = kp
###########
# Methods #
###########
def _update(self, x=None):
"""
Update the task by computing the A matrix and b vector that will be used by the task solver.
"""
x = self.model.get_pose(link=self.distal_link, wrt_link=self.base_link)
dx = np.dot(self.kp, (self.desired_wrench - self.wrench))
# update cartesian task
self._task.set_desired_references(x_des=x, dx_des=dx)
self._task.update()
self._A = self._task.A
self._b = self._task.b
# set wrench to None
self._wrench = None
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)