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)
