def _get_gripper(self, gripper_reverse):
try:
self._gripper=intera_interface.get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper, intera_interface.SimpleClickSmartGripper)
self._gripper_reverse=gripper_reverse
if self._is_clicksmart:
if self._gripper.needs_init():
self._gripper.initialize()
_signals=self._gripper.get_ee_signals()
if 'grip' in _signals:
self._gripper_type='grip'
elif 'vacuumOn' in _signals:
self._gripper_type='vacuum'
else:
self._gripper_type='unknown'
else:
if not (self._gripper.is_calibrated() or
self._gripper.calibrate() == True):
raise
except:
self._gripper=None
self._is_clicksmart=False
self._gripper_type=None
self._gripper_reverse=None
def handle_request(req):
try:
gripper = get_current_gripper_interface()
is_clicksmart = isinstance(gripper, SimpleClickSmartGripper)
if is_clicksmart:
if gripper.needs_init():
gripper.initialize()
else:
if not (gripper.is_calibrated() or
gripper.calibrate() == True):
raise
return
if(req.command=="open"):
try:
gripper.set_ee_signal_value('grip', True)
print "Gripper is open"
except Exception as e:
print e
return OpenGripperResponse(True)
elif(req.command=="close"):
try:
gripper.set_ee_signal_value('grip', False)
print "Gripper is closed"
except Exception as e:
print e
return OpenGripperResponse(False)
except:
gripper = None
is_clicksmart = False
else:
raise rospy.ServiceException("invalid command")
def is_gripper_removed():
"""
Verify grippers are removed for calibration.
"""
try:
gripper = intera_interface.get_current_gripper_interface()
except Exception, e:
return True
def is_gripper_removed():
"""
Verify grippers are removed for calibration.
"""
try:
gripper = intera_interface.get_current_gripper_interface()
except Exception as e:
return True
rospy.logerr("Calibration Client: Cannot calibrate with grippers attached."
" Remove grippers before calibration!")
return False
def init():
global g_sub_open, g_sub_close, g_gripper, g_is_pneumatic
g_sub_open = rospy.Subscriber('/cairo/sawyer_gripper_open', std_msgs.msg.Empty, open_grip)
g_sub_close = rospy.Subscriber('/cairo/sawyer_gripper_close', std_msgs.msg.Empty, close_grip)
g_gripper = intera_interface.get_current_gripper_interface()
g_is_pneumatic = isinstance(g_gripper, intera_interface.SimpleClickSmartGripper)
if g_is_pneumatic:
if g_gripper.needs_init():
g_gripper.initialize()
if g_gripper.needs_init(): return False
else:
if not g_gripper.is_calibrated():
g_gripper.calibrate()
if not g_gripper.is_calibrated(): return False
return True
def handle_request(req):
try:
gripper = get_current_gripper_interface()
isOpen = gripper.__dict__['gripper_io'].__dict__['signals'].get(
'open_B1ZwR2PmT7').get('data')
isClosed = gripper.__dict__['gripper_io'].__dict__['signals'].get(
'closed_H1GwChPmT7').get('data')
is_clicksmart = isinstance(gripper, SimpleClickSmartGripper)
return GripperGraspingResponse(isOpen, isClosed)
except:
gripper = None
is_clicksmart = False
else:
raise rospy.ServiceException("invalid command")
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._cuff = Cuff(limb=arm)
# connect callback fns to signals
self._lights = None
if lights:
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format(arm))
try:
self._gripper = get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper,
SimpleClickSmartGripper)
if self._is_clicksmart:
if self._gripper.needs_init():
self._gripper.initialize()
else:
if not (self._gripper.is_calibrated()
or self._gripper.calibrate() == True):
raise
self._cuff.register_callback(self._close_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
except:
self._gripper = None
self._is_clicksmart = False
msg = ("{0} Gripper is not connected to the robot."
" Running cuff-light connection only.").format(
arm.capitalize())
rospy.logwarn(msg)
def main():
"""Save / Load EndEffector Config utility
Read current config off of ClickSmart and save to file.
Or load config from file and reconfigure and save it to ClickSmart device.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument('-s',
'--save',
metavar='PATH',
help='save current EE config to given file')
parser.add_argument('-l',
'--load',
metavar='PATH',
help='load config from given file onto EE')
args = parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node('ee_config_editor', anonymous=True)
ee = intera_interface.get_current_gripper_interface()
if not ee:
rospy.logerr("Could not detect an attached EndEffector!")
return
if args.save:
rospy.loginfo("Saving EE config to {}".format(args.save))
save_config(ee, args.save)
if args.load:
rospy.loginfo(
"Loading config and writing config to ClickSmart from {}".format(
args.load))
load_config(ee, args.load)
def clean_shutdown():
print("\nExiting example...")
rospy.on_shutdown(clean_shutdown)
def __init__(self):
self.old_time = time.time()
self.learner_quotient = 0
self.interaction_counter = 0
self.total_interactions = 0
self.time_elapsed = 0
self.learner_interaction = False
self.old_time = time.time()
self.new_time = time.time()
self.waypoints = []
self.waypoints_back = []
self.waypoint_limit = 99
self.effort = []
self.new_angle = 0
self.old_angle = 0
self.changed_angle = 0
self.eventnumber = 0
self.iframe_sequence_pub = rospy.Publisher('/iframe_sequence',
Int32,
queue_size=10)
self.unique_input_pub = rospy.Publisher('/unique_input',
String,
queue_size=10)
self.zeroG_pub = rospy.Publisher('/zeroG_topic', String, queue_size=10)
self.gripper_pub = rospy.Publisher('/gripper_control',
Int32,
queue_size=10)
self.lights_pub = rospy.Publisher('/cuff_light', String, queue_size=10)
self.interaction_pub = rospy.Publisher('/interaction_topic',
Int32,
queue_size=10)
rospy.Subscriber('/upper_cuff_button', Int32,
self.upper_cuff_button_callback)
rospy.Subscriber('/lower_cuff_button', Int32,
self.lower_cuff_button_callback)
rospy.Subscriber('/bot_sequence', Int32, self.callback)
rospy.Subscriber('/save_info', String, self.save_info_callback)
rospy.Subscriber('/right_navigator_button', String,
self.navigator_callback)
rospy.init_node('Sawyer_Sparrow_comm_node', anonymous=True)
self.limb = intera_interface.Limb('right')
self._gripper = get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper,
SimpleClickSmartGripper)
self.startPose = self.limb.endpoint_pose()
self.startPose_container = cartPose()
self.startPose_arg = go_to.cartesian_pose_arg()
self.startJointAngles = self.limb.joint_angles()
self.newJointAngles_arg = go_to.joint_angle_arg()
self.newCartPose_arg = go_to.cartesian_pose_arg()
self.newCartPose_arg2 = go_to.cartesian_pose_arg()
self.newCartPose = cartPose()
self.newJointAngles = jointAngles()
self.waypoint_init_container = cartPose()
self.waypoint_init_arg = go_to.cartesian_pose_arg()
#Check if gripper is attached properly
self.grip = gripper_cuff.GripperConnect()
self.savedLocations = []
#Open gripper
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
#set constrained mode
zeroG.constrained(zeroG_all_constraints)
# self.startPose = self.limb.endpoint_pose()
# print(self.limb.endpoint_pose())
# self.startPose_container = self.pose_to_cartclass(pose = self.startPose, cartclass = self.startPose_container)
# print(self.startPose_container.position_z)
# print(self.startPose_container.position_z)
# self.startPose_arg = self.cartclass_to_cartarg(cartclass = self.startPose_container, cartarg = self.startPose_arg)
# print(self.startPose_container.position_z)
# go_to.cartesian_pose(self.startPose_arg)
#Check files and set text file to save to
if (file_save):
self.home = os.path.expanduser("~")
self.dir_path = self.home + '/Learner_Responses/module' + str(
module_number) + '/'
if not os.path.exists(self.dir_path):
os.makedirs(self.dir_path)
self.file_number = 0
self.file_path = self.dir_path + 'test_' + str(
self.file_number) + '.txt'
while (os.path.exists(self.file_path)):
self.file_number += 1
self.file_path = self.dir_path + 'test_' + str(
self.file_number) + '.txt'
self.file = open(self.file_path, 'w')
self.file.write('pretest \r\n' + str(self.old_time) + '\r\n')
self.file.close()
go_to.joint_angles(empty_angle_arg)
# #Complete grip
# grip_msg.data = 0
# self.gripper_pub.publish(grip_msg)
#Test code here
#effort testing
# self.waypoints = []
# self.waypoint_limit = 5
#initialize navigator for user inputs
navigator.right()
from vision import *
from move import *
from neutral_position import neutral_position
from intera_interface import (get_current_gripper_interface)
# Initialise node
rospy.init_node('robot_main')
# Initialise sleep time
rate = rospy.Rate(10)
command = []
gripper = get_current_gripper_interface()
gripper.calibrate()
def callback(data):
if data.data == True:
global start
start = True
# Read command text file and add it to the command list
with open("command.txt", "r") as inputfile:
for line in inputfile:
command.append(line.split(" "))
try:
def __init__(self):
# Initialize Sawyer
rospy.init_node('Sawyer_comm_node', anonymous=True)
intera_interface.HeadDisplay().display_image('logo.png')
# Publishing topics
suppress_cuff_interaction = rospy.Publisher(
'/robot/limb/right/suppress_cuff_interaction', Empty, queue_size=1)
self.position_topic = rospy.Publisher('/position',
JointInfo,
queue_size=1)
self.velocity_topic = rospy.Publisher('/velocity',
JointInfo,
queue_size=1)
self.effort_topic = rospy.Publisher('/effort', JointInfo, queue_size=1)
self.dev_topic = rospy.Publisher('/dev_topic', Int32, queue_size=10)
self.scroll_wheel_button_topic = rospy.Publisher(
'/scroll_wheel_button_topic', Empty, queue_size=10)
self.command_complete_topic = rospy.Publisher('/command_complete',
Empty,
queue_size=1)
self.wheel_delta_topic = rospy.Publisher('/wheel_delta',
Int32,
queue_size=10)
self.clicked = rospy.Publisher('scroll_wheel_pressed',
Bool,
queue_size=10)
self.toggle_completed_topic = rospy.Publisher('/toggle_completed',
Empty,
queue_size=1)
self.highTwo_success_topic = rospy.Publisher('/highTwo_success',
Bool,
queue_size=1)
self.endpoint_topic = rospy.Publisher('/EndpointInfo',
EndpointInfo,
queue_size=10)
self.interaction_control_topic = rospy.Publisher('/InteractionControl',
InteractionControl,
queue_size=10)
self.pickedup_topic = rospy.Publisher('/pickedup', Bool, queue_size=1)
self.pos_orient_topic = rospy.Publisher('/pos_orient',
String,
queue_size=1)
# Subscribing topics
rospy.Subscriber('/audio_duration', Float64, self.rx_audio_duration)
rospy.Subscriber('/robot/joint_states', sensor_msgs.msg.JointState,
self.forwardJointState)
rospy.Subscriber('/GoToJointAngles', GoToJointAngles,
self.cb_GoToJointAngles)
rospy.Subscriber('/wheel_subscription', Bool,
self.cb_WheelSubscription)
rospy.Subscriber('/joint_move', joint_move, self.cb_joint_move)
rospy.Subscriber('/InteractionControl', InteractionControl,
self.cb_interaction)
rospy.Subscriber('/JointAngle', String, self.cb_joint_angle)
rospy.Subscriber('/JointImpedance', JointImpedance, self.cb_impedance)
rospy.Subscriber('/multiple_choice', Bool, self.cb_multiple_choice)
rospy.Subscriber('/highTwo', Bool, self.cb_highTwo)
rospy.Subscriber('/robot/limb/right/endpoint_state',
intera_core_msgs.msg.EndpointState,
self.forwardEndpointState)
rospy.Subscriber('/adjustPoseTo', adjustPoseTo, self.cb_adjustPoseTo)
rospy.Subscriber('/closeGripper', Bool, self.cb_closeGripper)
rospy.Subscriber('/openGripper', Bool, self.cb_openGripper)
rospy.Subscriber('/GoToCartesianPose', GoToCartesianPose,
self.cb_GoToCartesianPose)
rospy.Subscriber('/cuffInteraction', cuffInteraction,
self.cb_cuffInteraction)
rospy.Subscriber('/check_pickup', Bool, self.cb_check_pickup)
rospy.Subscriber('/adjustPoseBy', adjustPoseBy, self.cb_adjustPoseBy)
rospy.Subscriber('/camera', Bool, self.cb_camera)
rospy.Subscriber('/robot/digital_io/right_lower_button/state',
intera_core_msgs.msg.DigitalIOState,
self.cb_cuff_lower)
rospy.Subscriber('/robot/digital_io/right_upper_button/state',
intera_core_msgs.msg.DigitalIOState,
self.cb_cuff_upper)
# Global Vars
self.audio_duration = 0
self.finished = False
self.startPos = 0
self.devMode = False
self.seqArr = []
self.p_command = lambda self: self.pub_num(180 / math.pi * self.limb.
joint_angle(shoulder))
wheel = self.finished
b_less = lambda self: self.limb.joint_angle(shoulder) < point_b[0]
b_more = lambda self: self.limb.joint_angle(shoulder) > point_b[0]
dot_2 = lambda self: self.limb.joint_angle(shoulder) < joint_dot_2[0]
#endpoint = lambda self : self.endpoints_equal(self.limb.endpoint_pose(),dot_3,tol) or self.finished
# Limb
self.limb = LimbPlus()
self.limb.go_to_joint_angles()
# Lights
self.lights = intera_interface.Lights()
for light in self.lights.list_all_lights():
self.lights.set_light_state(light, False)
# Navigator
self.navigator = intera_interface.Navigator()
self.multi_choice_callback_ids = self.BUTTON.copy()
for key in self.multi_choice_callback_ids:
self.multi_choice_callback_ids[key] = -1
self.wheel_callback_id = -1
self.wheel_state = self.navigator.get_wheel_state('right_wheel')
self.navigator.register_callback(self.rx_finished, 'right_button_ok')
self.navigator.register_callback(self.rx_cheat_code, 'head_button_ok')
# Gripper
self.gripper = intera_interface.get_current_gripper_interface()
if isinstance(self.gripper, intera_interface.SimpleClickSmartGripper):
if self.gripper.needs_init():
self.gripper.initialize()
else:
if not (self.gripper.is_calibrated()
or self.gripper.calibrate() == True):
raise
self.open_gripper()
# Cuff
self.cuff = intera_interface.Cuff()
self.cuff_callback_ids = self.CUFF_BUTTON.copy()
for key in self.cuff_callback_ids:
self.cuff_callback_ids[key] = -1
# Initialization complete. Spin.
rospy.loginfo('Ready.')
r = rospy.Rate(10)
while not rospy.is_shutdown():
suppress_cuff_interaction.publish()
r.sleep()
def __init__(self):
self.slideEvent = True
self.slideClose = False
self.movedBoxes = 0
self.ready = False
self.last_msg = Int32()
self.zeroG_pub = rospy.Publisher('/zeroG_topic', String, queue_size=10)
self.gripper_pub = rospy.Publisher('/gripper_control',
Int32,
queue_size=10)
self.lights_pub = rospy.Publisher('/cuff_light', String, queue_size=10)
self.return_pub = rospy.Publisher('/sawyer_slider_return',
Int32,
queue_size=2)
rospy.Subscriber('/sawyer_sequence', Int32, self.callback)
#rospy.Subscriber('/sawyer_mode', Int32, self.sawyerMode_callback)
#rospy.Subscriber('/sawyer_time', Float32, self.sawyerTime_callback)
rospy.init_node('Sawyer_node', anonymous=True)
self.limb = intera_interface.Limb('right')
self._gripper = get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper,
SimpleClickSmartGripper)
self.startPose = self.limb.endpoint_pose()
self.startPose_container = cartPose()
self.startPose_arg = go_to.cartesian_pose_arg()
self.startJointAngles = self.limb.joint_angles()
self.newCartPose_container = cartPose()
self.newJointAngles_arg = go_to.joint_angle_arg()
self.newCartPose_arg = go_to.cartesian_pose_arg()
self.newCartPose_arg2 = go_to.cartesian_pose_arg()
self.newCartPose = cartPose()
self.newJointAngles = jointAngles()
self.waypoint_init_container = cartPose()
self.waypoint_init_arg = go_to.cartesian_pose_arg()
#Check if gripper is attached properly
self.grip = gripper_cuff.GripperConnect()
#Open gripper
grip_msg.data = 1
self.gripper_pub.publish(grip_msg)
if (go_to.joint_angles(empty_angle_arg)):
int_msg.data = 1
self.return_pub.publish(int_msg)
# go_to.joint_angles(step2_conveyor_arg)
# self.startPose = self.limb.endpoint_pose()
# self.startPose_container = self.pose_to_cartclass(self.startPose, self.startPose_container)
# self.startPose_arg = self.cartclass_to_cartarg(self.startPose_container, self.startPose_arg)
# self.startPose_container.position_z = z_conveyor + box_height - gripper_width
# self.newCartPose_arg = self.cartclass_to_cartarg(self.startPose_container, self.newCartPose_arg)
# if(go_to.cartesian_pose(self.newCartPose_arg)):
# grip_msg.data = 0
# self.gripper_pub.publish(grip_msg)
# rospy.sleep(1)
# go_to.cartesian_pose(self.startPose_arg)
# go_to.cartesian_pose(self.newCartPose_arg)
# grip_msg.data = 1
# self.gripper_pub.publish(grip_msg)
# go_to.cartesian_pose(self.startPose_arg)
navigator.right()
