def __init__(self):
# Timeout for input inactivity
self._timeout = 0.01
# Last input update
self._last_update = rospy.get_time()
# Initializing the gripper interface for the current namespace
self._gripper = GripperInterface()
# Retrieve the publish rate
self._publish_rate = 1000
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._command_sub = rospy.Subscriber('gripper_control/command',
EndeffectorCommand,
self._on_gripper_command)
self._command = 'stop'
self._pos_goal = self._gripper.closed_pos
self._ratio_goal = self._gripper.get_position_ratio(
self._gripper.closed_pos)
print 'ratio=', self._ratio_goal
self._kp = 100
if rospy.has_param('~kp'):
self._kp = rospy.get_param('~kp')
if self._gripper.control_joint is None:
raise rospy.ROSException('Gripper cannot be controlled')
self._limits = self._gripper.control_joint_limits
self._controller_update_timer = rospy.Timer(
rospy.Duration(1.0 / self._publish_rate), self._update)
rospy.on_shutdown(self._on_shutdown)
def __init__(self):
# Timeout for input inactivity
self._timeout = 0.01
# Initializing the gripper interface for the current namespace
self._gripper = GripperInterface()
# Initial values for the gripper joystick buttons
# Default - B button from XBox 360 controller
self._open_button = 1
if rospy.has_param('~open_button'):
self._open_button = rospy.get_param('~open_button')
# Default - X button from XBox 360 controller
self._close_button = 2
if rospy.has_param('~close_button'):
self._close_button = rospy.get_param('~close_button')
# Default - RB button from XBox 360 controller
self._deadman_button = 5
if rospy.has_param('~deadman_button'):
self._deadman_button = rospy.get_param('~deadman_button')
if rospy.has_param('~exclusion_buttons'):
self._exclusion_buttons = rospy.get_param('~exclusion_buttons')
if type(self._exclusion_buttons) in [float, int]:
self._exclusion_buttons = [int(self._exclusion_buttons)]
elif type(self._exclusion_buttons) == list:
for n in self._exclusion_buttons:
if type(n) != float:
raise rospy.ROSException(
'Exclusion buttons must be an'
' integer index to the joystick button')
else:
self._exclusion_buttons = list()
self._joy_gain = 0.1
if rospy.has_param('~joy_gain'):
self._joy_gain = rospy.get_param('~joy_gain')
# Retrieve the publish rate
self._publish_rate = 100
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._pos_goal = self._gripper.closed_pos
self._ratio_goal = 0.0
if rospy.has_param('~kp'):
self._kp = rospy.get_param('~kp')
else:
self._kp = 100.0
if rospy.has_param('~ki'):
self._ki = rospy.get_param('~ki')
else:
self._ki = 0.0
if rospy.has_param('~kd'):
self._kd = rospy.get_param('~kd')
else:
self._kd = 0.0
self._controllers = dict()
self._controllers[self._gripper.control_joint] = PIDRegulator(
self._kp, self._ki, self._kd, 100)
self._controllers[self._gripper.mimic_joint] = PIDRegulator(
self._kp, self._ki, self._kd, 100)
if self._gripper.control_joint is None:
raise rospy.ROSException('Gripper cannot be controlled')
self._limits = self._gripper.control_joint_limits
self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback)
rate = rospy.Rate(self._publish_rate)
while not rospy.is_shutdown():
self._update()
rate.sleep()
class GripperController:
def __init__(self):
# Timeout for input inactivity
self._timeout = 0.01
# Initializing the gripper interface for the current namespace
self._gripper = GripperInterface()
# Initial values for the gripper joystick buttons
# Default - B button from XBox 360 controller
self._open_button = 1
if rospy.has_param('~open_button'):
self._open_button = rospy.get_param('~open_button')
# Default - X button from XBox 360 controller
self._close_button = 2
if rospy.has_param('~close_button'):
self._close_button = rospy.get_param('~close_button')
# Default - RB button from XBox 360 controller
self._deadman_button = 5
if rospy.has_param('~deadman_button'):
self._deadman_button = rospy.get_param('~deadman_button')
if rospy.has_param('~exclusion_buttons'):
self._exclusion_buttons = rospy.get_param('~exclusion_buttons')
if type(self._exclusion_buttons) in [float, int]:
self._exclusion_buttons = [int(self._exclusion_buttons)]
elif type(self._exclusion_buttons) == list:
for n in self._exclusion_buttons:
if type(n) != float:
raise rospy.ROSException(
'Exclusion buttons must be an'
' integer index to the joystick button')
else:
self._exclusion_buttons = list()
self._joy_gain = 0.1
if rospy.has_param('~joy_gain'):
self._joy_gain = rospy.get_param('~joy_gain')
# Retrieve the publish rate
self._publish_rate = 100
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._pos_goal = self._gripper.closed_pos
self._ratio_goal = 0.0
if rospy.has_param('~kp'):
self._kp = rospy.get_param('~kp')
else:
self._kp = 100.0
if rospy.has_param('~ki'):
self._ki = rospy.get_param('~ki')
else:
self._ki = 0.0
if rospy.has_param('~kd'):
self._kd = rospy.get_param('~kd')
else:
self._kd = 0.0
self._controllers = dict()
self._controllers[self._gripper.control_joint] = PIDRegulator(
self._kp, self._ki, self._kd, 100)
self._controllers[self._gripper.mimic_joint] = PIDRegulator(
self._kp, self._ki, self._kd, 100)
if self._gripper.control_joint is None:
raise rospy.ROSException('Gripper cannot be controlled')
self._limits = self._gripper.control_joint_limits
self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback)
rate = rospy.Rate(self._publish_rate)
while not rospy.is_shutdown():
self._update()
rate.sleep()
def _update(self):
# Compute the necessary effort for the requested joint position
self._pos_goal = self._ratio_goal * (self._gripper.fully_open_pos - self._gripper.closed_pos) + \
self._gripper.closed_pos
# Set the torque for the control joint
error = self._pos_goal - self._gripper.control_joint_position
torque = self._controllers[self._gripper.control_joint].regulate(
error, rospy.get_time())
self._gripper.set_controller_command(self._gripper.control_joint,
torque)
# Set the torque for the mimic joint
error = self._pos_goal - self._gripper.mimic_joint_position
torque = self._controllers[self._gripper.mimic_joint].regulate(
error, rospy.get_time())
self._gripper.set_controller_command(self._gripper.mimic_joint, torque)
if self._gripper.is_parallel:
pass
def _joy_callback(self, joy):
try:
if self._deadman_button > -1:
if not joy.buttons[self._deadman_button]:
return
for n in self._exclusion_buttons:
if joy.buttons[n] == 1:
return
self._ratio_goal += self._joy_gain * (
joy.buttons[self._open_button] -
joy.buttons[self._close_button])
if self._ratio_goal < 0:
self._ratio_goal = 0.0
if self._ratio_goal > 1:
self._ratio_goal = 1.0
except Exception, e:
print 'Error occurred while parsing joystick input, check if the joy_id corresponds to the joystick ' \
'being used. message=%s' % str(e)
def __init__(self):
# Timeout for input inactivity
self._timeout = 0.01
# Initializing the gripper interface for the current namespace
self._gripper = GripperInterface()
# Initial values for the gripper joystick buttons
# Default - B button from XBox 360 controller
self._open_button = 1
if rospy.has_param('~open_button'):
self._open_button = rospy.get_param('~open_button')
# Default - X button from XBox 360 controller
self._close_button = 2
if rospy.has_param('~close_button'):
self._close_button = rospy.get_param('~close_button')
# Default - RB button from XBox 360 controller
self._deadman_button = 5
if rospy.has_param('~deadman_button'):
self._deadman_button = rospy.get_param('~deadman_button')
if rospy.has_param('~exclusion_buttons'):
self._exclusion_buttons = rospy.get_param('~exclusion_buttons')
if type(self._exclusion_buttons) in [float, int]:
self._exclusion_buttons = [int(self._exclusion_buttons)]
elif type(self._exclusion_buttons) == list:
for n in self._exclusion_buttons:
if type(n) != float:
raise rospy.ROSException('Exclusion buttons must be an'
' integer index to the joystick button')
else:
self._exclusion_buttons = list()
self._joy_gain = 0.1
if rospy.has_param('~joy_gain'):
self._joy_gain = rospy.get_param('~joy_gain')
# Retrieve the publish rate
self._publish_rate = 100
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._pos_goal = self._gripper.closed_pos
self._ratio_goal = 0.0
if rospy.has_param('~kp'):
self._kp = rospy.get_param('~kp')
else:
self._kp = 100.0
if rospy.has_param('~ki'):
self._ki = rospy.get_param('~ki')
else:
self._ki = 0.0
if rospy.has_param('~kd'):
self._kd = rospy.get_param('~kd')
else:
self._kd = 0.0
self._controllers = dict()
self._controllers[self._gripper.control_joint] = PIDRegulator(self._kp, self._ki, self._kd, 100)
self._controllers[self._gripper.mimic_joint] = PIDRegulator(self._kp, self._ki, self._kd, 100)
if self._gripper.control_joint is None:
raise rospy.ROSException('Gripper cannot be controlled')
self._limits = self._gripper.control_joint_limits
self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback)
rate = rospy.Rate(self._publish_rate)
while not rospy.is_shutdown():
self._update()
rate.sleep()
class GripperController:
def __init__(self):
# Timeout for input inactivity
self._timeout = 0.01
# Initializing the gripper interface for the current namespace
self._gripper = GripperInterface()
# Initial values for the gripper joystick buttons
# Default - B button from XBox 360 controller
self._open_button = 1
if rospy.has_param('~open_button'):
self._open_button = rospy.get_param('~open_button')
# Default - X button from XBox 360 controller
self._close_button = 2
if rospy.has_param('~close_button'):
self._close_button = rospy.get_param('~close_button')
# Default - RB button from XBox 360 controller
self._deadman_button = 5
if rospy.has_param('~deadman_button'):
self._deadman_button = rospy.get_param('~deadman_button')
if rospy.has_param('~exclusion_buttons'):
self._exclusion_buttons = rospy.get_param('~exclusion_buttons')
if type(self._exclusion_buttons) in [float, int]:
self._exclusion_buttons = [int(self._exclusion_buttons)]
elif type(self._exclusion_buttons) == list:
for n in self._exclusion_buttons:
if type(n) != float:
raise rospy.ROSException('Exclusion buttons must be an'
' integer index to the joystick button')
else:
self._exclusion_buttons = list()
self._joy_gain = 0.1
if rospy.has_param('~joy_gain'):
self._joy_gain = rospy.get_param('~joy_gain')
# Retrieve the publish rate
self._publish_rate = 100
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._pos_goal = self._gripper.closed_pos
self._ratio_goal = 0.0
if rospy.has_param('~kp'):
self._kp = rospy.get_param('~kp')
else:
self._kp = 100.0
if rospy.has_param('~ki'):
self._ki = rospy.get_param('~ki')
else:
self._ki = 0.0
if rospy.has_param('~kd'):
self._kd = rospy.get_param('~kd')
else:
self._kd = 0.0
self._controllers = dict()
self._controllers[self._gripper.control_joint] = PIDRegulator(self._kp, self._ki, self._kd, 100)
self._controllers[self._gripper.mimic_joint] = PIDRegulator(self._kp, self._ki, self._kd, 100)
if self._gripper.control_joint is None:
raise rospy.ROSException('Gripper cannot be controlled')
self._limits = self._gripper.control_joint_limits
self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback)
rate = rospy.Rate(self._publish_rate)
while not rospy.is_shutdown():
self._update()
rate.sleep()
def _update(self):
# Compute the necessary effort for the requested joint position
self._pos_goal = self._ratio_goal * (self._gripper.fully_open_pos - self._gripper.closed_pos) + \
self._gripper.closed_pos
# Set the torque for the control joint
error = self._pos_goal - self._gripper.control_joint_position
torque = self._controllers[self._gripper.control_joint].regulate(error, rospy.get_time())
self._gripper.set_controller_command(self._gripper.control_joint, torque)
# Set the torque for the mimic joint
error = self._pos_goal - self._gripper.mimic_joint_position
torque = self._controllers[self._gripper.mimic_joint].regulate(error, rospy.get_time())
self._gripper.set_controller_command(self._gripper.mimic_joint, torque)
if self._gripper.is_parallel:
pass
def _joy_callback(self, joy):
try:
if self._deadman_button > -1:
if not joy.buttons[self._deadman_button]:
return
for n in self._exclusion_buttons:
if joy.buttons[n] == 1:
return
self._ratio_goal += self._joy_gain * (joy.buttons[self._open_button] - joy.buttons[self._close_button])
if self._ratio_goal < 0:
self._ratio_goal = 0.0
if self._ratio_goal > 1:
self._ratio_goal = 1.0
except Exception, e:
print 'Error occurred while parsing joystick input, check if the joy_id corresponds to the joystick ' \
'being used. message=%s' % str(e)
class GripperController(object):
COMMAND = ['move', 'stop']
def __init__(self):
# Timeout for input inactivity
self._timeout = 0.01
# Last input update
self._last_update = rospy.get_time()
# Initializing the gripper interface for the current namespace
self._gripper = GripperInterface()
# Retrieve the publish rate
self._publish_rate = 1000
if rospy.has_param('~publish_rate'):
self._publish_rate = rospy.get_param('~publish_rate')
if self._publish_rate <= 0:
raise rospy.ROSException('Invalid negative publish rate')
self._command_sub = rospy.Subscriber('gripper_control/command',
EndeffectorCommand,
self._on_gripper_command)
self._command = 'stop'
self._pos_goal = self._gripper.closed_pos
self._ratio_goal = self._gripper.get_position_ratio(
self._gripper.closed_pos)
print 'ratio=', self._ratio_goal
self._kp = 100
if rospy.has_param('~kp'):
self._kp = rospy.get_param('~kp')
if self._gripper.control_joint is None:
raise rospy.ROSException('Gripper cannot be controlled')
self._limits = self._gripper.control_joint_limits
self._controller_update_timer = rospy.Timer(
rospy.Duration(1.0 / self._publish_rate), self._update)
rospy.on_shutdown(self._on_shutdown)
def _on_shutdown(self):
self._controller_update_timer.shutdown()
def _on_gripper_command(self, command):
if not self._gripper.is_ready:
return
self._command = command.command
if self._command == 'move':
self._ratio_goal = command.ratio + self._gripper.control_joint_pos_ratio
elif self._command == 'stop':
self._ratio_goal = self._gripper.control_joint_pos_ratio
if self._ratio_goal < 0:
self._ratio_goal = 0.0
elif self._ratio_goal > 1:
self._ratio_goal = 1.0
self._last_update = rospy.get_time()
def _reset_gripper_command(self):
self._command = 'stop'
def _update(self, event):
if rospy.get_time() - self._last_update > self._timeout:
self._reset_gripper_command()
self._pos_goal = self._ratio_goal * (
self._gripper.fully_open_pos -
self._gripper.closed_pos) + self._gripper.closed_pos
error = self._pos_goal - self._gripper.control_joint_position
torque = self._kp * error
self._gripper.set_command(torque)