def __init__(self, speed=0.28, accuracy=0.003726646, timeout=5):
self.sawyer_arm = intera_interface.Limb('right')
self.joint_speed = speed
self.accuracy = accuracy
self.timeout = timeout
self.waypoints = list()
self._is_recording_waypoints = False
self.rs = intera_interface.RobotEnable()
self._init_state = self.rs.state().enabled
self.rs.enable()
# Create Navigator
self.navigator_io = intera_interface.Navigator()
self.sawyer_lights = intera_interface.Lights()
# Create Gripper & Cuff
self.sawyer_gripper = intera_interface.Gripper()
self.sawyer_cuff = intera_interface.Cuff(limb='right')
self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback,
'{0}_button_upper'.format('right'))
self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback,
'{0}_button_lower'.format('right'))
# print(self.navigator_io.list_all_items())
self.sawyer_arm.move_to_neutral(timeout=5, speed=self.joint_speed)
self.sawyer_gripper.open()
self.set_light_status('red', False)
self.set_light_status('green', False)
self.set_light_status('blue', True)
def _init_nodes(self, limb, light):
try:
self._limb=intera_interface.Limb(limb)
self._head=intera_interface.Head()
self._light=SawyerLight(light)
#self._head_display=intera_interface.HeadDisplay()
self._display=SawyerDisplay()
self._cuff=intera_interface.Cuff()
self._limits=intera_interface.JointLimits()
self._navigator=intera_interface.Navigator()
self._joint_names=self._limb.joint_names()
self._velocity_limits=self._limits.joint_velocity_limits()
self._stop_cmd={}
for i,name in enumerate(self._joint_names):
self._stop_cmd[name]=0.0
except:
print("Warning caught exception...")
traceback.print_exc()
pass
def __init__(self):
super(LimbPlus, self).__init__()
self.nav = intera_interface.Navigator()
self.icc_pub = rospy.Publisher(
'/robot/limb/right/interaction_control_command',
InteractionControlCommand,
queue_size=1)
def __init__(self, control_rate, robot_name, print_debug):
self.max_release = 0
self._print_debug = print_debug
RobotController.__init__(self)
self.sem_list = [Semaphore(value=0)]
self._status_mutex = Lock()
self.robot_name = robot_name
self._desired_gpos = GRIPPER_OPEN
self.gripper_speed = 300
self._force_counter = 0
self._integrate_gripper_force, self._last_integrate = 0., None
self.num_timeouts = 0
self._cmd_publisher = rospy.Publisher(
'/robot/limb/right/joint_command', JointCommand, queue_size=100)
self.gripper_pub = rospy.Publisher('/wsg_50_driver/goal_position',
Cmd,
queue_size=10)
rospy.Subscriber("/wsg_50_driver/status", Status,
self._gripper_callback)
print("waiting for first status")
self.sem_list[0].acquire()
print('gripper initialized!')
self.control_rate = rospy.Rate(control_rate)
self._navigator = intera_interface.Navigator()
self._navigator.register_callback(self._close_gripper_handler,
'right_button_ok')
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-m',
'--markers',
nargs='+',
dest='markers',
help='Valid Marker Points')
parser.add_argument('-c',
'--cameras',
nargs='+',
dest='cameras',
help='Operating Cameras')
parser.add_argument('--validate_calib',
action='store_true',
help='Validate Calibration')
args = parser.parse_args()
marker_list = args.markers
camera_list = args.cameras
v_check = args.validate_calib
marker_set = {0, 1, 2}
if marker_list is not None:
marker_set = set([int(m) for m in marker_list])
camera_set = {0, 1}
if camera_list is not None:
camera_set = set([int(c) for c in camera_list])
rospy.init_node('get_ar_points')
#initialize robot
rs = intera_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
limb_right = intera_interface.Limb("right")
name_of_service = "ExternalTools/right/PositionKinematicsNode/FKService"
fksvc = rospy.ServiceProxy(name_of_service, SolvePositionFK)
#initialize tracking code
camera_tracker = CameraRegister(camera_set,
marker_set,
limb_right,
name_of_service,
fksvc,
validate=v_check)
#init robot ui
navigator = intera_interface.Navigator()
cam_collect = navigator.register_callback(camera_tracker.toggle_collection,
'right_button_ok')
cam_print = navigator.register_callback(camera_tracker.print_callback,
'right_button_square')
robot_point_print = navigator.register_callback(
camera_tracker.print_robot_eep, 'right_button_show')
# rospy.Subscriber("/ar_pose_marker", AlvarMarkers, camera_tracker.camera_track_callback)
print("BEGINNING SPIN")
rospy.spin()
def __init__(self):
self.ctrl = RobotController(self.CONTROL_RATE)
self.recorder = RobotRecorder(save_dir='',
use_aux=False,
save_actions=False,
save_images=False)
self.image_publisher = rospy.Publisher('/robot/head_display',
Image_msg,
queue_size=2)
self.load_splashes()
self.control_rate = rospy.Rate(self.PREDICT_RATE)
self.ui_rate = rospy.Rate(self.UI_RATE)
self.robot_predictor = setup_predictor(
self.ROBOT_MODEL, self.ROBOT_BIAS, self.CROP_H_MIN,
self.CROP_H_MAX, self.CROP_W_MIN, self.CROP_W_MAX)
self.human_predictor = setup_predictor_human(
self.HUMAN_MODEL, self.HUMAN_BIAS, self.CROP_H_MIN,
self.CROP_H_MAX, self.CROP_W_MIN, self.CROP_W_MAX)
self.is_human = False
self.predictor = self.robot_predictor
rospy.Subscriber("/keyboard/keyup", Key, self.keyboard_up_listener)
self.weiss_pub = rospy.Publisher('/wsg_50_driver/goal_position',
Cmd,
queue_size=10)
# rospy.Subscriber("/wsg_50_driver/status", Status, self.weiss_status_listener)
self._navigator = intera_interface.Navigator()
self.demo_collect = self._navigator.register_callback(
self.record_demo, 'right_button_ok')
self.swap_key = self._navigator.register_callback(
self.swap_sawyer_cuff, 'right_button_back')
self.start_key = self._navigator.register_callback(
self.start_traj_cuff, 'right_button_square')
self.neutral_key = self._navigator.register_callback(
self.neutral_cuff, 'right_button_show')
self.x_button = self._navigator.register_callback(
self.gripper_cuff, 'right_button_triangle')
self.calc = EE_Calculator()
print 'ONE SHOT READY!'
self.running = False
self.demo_imgs = None
self.move_netural()
rospy.spin()
def __init__(self, constraint_id, cuff_button):
"""
Parameters
----------
constraint_id : int
The id of the constraint for the trigger.
cuff_button : str
The cuff button name.
"""
self.constraint_id = constraint_id
self.button = cuff_button
self.nav = intera_interface.Navigator()
def __init__(self):
# Initialize Sawyer
#rospy.init_node('image', anonymous=True)
# Publishing topics
#suppress_cuff_interaction = rospy.Publisher('/robot/limb/right/suppress_cuff_interaction', Empty, queue_size=1)
self.received = False
# rospy.Subscriber('/Image', Image, self.get_image)
self.img = np.zeros((480, 752, 3), np.uint8)
self.navigator = intera_interface.Navigator()
def __init__(self, arm, name, start):
uri = "mongodb://*****:*****@sawyer-mongo.documents.azure.com:10255/?ssl=true"
client = MongoClient(uri)
self.db = client.SawyerDB
self.collection = self.db.Commands
self.commandName = name
self.commandNumber = start
rp = RobotParams()
self._lastJoin = {}
self.lastButtonPress = datetime.datetime.now()
self._rs = intera_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self._navigator_io = intera_interface.Navigator()
head_display = intera_interface.HeadDisplay()
head_display.display_image(
"/home/microshak/Pictures/Sawyer_Navigator_Main.png", False, 1.0)
valid_limbs = rp.get_limb_names()
self._arm = rp.get_limb_names()[0]
self._limb = intera_interface.Limb(arm)
if start == None:
limb = intera_interface.Limb("right")
limb.move_to_neutral()
print(self._arm)
# inputs
self._cuff = Cuff(limb='right')
self._navigator = Navigator()
# connect callback fns to signals
self._lights = None
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format('right'))
try:
self._gripper = Gripper(self._arm)
#if not (self._gripper.is_calibrated() or self._gripper.calibrate() == True):
# rospy.logerr("({0}_gripper) calibration failed.".format(self._gripper.name))
# raise
except:
self._gripper = None
msg = ("{0} Gripper is not connected to the robot."
" Running cuff-light connection only.").format(
arm.capitalize())
rospy.logwarn(msg)
def main_sawyer():
import intera_interface
from visual_mpc.envs.robot_envs.sawyer.sawyer_impedance import SawyerImpedanceController
controller = SawyerImpedanceController('sawyer', True, gripper_attached='none') # doesn't initial gripper object even if gripper is attached
def print_eep(value):
if not value:
return
xyz, quat = controller.get_xyz_quat()
yaw, roll, pitch = [np.rad2deg(x) for x in controller.quat_2_euler(quat)]
logging.getLogger('robot_logger').info("XYZ IS: {}, ROTATION IS: yaw={} roll={} pitch={}".format(xyz, yaw, roll, pitch))
navigator = intera_interface.Navigator()
navigator.register_callback(print_eep, 'right_button_show')
rospy.spin()
def echo_input(nav_name='right'):
nav = intera_interface.Navigator()
def back_pressed(v):
print("Button 'Back': {0}".format(nav.button_string_lookup(v)))
def rethink_pressed(v):
print("Button 'Rethink': {0}".format(nav.button_string_lookup(v)))
def circle_pressed(v):
print("Button 'Circle': {0}".format(nav.button_string_lookup(v)))
def square_pressed(v):
print("Button 'Square': {0}".format(nav.button_string_lookup(v)))
def x_pressed(v):
print("Button 'X': {0}".format(nav.button_string_lookup(v)))
def ok_pressed(v):
print("Button 'OK': {0}".format(nav.button_string_lookup(v)))
def wheel_moved(v):
print("Wheel value: {0}".format(v))
nav.register_callback(back_pressed, '_'.join([nav_name, 'button_back']))
nav.register_callback(rethink_pressed, '_'.join([nav_name, 'button_show']))
nav.register_callback(circle_pressed, '_'.join([nav_name,
'button_circle']))
nav.register_callback(square_pressed, '_'.join([nav_name,
'button_square']))
nav.register_callback(x_pressed, '_'.join([nav_name, 'button_triangle']))
nav.register_callback(ok_pressed, '_'.join([nav_name, 'button_ok']))
nav.register_callback(wheel_moved, '_'.join([nav_name, 'wheel']))
print("Press input buttons on the right navigator, "
"input will be echoed here.")
rate = rospy.Rate(1)
i = 0
while not rospy.is_shutdown() and i < 10:
rate.sleep()
i += 1
def __init__(self, control_rate, robot_name):
self.max_release = 0
RobotController.__init__(self)
self.sem_list = [Semaphore(value=0)]
self._status_mutex = Lock()
self.robot_name = robot_name
self._desired_gpos = GRIPPER_OPEN
self.gripper_speed = 300
self._force_counter = 0
self._integrate_gripper_force = 0.
self.num_timeouts = 0
self.gripper_pub = rospy.Publisher('/wsg_50_driver/goal_position',
Cmd,
queue_size=10)
rospy.Subscriber("/wsg_50_driver/status", Status,
self._gripper_callback)
print("waiting for first status")
self.sem_list[0].acquire()
print('gripper initialized!')
self.imp_ctrl_publisher = rospy.Publisher('/desired_joint_pos',
JointState,
queue_size=1)
self.imp_ctrl_release_spring_pub = rospy.Publisher('/release_spring',
Float32,
queue_size=10)
self.imp_ctrl_active = rospy.Publisher('/imp_ctrl_active',
Int64,
queue_size=10)
self.control_rate = rospy.Rate(control_rate)
self.imp_ctrl_release_spring(100)
self.imp_ctrl_active.publish(1)
self._navigator = intera_interface.Navigator()
self._navigator.register_callback(self._close_gripper_handler,
'right_button_ok')
def __init__(self):
"""
Records joint data to a file at a specified rate.
rate: recording frequency in Hertz
"""
print("Initializing node... ")
rospy.init_node("pushback_recorder")
parser = argparse.ArgumentParser(
description='select whehter to record or replay')
parser.add_argument('robot', type=str, help='robot name')
args = parser.parse_args()
self.robotname = args.robot
self.file = '/'.join(
str.split(visual_mpc_rospkg.__file__,
"/")[:-1]) + '/src/utils/pushback_traj_{}.pkl'.format(
self.robotname)
self.rs = intera_interface.RobotEnable(CHECK_VERSION)
self.init_state = self.rs.state().enabled
self.limb = intera_interface.Limb("right")
self._navigator = intera_interface.Navigator()
self.start_callid = self._navigator.register_callback(
self.start_recording, 'right_button_ok')
# Navigator Rethink button press
self.stop_callid = self._navigator.register_callback(
self.stop_recording, 'right_button_show')
self.control_rate = rospy.Rate(800)
self.collect_active = False
rospy.on_shutdown(self.clean_shutdown)
self.joint_pos = []
print('ready for recording!')
rospy.spin()
def __init__(self, item_id, reference_pose, base_frame="world", child_frame="right_gripper_tip", service_name="transform_lookup_service"):
"""
Parameters
----------
robot_id : int
Id of robot assigned in the config.json configuration files.
reference_pose : dict
Dictionary with position and orientation fields
base_frame : str
The base / world frame from which to calculate the transformation to the child frame.
child_frame : str
The ending TF frame used to generate the pose / orientation of the robot (usually the end effector tip).
service_name : str
Name of transformation lookup service used by _get_transform() to calculate the transformation between world_frame and child_frame
"""
self.id = item_id
self.reference_pose = reference_pose
self._limb = intera_interface.Limb("right")
self._cuff = intera_interface.Cuff("right")
self._navigator = intera_interface.Navigator()
try:
self._gripper = intera_interface.Gripper("right")
rospy.loginfo("Electric gripper detected.")
if self._gripper.has_error():
rospy.loginfo("Gripper error...rebooting.")
self._gripper.reboot()
if not self._gripper.is_calibrated():
rospy.loginfo("Calibrating gripper.")
self._gripper.calibrate()
except Exception as e:
self._gripper = None
rospy.loginfo("No electric gripper detected.")
self.base_frame = base_frame
self.child_frame = child_frame
self.tlc = TransformLookupClient(service_name)
def __init__(self, speed, accuracy, limb="right"):
# Create intera_interface limb instance
self._arm = limb
self._limb = intera_interface.Limb(self._arm)
# Parameters which will describe joint position moves
self._speed = speed
self._accuracy = accuracy
# Recorded waypoints
self._waypoints = list()
# Recording state
self._is_recording = False
# Verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# Create Navigator I/O
self._navigator = intera_interface.Navigator()
def __init__(self):
self._navigator = intera_interface.Navigator()
self.cmd_pub = rospy.Publisher('/cairo_lfd/record_commands',
String,
queue_size=10)
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, robot_type, file_name):
"""
Records joint data to a file at a specified rate.
rate: recording frequency in Hertz
"""
if robot_type == 'sawyer':
import intera_interface
from visual_mpc.envs.robot_envs.sawyer.sawyer_impedance import SawyerImpedanceController
self._controller = SawyerImpedanceController(
'recorder_bot', False, gripper_attached='none')
self._controller.move_to_neutral()
# Navigator Rethink button press
self._navigator = intera_interface.Navigator()
self.start_callid = self._navigator.register_callback(
self.start_recording, 'right_button_ok')
self._control_rate = rospy.Rate(800)
self.stop_callid = self._navigator.register_callback(
self.stop_recording, 'right_button_square')
elif robot_type == 'widowx':
import threading
from visual_mpc.envs.robot_envs.widowx.widowx_controller import WidowXController
from keyboard.msg import Key
self._controller = WidowXController('recorder_bot', False)
self._control_rate = rospy.Rate(50)
self._controller.move_to_neutral()
def keyboard_listener(msg):
if msg.code == 115:
rec_thread = threading.Thread(target=self.start_recording,
args=(1, ))
rec_thread.start()
else:
self.stop_recording(1)
rospy.Subscriber("/keyboard/keydown", Key, keyboard_listener)
elif robot_type == 'baxter':
from pynput import mouse
from pynput import keyboard
import baxter_interface
from visual_mpc.envs.robot_envs.baxter.baxter_impedance import BaxterImpedanceController
self._controller = BaxterImpedanceController(
'baxter', False, gripper_attached='none', limb='right')
self._controller.move_to_neutral()
# Navigator Rethink button press
self._navigator = baxter_interface.Navigator('right')
self._navigator1 = baxter_interface.Navigator('left')
self.start_callid = self._navigator.button0_changed.connect(
self.start_recording)
self.stop_callid = self._navigator1.button0_changed.connect(
self.stop_recording)
else:
raise NotImplementedError
self._collect_active = False
self._joint_pos = []
self._file = file_name
logging.getLogger('robot_logger').info('ready for recording!')
rospy.spin()
def __init__(self):
print "Initializing Node"
rospy.init_node('poirot_rrhost')
print "Enabling Robot"
rs = intera_interface.RobotEnable()
rs.enable()
self.rs1 = intera_interface.RobotEnable()
rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback)
rospy.Subscriber("/robot/digital_io/right_lower_cuff/state",
DigitalIOState, self.CuffCallback)
rospy.Subscriber("/robot/digital_io/right_button_ok/state",
DigitalIOState, self.NavWheelCallback)
rospy.Subscriber("/robot/digital_io/right_button_triangle/state",
DigitalIOState, self.XCallback)
self.DIOpub = rospy.Publisher('/io/comms/io/command',
IOComponentCommand,
queue_size=10)
self.h = IOComponentCommand()
self.h.time = rospy.Time.now()
self.h.op = "set"
self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }"
self.nav = intera_interface.Navigator()
#self.nav.register_callback(self.ok_pressed, '_'.join([nav_name, 'button_ok']))
#intera_interface/robot/digital_io/right_lower_cuff/state
# self._valid_limb_names = {'left': 'left',
# 'l': 'left',
# 'right': 'right',
# 'r': 'right'}
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# get information from the SDK
# self._left = intera_interface.Limb('left')
self._right = intera_interface.Limb('right')
#self._l_jnames = self._left.joint_names()
self._r_jnames = self._right.joint_names()
self.kin = sawyer_kinematics('right')
# data initializations
self._jointpos = [0] * 7
self._jointvel = [0] * 7
self._jointtor = [0] * 7
self._ee_pos = [0] * 3
self._ee_or = [0] * 4
self._ee_tw = [0] * 6
self._ee_wr = [0] * 6
self._ee_vel = [0] * 6
self._pijac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._jac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._inertia_mat = [
] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0')
self.Jxl_raw = 0
self.Jyl_raw = 0
self.Jzl_raw = 0
self.Jxa_raw = 0
self.Jya_raw = 0
self.Jza_raw = 0
self.leftbutton = 0
self.rightbutton = 0
self.spacenav = []
#self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7))
self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7))
self.MODE_POSITION = 0
self.MODE_VELOCITY = 1
self.MODE_TORQUE = 2
self._mode = self.MODE_POSITION
self.RMH_delay = .01
# initial joint command is current pose
self.readJointPositions()
#self.setJointCommand('left',self._jointpos[0:7])
self.setJointCommand('right', self._jointpos[0:7])
self.head_display = intera_interface.HeadDisplay()
print('head display!')
self.image = "/home/rachel/ros_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg"
self.head_display.display_image(self.image, display_rate=100)
print('finished first head display')
# Start background threads
self._running = True
self._t_joints = threading.Thread(target=self.jointspace_worker)
self._t_joints.daemon = True
self._t_joints.start()
self._t_effector = threading.Thread(target=self.endeffector_worker)
self._t_effector.daemon = True
self._t_effector.start()
self._t_command = threading.Thread(target=self.command_worker)
self._t_command.daemon = True
self._t_command.start()
self._t_display = threading.Thread(target=self.display_worker)
self._t_display.daemon = True
self._t_display.start()
self._t_dio = threading.Thread(target=self.dio_worker)
self._t_dio.daemon = True
self._t_dio.start()
def __init__(self):
print "Initializing Node"
rospy.init_node('sawyer_peripherals')
self._running = True
# self._valid_limb_names = {'left': 'left',
# 'l': 'left',
# 'right': 'right',
# 'r': 'right'}
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# gripper initialization
#self._grippers = {'right': intera_interface.Gripper('right')}
# self._grippers = {#'left': intera_interface.Gripper('left'),
# 'right': intera_interface.Gripper('right')}
# Set grippers to defaults
# self._grippers['left'].set_parameters(
# self._grippers['left'].valid_parameters())
#self._grippers['right'].set_parameters(
#self._grippers['right'].valid_parameters())
# ranger initialization
#self._rangers = {'right': intera_interface.AnalogIO('right_hand_range')}
# self._rangers = {'left': intera_interface.AnalogIO('left_hand_range'),
# 'right': intera_interface.AnalogIO('right_hand_range')}
# accelerometer initialization
#self._accelerometers = {'left': [0.0]*3, 'right': [0.0]*3}
self._accelerometers = {'right': [0.0] * 3}
# rospy.Subscriber("/robot/accelerometer/left_accelerometer/state",
# Imu,
# self.left_accel_callback)
rospy.Subscriber("/robot/accelerometer/right_accelerometer/state", Imu,
self.right_accel_callback)
# head control initialization
self._head = intera_interface.Head()
# sonar initialization
self._sonar_pointcloud = RR.RobotRaconteurNode.s.NewStructure(
'SawyerPeripheral_interface.SonarPointCloud')
self._sonar_state_sub = rospy.Subscriber(
"/robot/sonar/head_sonar/state", PointCloud, self.sonar_callback)
self._sonar_enable_pub = rospy.Publisher(
"/robot/sonar/head_sonar/set_sonars_enabled", UInt16, latch=True)
# initially all sonar sensors on
self._sonar_enabled = True
self._sonar_enable_pub.publish(4095)
# suppressions
self._suppress_body_avoidance = {'right': False}
#self._suppress_body_avoidance = {'left': False, 'right': False}
self._supp_body_avoid_pubs = {
'left':
rospy.Publisher("/robot/limb/left/suppress_body_avoidance",
Empty,
latch=True),
'right':
rospy.Publisher("/robot/limb/right/suppress_body_avoidance",
Empty,
latch=True)
}
self._supp_body_avoid_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_body_avoidance",
Empty,
latch=True)
}
self._suppress_collision_avoidance = {'right': False}
# self._suppress_collision_avoidance = {'left': False, 'right': False}
self._supp_coll_avoid_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_collision_avoidance",
Empty,
latch=True)
}
# self._supp_coll_avoid_pubs = {'left':
# rospy.Publisher("/robot/limb/left/suppress_collision_avoidance",
# Empty,
# latch=True),
# 'right':
# rospy.Publisher("/robot/limb/right/suppress_collision_avoidance",
# Empty,
# latch=True)}
self._suppress_contact_safety = {'right': False}
# self._suppress_contact_safety = {'left': False, 'right': False}
self._supp_con_safety_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_contact_safety",
Empty,
latch=True)
}
# self._supp_con_safety_pubs = {'left':
# rospy.Publisher("/robot/limb/left/suppress_contact_safety",
# Empty,
# latch=True),
# 'right':
# rospy.Publisher("/robot/limb/right/suppress_contact_safety",
# Empty,
# latch=True)}
self._suppress_cuff_interaction = {'right': False}
# self._suppress_cuff_interaction = {'left': False, 'right': False}
self._supp_cuff_int_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_cuff_interaction",
Empty,
latch=True)
}
# self._supp_cuff_int_pubs = {'left':
# rospy.Publisher("/robot/limb/left/suppress_cuff_interaction",
# Empty,
# latch=True),
# 'right':
# rospy.Publisher("/robot/limb/right/suppress_cuff_interaction",
# Empty,
# latch=True)}
self._suppress_gravity_compensation = {'right': False}
# self._suppress_gravity_compensation = {'left': False, 'right': False}
self._supp_grav_comp_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_gravity_compensation",
Empty,
latch=True)
}
# self._supp_grav_comp_pubs = {'left':
# rospy.Publisher("/robot/limb/left/suppress_gravity_compensation",
# Empty,
# latch=True),
# 'right':
# rospy.Publisher("/robot/limb/right/suppress_gravity_compensation",
# Empty,
# latch=True)}
# start suppressions background thread
self._t_suppressions = threading.Thread(
target=self.suppressions_worker)
self._t_suppressions.daemon = True
self._t_suppressions.start()
# gravity compensation subscription
self._grav_comp_lock = threading.Lock()
self._gravity_compensation_torques = OrderedDict(
zip(intera_interface.Limb('right').joint_names(), [0.0] * 7))
# self._gravity_compensation_torques = OrderedDict(
# zip(intera_interface.Limb('left').joint_names() + \
# intera_interface.Limb('right').joint_names(),
# [0.0]*14))
# rospy.Subscriber("/robot/limb/left/gravity_compensation_torques",
# SEAJointState, self.grav_comp_callback)
rospy.Subscriber("/robot/limb/right/gravity_compensation_torques",
SEAJointState, self.grav_comp_callback)
# navigators
self._navigators = {'right': intera_interface.Navigator('right')}
# self._navigators = {'left': intera_interface.Navigator('left'),
# 'right': intera_interface.Navigator('right'),
# 'torso_left':
# intera_interface.Navigator('torso_left'),
# 'torso_right':
# intera_interface.Navigator('torso_right')}
# initialize frame transform
self._listener = tf.TransformListener()
def __init__(self, file=None):
"""
Records joint data to a file at a specified rate.
rate: recording frequency in Hertz
"""
print("Initializing node... ")
rospy.init_node("pushback_recorder")
parser = argparse.ArgumentParser(
description='select whehter to record or replay')
parser.add_argument('--record', type=str, default='True', help='')
args = parser.parse_args()
self.rs = intera_interface.RobotEnable(CHECK_VERSION)
self.init_state = self.rs.state().enabled
self.limb = intera_interface.Limb("right")
self.joint_names = self.limb.joint_names()
# self.gripper = intera_interface.Gripper("right")
self.name_of_service = "ExternalTools/right/PositionKinematicsNode/FKService"
self.fksvc = rospy.ServiceProxy(self.name_of_service, SolvePositionFK)
self._navigator = intera_interface.Navigator()
self.start_callid = self._navigator.register_callback(
self.start_recording, 'right_button_ok')
# Navigator Rethink button press
self.stop_callid = self._navigator.register_callback(
self.stop_recording, 'right_button_show')
# self.delete_callid = self._navigator.register_callback(self.delete_recording, 'right_button_back')
self.imp_ctrl_publisher = rospy.Publisher('desired_joint_pos',
JointState,
queue_size=1)
self.imp_ctrl_release_spring_pub = rospy.Publisher('release_spring',
Float32,
queue_size=10)
self.imp_ctrl_active = rospy.Publisher('imp_ctrl_active',
Int64,
queue_size=10)
# self.gripper.set_velocity(self.gripper.MAX_VELOCITY) # "set 100% velocity"),
# self.gripper.open()
self.control_rate = rospy.Rate(20)
self.imp_ctrl_active.publish(0)
self.collect_active = False
rospy.on_shutdown(self.clean_shutdown)
self.joint_pos = []
self.set_neutral()
self.recorder = DemoRobotRecorder('/home/sudeep/outputs',
self.N_SAMPLES,
use_aux=False)
self.record_iter = 0
if args.record == 'False':
self.playback(file)
if args.record == 'True':
print 'ready for recording!'
rospy.spin()
raise ValueError('wrong argument!')
def __init__(self):
print "Initializing Node"
rospy.init_node('poirot_rrhost')
print "Enabling Robot"
rs = intera_interface.RobotEnable()
rs.enable()
self.rs1 = intera_interface.RobotEnable()
rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback)
rospy.Subscriber("/robot/digital_io/right_lower_cuff/state",
DigitalIOState, self.CuffCallback)
rospy.Subscriber("/robot/digital_io/right_button_ok/state",
DigitalIOState, self.NavWheelCallback)
rospy.Subscriber("/robot/digital_io/right_button_triangle/state",
DigitalIOState, self.XCallback)
rospy.Subscriber("/robot/digital_io/right_lower_button/state",
DigitalIOState, self.GripperCloseCallback)
rospy.Subscriber("/robot/digital_io/right_upper_button/state",
DigitalIOState, self.GripperOpenCallback)
rospy.Subscriber("/pushed1", Bool, self.pushed1callback)
rospy.Subscriber("/pushed2", Bool, self.pushed2callback)
rospy.Subscriber("/pushed3", Bool, self.pushed3callback)
rospy.Subscriber("/pushed4", Bool, self.pushed4callback)
self.DIOpub = rospy.Publisher('/io/comms/io/command',
IOComponentCommand,
queue_size=10)
self.h = IOComponentCommand()
self.h.time = rospy.Time.now()
self.h.op = "set"
self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }"
rospy.Subscriber("Robotiq2FGripperRobotInput",
inputMsg.Robotiq2FGripper_robot_input,
self.GripperStatusCallback)
self.gripperpub = rospy.Publisher(
'Robotiq2FGripperRobotOutput',
outputMsg.Robotiq2FGripper_robot_output)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.gripperstatus = inputMsg.Robotiq2FGripper_robot_input()
self.grip_des_pos = 444
#self.des_gripper_pos=0
#activate-reset-activate gripper
self.grippercommand.rACT = 1
self.grippercommand.rGTO = 1
self.grippercommand.rSP = 255
self.grippercommand.rFR = 150
self.gripperpub.publish(self.grippercommand)
time.sleep(.5)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.grippercommand.rACT = 0
self.gripperpub.publish(self.grippercommand)
time.sleep(.5)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.grippercommand.rACT = 1
self.grippercommand.rGTO = 1
self.grippercommand.rSP = 255
self.grippercommand.rFR = 150
self.pb1 = 1
self.pb2 = 1
self.pb3 = 1
self.pb4 = 1
time.sleep(.5)
self.nav = intera_interface.Navigator()
self.head = intera_interface.Head()
self.headangle = 0
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# get information from the SDK
# self._left = intera_interface.Limb('left')
self._right = intera_interface.Limb('right')
#self._l_jnames = self._left.joint_names()
self._r_jnames = self._right.joint_names()
self.kin = sawyer_kinematics('right')
# data initializations
self._jointpos = [0] * 7
self._jointvel = [0] * 7
self._jointtor = [0] * 7
self._ee_pos = [0] * 3
self._ee_or = [0] * 4
self._ee_tw = [0] * 6
self._ee_wr = [0] * 6
self._ee_vel = [0] * 6
self._pijac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._jac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._inertia_mat = [
] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0')
self.Jxl_raw = 0
self.Jyl_raw = 0
self.Jzl_raw = 0
self.Jxa_raw = 0
self.Jya_raw = 0
self.Jza_raw = 0
self.leftbutton = 0
self.rightbutton = 0
self.spacenav = []
#self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7))
self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7))
self.MODE_POSITION = 0
self.MODE_VELOCITY = 1
self.MODE_TORQUE = 2
self._mode = self.MODE_POSITION
self.RMH_delay = .01
# initial joint command is current pose
self.readJointPositions()
#self.setJointCommand('left',self._jointpos[0:7])
self.setJointCommand('right', self._jointpos[0:7])
self.hascollided = 0
self.head_display = intera_interface.HeadDisplay()
print('head display!')
#self.image="/home/rachel/rawhide/rawhide_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg"
self.image = "/home/rachel/rawhide/rmh_code/puppy.png"
self.head_display.display_image(self.image, display_rate=100)
print('finished first head display')
# Start background threads
self._running = True
self._t_joints = threading.Thread(target=self.jointspace_worker)
self._t_joints.daemon = True
self._t_joints.start()
self._t_effector = threading.Thread(target=self.endeffector_worker)
self._t_effector.daemon = True
self._t_effector.start()
self._t_command = threading.Thread(target=self.command_worker)
self._t_command.daemon = True
self._t_command.start()
self._t_display = threading.Thread(target=self.display_worker)
self._t_display.daemon = True
self._t_display.start()
self._t_dio = threading.Thread(target=self.dio_worker)
self._t_dio.daemon = True
self._t_dio.start()
