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)