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 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
#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") rospy.sleep(1)
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)