def gripper_control(action):
"""Main loop which requests new commands and publish them on the SModelRobotOutput topic."""
# rospy.init_node('SModelSimpleController')
topic_name = 'left_hand/command'
# topic_name = 'right_hand/command'
pub = rospy.Publisher(topic_name, SModelRobotOutput)
command = SModelRobotOutput()
gripper_cmd = action[-1]
if action[-1] == 1: # open
gripper_cmd = 'o'
elif action[1] == -1: # close
gripper_cmd = 'c'
if not rospy.is_shutdown():
command = genCommand(gripper_cmd, command)
# print(command)
pub.publish(command)
rospy.sleep(0.1)
def gripper_close(self, gripper):
command = SModelRobotOutput()
command = self.gripper_gen_cmd('a', command)
command = self.gripper_gen_cmd('c', command)
# if not rospy.is_shutdown():
self.gripper_pub_cmd.publish(command)
rospy.sleep(2.0)
rospy.logwarn("Gripper Closed")
def gripper_open(self):
command = SModelRobotOutput()
command = self.gripper_gen_cmd('a', command)
command = self.gripper_gen_cmd('o', command)
self.gripper_pub_cmd.publish(command)
rospy.sleep(2.0)
rospy.logwarn("Gripper Opened")
def gripper_activate(self):
command = SModelRobotOutput()
command = self.gripper_gen_cmd('a', command)
# if not rospy.is_shutdown():
self.gripper_pub_cmd.publish(command)
# print(command)
rospy.sleep(1.0)
rospy.logwarn("Gripper Activated")
def gripper_reset(self):
command = SModelRobotOutput()
command = self.gripper_gen_cmd('r',command)
# if not rospy.is_shutdown():
self.gripper_pub_cmd.publish(command)
# print(command)
rospy.sleep(1.0)
rospy.logwarn("Gripper Reset")
def activate(self):
self.command = SModelRobotOutput()
self.command.rACT = 1
self.command.rGTO = 1
self.command.rSPA = 255
self.command.rFRA = self.gripper_force
self.publish_command(self.command)
self._sit_and_wait_activation()
self.open()
return self
def gripper_close(self, gripper):
command = SModelRobotOutput()
command = self.gripper_gen_cmd('a', command)
if gripper == 'left':
command = self.gripper_gen_cmd('c', command)
# if not rospy.is_shutdown():
self.gripper_left_pub_cmd.publish(command)
rospy.sleep(2.0)
rospy.logwarn("Left Gripper Closed")
if gripper == 'right':
command = self.gripper_gen_cmd('c', command)
# if not rospy.shutdown():
self.gripper_right_pub_cmd.publish(command)
rospy.sleep(2.0)
rospy.logwarn("Right Gripper Closed")
def gripper_reset(self, gripper):
command = SModelRobotOutput()
if gripper == 'left':
command = self.gripper_gen_cmd('r', command)
# if not rospy.is_shutdown():
self.gripper_left_pub_cmd.publish(command)
# print(command)
rospy.sleep(1.0)
rospy.logwarn("Left Gripper Reset")
if gripper == 'right':
command = self.gripper_gen_cmd('r', command)
# if not rospy.is_shutdown():
self.gripper_right_pub_cmd.publish(command)
rospy.sleep(1.0)
rospy.logwarn("Right Gripper Reset")
def publisher():
"""Main loop which requests new commands and publish them on the SModelRobotOutput topic."""
rospy.init_node('SModelSimpleController')
topic_name = rospy.get_param('~topic', '/UR_1/SModelRobotOutput')
pub = rospy.Publisher(topic_name, SModelRobotOutput)
command = SModelRobotOutput()
while not rospy.is_shutdown():
command = genCommand(askForCommand(command), command)
pub.publish(command)
rospy.sleep(0.1)
def __init__(self,
control_topic='/UR_1/SModelRobotOutput',
feedback_topic='/UR_1/SModelRobotInput',
gripper_force=10,
open_value=0.0,
close_value=1.0,
register_for_shutdown_signal=False):
# initialize interface node (if needed)
if rospy.get_node_uri() is None:
self.node_name = 'robotiq_s_interface_node'
rospy.init_node(self.node_name)
ROS_INFO(
"The Gripper interface was created outside a ROS node, a new node will be initialized!"
)
else:
ROS_INFO(
"The Gripper interface was created within a ROS node, no need to create a new one!"
)
# parameters
self._open_value = float(max(0.0, min(open_value, 1.0)))
self._close_value = float(max(0.0, min(close_value, 1.0)))
# status of the gripper
self.status_raw = None
self.is_shutdown = False
self.grasp_mode = GraspMode.UNITIALIZED
# create gripper commands publisher
self.control_topic = control_topic
self.pub = rospy.Publisher(control_topic,
SModelRobotOutput,
queue_size=1)
# create gripper status subscriber
self.feedback_topic = feedback_topic
self.sub = rospy.Subscriber(feedback_topic,
SModelRobotInput,
self.update_status,
queue_size=1)
self.sub_last_status_message_t = -1
# create command message
self.command = SModelRobotOutput()
# set gripper force
self.gripper_force = gripper_force
self.command.rFRA = self.gripper_force
self.controller_clock_sec = 0.2
self.pinch_mode_closed_unnorm = 113
self._sit_and_wait_status()
self.controller_thread = Thread(target=_gripper_interface_controller,
args=[
self,
])
self.controller_thread.start()
if register_for_shutdown_signal:
signal.signal(signal.SIGINT, self.shutdown)
def reset(self):
self.command = SModelRobotOutput()
self.command.rACT = 0
self.command.rFRA = self.gripper_force
self.publish_command(self.command)
return self
def gripper_gen_cmd(self, char, command):
"""Update the command according to the character entered by the user."""
# command = SModelRobotOutput()
if char == 'a':
command = SModelRobotOutput()
command.rACT = 1
command.rGTO = 1
command.rSPA = 255
command.rFRA = 150
if char == 'r':
command = SModelRobotOutput()
command.rACT = 0
if char == 'c':
command.rPRA = 255
if char == 'o':
command.rPRA = 0
if char == 'b':
command.rMOD = 0
if char == 'p':
command.rMOD = 1
if char == 'w':
command.rMOD = 2
if char == 's':
command.rMOD = 3
#If the command entered is a int, assign this value to rPRA
try:
command.rPRA = int(char)
if command.rPRA > 255:
command.rPRA = 255
if command.rPRA < 0:
command.rPRA = 0
except ValueError:
pass
if char == 'f':
command.rSPA += 25
if command.rSPA > 255:
command.rSPA = 255
if char == 'l':
command.rSPA -= 25
if command.rSPA < 0:
command.rSPA = 0
if char == 'i':
command.rFRA += 25
if command.rFRA > 255:
command.rFRA = 255
if char == 'd':
command.rFRA -= 25
if command.rFRA < 0:
command.rFRA = 0
# print("generated command: ", command)
return command
#interpreter.execute("go " + "up" + " " + str(0.05))
#finger thang
gripper_publisher = rospy.Publisher("/l_gripper/SModelRobotOutput",
SModelRobotOutput)
#activate_gripper = SModelRobotOutput()
#activate_gripper.rACT = 1
#activate_gripper.rGTO = 1
#activate_gripper.rSPA = 255
#activate_gripper.rFRA = 150
#open_gripper = activate_gripper
#close_gripper = activate_gripper
#open_gripper.rPRA = 0
#close_gripper.rPRA = 255
gcommand = SModelRobotOutput()
gcommand.rACT = 1
gcommand.rGTO = 1
gcommand.rSPA = 255
gcommand.rFRA = 150
gripper_publisher.publish(gcommand)
#all the other shizzle
rate = rospy.Rate(1)
listener = tf.TransformListener()
stage = 0
retry = 1
while not rospy.is_shutdown():
if stage == 0:
rospy.loginfo("time to go in:")
rospy.loginfo("3")
def send_move_to(self, scissor_pos, finger_1_pos, finger_2_pos,
finger_3_pos, scissor_vel, finger_1_vel, finger_2_vel,
finger_3_vel, scissor_force, finger_1_force,
finger_2_force, finger_3_force):
"""
Sends the specified commands to the Robotiq hand.
:*_pos: Position of the respective joint (in Radians)
:*_vel: Velocity of the respective joint (in Radians / second)
:*_vel: Force of the respective joint (percentage in [0, 1] of the maximal force)
"""
output_msg = SModelRobotOutput()
output_msg.rACT = 1
output_msg.rGTO = 1
output_msg.rICF = 1
output_msg.rICS = 1
def to_integer_range_value(val, lower_limit, upper_limit):
return min(
max(
0,
int((val - lower_limit) / (upper_limit - lower_limit) *
255)), 255)
output_msg.rPRA = to_integer_range_value(finger_1_pos,
self.MIN_FINGER_JOINT_VALUE,
self.MAX_FINGER_JOINT_VALUE)
output_msg.rPRB = to_integer_range_value(finger_2_pos,
self.MIN_FINGER_JOINT_VALUE,
self.MAX_FINGER_JOINT_VALUE)
output_msg.rPRC = to_integer_range_value(finger_3_pos,
self.MIN_FINGER_JOINT_VALUE,
self.MAX_FINGER_JOINT_VALUE)
output_msg.rPRS = to_integer_range_value(scissor_pos,
self.MIN_SCISSOR_VALUE,
self.MAX_SCISSOR_VALUE)
output_msg.rSPA = to_integer_range_value(finger_1_vel, 0.0,
self.MAX_ROTATIONAL_SPEED)
output_msg.rSPB = to_integer_range_value(finger_2_vel, 0.0,
self.MAX_ROTATIONAL_SPEED)
output_msg.rSPC = to_integer_range_value(finger_3_vel, 0.0,
self.MAX_ROTATIONAL_SPEED)
output_msg.rSPS = to_integer_range_value(scissor_vel, 0.0,
self.MAX_ROTATIONAL_SPEED)
output_msg.rFRA = to_integer_range_value(finger_1_force, 0.0, 1.0)
output_msg.rFRB = to_integer_range_value(finger_2_force, 0.0, 1.0)
output_msg.rFRC = to_integer_range_value(finger_3_force, 0.0, 1.0)
output_msg.rFRS = to_integer_range_value(scissor_force, 0.0, 1.0)
self._command_publisher.publish(output_msg)
def activate(self):
msg = SModelRobotOutput()
msg.rACT = 1
self._command_publisher.publish(msg)
def _turn_on_robotiq(self):
msg = SModelRobotOutput()
msg.rACT = 1
self._robotiq_publisher.publish(msg)
