def __init__(self):
"""
Init BaxterRobot object
"""
# Init the limbs of the robot
rospy.init_node("inverse_kinematics_task")
self._limb_left = baxter_interface.Limb("left")
self._limb_right = baxter_interface.Limb("right")
# Init grippers of the robot
self._gripper_left = baxter_interface.Gripper(
"left", baxter_interface.CHECK_VERSION)
self._gripper_right = baxter_interface.Gripper(
"right", baxter_interface.CHECK_VERSION)
self._gripper_left.calibrate()
self._gripper_right.calibrate()
self._gripper_left_pos = 100.0
self._gripper_right_pos = 100.0
# Create Navigator I/O
self._navigator_left = baxter_interface.Navigator("left")
self._navigator_right = baxter_interface.Navigator("right")
# State of the robot
self.state = 0
# Init the robot
rospy.loginfo("Getting robot state... ")
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
rospy.loginfo("Enabling robot... ")
self._rs.enable()
def __init__(self, limb, filename, speed, accuracy):
# Create baxter_interface limb instance
self._arm = limb
self.file=filename
self._limb = baxter_interface.Limb(self._arm)
self.result = [False, '']
# Parameters which will describe joint position moves
self._speed = speed
self._accuracy = accuracy
#activate all limbs
self._limb_left = baxter_interface.Limb("left")
self._limb_right = baxter_interface.Limb("right")
self._gripper_left = baxter_interface.Gripper("left", CHECK_VERSION)
self._gripper_right = baxter_interface.Gripper("right", CHECK_VERSION)
self._io_left_lower = baxter_interface.DigitalIO('left_lower_button')
self._io_left_upper = baxter_interface.DigitalIO('left_upper_button')
self._io_right_lower = baxter_interface.DigitalIO('right_lower_button')
self._io_right_upper = baxter_interface.DigitalIO('right_upper_button')
self._navigator_io = baxter_interface.Navigator("right")
# Verify Grippers Have No Errors and are Calibrated
if self._gripper_left.error():
self._gripper_left.reset()
if self._gripper_right.error():
self._gripper_right.reset()
if (not self._gripper_left.calibrated() and
self._gripper_left.type() != 'custom'):
self._gripper_left.calibrate()
if (not self._gripper_right.calibrated() and
self._gripper_right.type() != 'custom'):
self._gripper_right.calibrate()
# Recorded waypoints
self._waypoints = list()
# Recording state
self._is_recording = False
# Verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# Create Navigator I/O
self._navigator_io = baxter_interface.Navigator(self._arm)
def __init__(self, filename, rate):
"""
Records joint data to a file at a specified rate.
"""
self._filename = filename
self._raw_rate = rate
self._rate = rospy.Rate(rate)
self._start_time = rospy.get_time()
self._time_shift = 0
self._done = False
self._pause = False
#self._navigator_io = baxter_interface.Navigator("right")
self._limb_left = baxter_interface.Limb("left")
self._limb_right = baxter_interface.Limb("right")
self._gripper_left = baxter_interface.Gripper("left", CHECK_VERSION)
self._gripper_right = baxter_interface.Gripper("right", CHECK_VERSION)
self._io_left_lower = baxter_interface.DigitalIO('left_lower_button')
self._io_left_upper = baxter_interface.DigitalIO('left_upper_button')
self._io_right_lower = baxter_interface.DigitalIO('right_lower_button')
self._io_right_upper = baxter_interface.DigitalIO('right_upper_button')
self._navigator_io = baxter_interface.Navigator("right")
# Verify Grippers Have No Errors and are Calibrated
if self._gripper_left.error():
self._gripper_left.reset()
if self._gripper_right.error():
self._gripper_right.reset()
if (not self._gripper_left.calibrated()
and self._gripper_left.type() != 'custom'):
self._gripper_left.calibrate()
if (not self._gripper_right.calibrated()
and self._gripper_right.type() != 'custom'):
self._gripper_right.calibrate()
def __init__(self, limb, speed, accuracy):
self.lastRotate = time.time()
# Create baxter_interface limb instance
self._arm = limb
self._limb = baxter_interface.Limb(self._arm)
# Parameters which will describe joint position moves
self._speed = speed
self._accuracy = accuracy
# Recorded waypoints
self._waypoints = list()
self._suctionPoints = list()
self._sucking = False
self._right_gripper = baxter_interface.Gripper('right')
# Recording state
self._is_recording = False
# Verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# Create Navigator I/O
self._navigator_io = baxter_interface.Navigator(self._arm)
self._close_io = baxter_interface.DigitalIO('%s_upper_button' % (self._arm,)) # 'dash' btn
self._open_io = baxter_interface.DigitalIO('%s_lower_button' % (self._arm,)) # 'circle' btn
def setButtonHandlers(arm):
# inputs
if arm == 'left':
_close_io = baxter_interface.DigitalIO('%s_upper_button' % (arm,)) # 'dash' btn
#_close_io.state_changed.connect(toggleLeftGripper)
_open_io = baxter_interface.DigitalIO('%s_lower_button' % (arm,)) # 'circle' btn
#_open_io.state_changed.connect(rotateLeftGripper)
Lnav = (baxter_interface.Navigator('left'))
#Lnav.button0_changed.connect(scrollWheel)
#Lnav.wheel_changed.connect(wheel_moved)
elif arm == 'right':
pass
#Rnav = (baxter_interface.Navigator('right'))
#press 'ok' button to record
#Rnav.button0_changed.connect(pickFlower)
#press 'Rethink' button to record everything
#Rnav.button2_changed.connect(recordNewFlowerPick)
else:
print 'uhm something went wrong here'
def blink():
navs = (
baxter_interface.Navigator('left'),
baxter_interface.Navigator('right'),
baxter_interface.Navigator('torso_left'),
baxter_interface.Navigator('torso_right'),)
print ("Blinking LED's for 10 seconds")
rate = rospy.Rate(10)
i = 0
while not rospy.is_shutdown() and i < 100:
for nav in navs:
nav.inner_led = not nav.inner_led
nav.outer_led = not nav.outer_led
rate.sleep()
i += 1
def echo_input():
def b0_pressed(v):
print("Button 0: %s" % (v, ))
def b1_pressed(v):
print("Button 1: %s" % (v, ))
def b2_pressed(v):
print("Button 2: %s" % (v, ))
def wheel_moved(v):
print("Wheel Increment: %d, New Value: %s" % (v, nav.wheel))
nav = baxter_interface.Navigator('left')
nav.button0_changed.connect(b0_pressed)
nav.button1_changed.connect(b1_pressed)
nav.button2_changed.connect(b2_pressed)
nav.wheel_changed.connect(wheel_moved)
print("Press input buttons on the left 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, arm):
self.arm = arm
self.brazo = baxter_interface.Limb(self.arm)
# Declaracion del boton
self.boton_brazo = baxter_interface.Navigator(self.arm)
# Gripper
self.gripper = baxter_interface.Gripper(self.arm)
# Calibracion
self.gripper.calibrate()
self.pos_caja = [1] * 6
self.pos_caja_vacia = [1] * 6
self.pos_caja_llena = [1] * 6
# Rotacion estandar
self.rot = [math.pi, 0, -math.pi / 2]
# Publicador a pantalla
self.pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True,
queue_size=10)
# Datos de cajas
self.num_cajas = 2
self.altura_cajas = 0.1
self.nivel = 0
self.contador = [False] * 6
self.lugar = 0
def __init__(self, intname) :
self.intname =intname
self.toggled = False
self.blinking = False
self.nav = baxter_interface.Navigator(intname)
self.nav.button0_changed.connect(self.b0_pressed)
self.nav.button1_changed.connect(self.b1_pressed)
self.nav.button2_changed.connect(self.b2_pressed)
self.b=[0,0,0]
def main():
def left_pressed(v):
global pressed
pressed = not pressed
if pressed:
print(left.joint_angles())
print
def left_wheel_moved(v):
print(left.joint_angles())
print
def right_pressed(v):
pressed = not pressed
if pressed:
print(right.joint_angles())
print
def right_wheel_moved(v):
print(right.joint_angles())
print
rospy.init_node("navigator_test")
left = baxter_interface.Limb('left')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
nav_left = baxter_interface.Navigator('left')
nav_right = baxter_interface.Navigator('right')
nav_left.button0_changed.connect(left_pressed)
nav_left.button1_changed.connect(left_pressed)
nav_left.button2_changed.connect(left_pressed)
nav_left.wheel_changed.connect(left_wheel_moved)
nav_right.button0_changed.connect(right_pressed)
nav_right.button1_changed.connect(right_pressed)
nav_right.button2_changed.connect(right_pressed)
nav_right.wheel_changed.connect(right_wheel_moved)
while not rospy.is_shutdown():
rate = rospy.Rate(1)
rate.sleep()
def save_posture(posture_name, button_control=True, arm=None, record_path="posture_records.yaml"):
if button_control:
left_nav = baxter_interface.Navigator('left')
right_nav = baxter_interface.Navigator('right')
while not left_nav.button0 and not right_nav.button0:
rospy.sleep(0.1)
#save the position
left_joint_angles = baxter_interface.Limb('left').joint_angles()
right_joint_angles = baxter_interface.Limb('right').joint_angles()
posture_list = dict()
#resolve path
record_path = alloy.ros.resolve_res_path(record_path,"tbd_baxter_tools")
#create the file at the root of `tbd_baxter_tools\res` if doesn't exist
if record_path is None:
record_path = os.path.join(alloy.ros.create_res_dir("tbd_baxter_tools"),"posture_records.yaml")
#save them to some type of files
if not os.path.exists(record_path):
yaml.dump(posture_list,file(record_path,'w'))
with open(record_path,'rw') as f:
posture_list = yaml.load(f)
if arm == 'right':
posture_list[posture_name] = {
'right': right_joint_angles
}
elif arm == 'left':
posture_list[posture_name] = {
'left': left_joint_angles,
}
else:
posture_list[posture_name] = {
'left': left_joint_angles,
'right': right_joint_angles
}
yaml.dump(posture_list, file(record_path,'w'))
def __init__(self):
"""
Initialize control with Baxter
"""
# verify robot is enabled
rospy.loginfo("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
rospy.loginfo("Enabling robot... ")
self._rs.enable()
rospy.on_shutdown(self.clean_shutdown)
rospy.loginfo("Running...")
# Get the body running
self.head = baxter_interface.Head()
self.right_arm = baxter_interface.Limb('right')
self.left_arm = baxter_interface.Limb('left')
self.left_gripper = baxter_interface.Gripper('left', CHECK_VERSION)
self.right_gripper = baxter_interface.Gripper('right', CHECK_VERSION)
self.left_navigator = baxter_interface.Navigator('left')
self.right_navigator = baxter_interface.Navigator('right')
self.left_torso_navigator = baxter_interface.Navigator('torso_left')
self.right_torso_navigator = baxter_interface.Navigator('torso_right')
def __init__(self):
self.goalPose = {}
#Initialize ROS node
rospy.init_node('Pick_N_Place', anonymous=True)
#Enable Robot
self.en = baxter_interface.RobotEnable()
self.en.enable()
print 'Robot Enabled...'
#Initialize right arm and right gripper
self.rightArm = baxter_interface.Limb('right')
self.rightArm.set_joint_position_speed(0.5)
self.rightArm.set_command_timeout(0.1)
self.rightGripper = baxter_interface.Gripper('right')
#instantiate range sensor
self.rs = BaxterRangeSensor()
#initialize right arm wheel button
self.rightNavigator = baxter_interface.Navigator('right')
#Blink LEDs
self.rightNavigator.inner_led = False
self.rightNavigator.outer_led = False
for i in range(0, 6):
self.rightNavigator.inner_led = not self.rightNavigator.inner_led
self.rightNavigator.outer_led = not self.rightNavigator.outer_led
rospy.sleep(0.5)
#initialize baxter KDL
self.KDL = baxter_kinematics('right')
print ''
#move arm to neutral position and calibrate gripper
self.rightArm.move_to_joint_positions(neutralPose)
self.rightGripper.calibrate()
#Create z-height PID control
self.zReg = PID.PID(Kp=0.2, Kd=0.1, Ki=0, scaleFactor=2)
#create x PID control
self.xReg = PID.PID(Kp=0.1, Kd=0.01, Ki=0, scaleFactor=0.005)
#create y PID control
self.yReg = PID.PID(Kp=0.1, Kd=0.01, Ki=0, scaleFactor=0.005)
#subscribe to 'center' message (sent from CV module) and get center of object
self.centerFromCv = rospy.Subscriber('/center', Point,
self._get_center)
self.centerX = 0
self.centerY = 0
def main_baxter(limb='right'):
import baxter_interface
from visual_mpc.envs.robot_envs.baxter.baxter_impedance import BaxterImpedanceController
controller = BaxterImpedanceController('baxter', True, gripper_attached='none', limb=limb)
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 = baxter_interface.Navigator(limb)
navigator.button0_changed.connect(print_eep)
rospy.spin()
def main():
rospy.init_node('light_show')
navs = (
baxter_interface.Navigator('left'),
baxter_interface.Navigator('right'),
baxter_interface.Navigator('torso_left'),
baxter_interface.Navigator('torso_right'),
)
rate = rospy.Rate(10)
i = 0
for nav in navs:
nav.inner_led = True
nav.outer_led = False
while not rospy.is_shutdown() and i < 10:
for nav in navs:
nav.inner_led = not nav.inner_led
nav.outer_led = not nav.outer_led
rate.sleep()
i += 1
off_lights(navs)
def __init__(self, arm, separacion):
self.arm = arm
# Declaracion del boton
self.boton_brazo = baxter_interface.Navigator(self.arm)
# Gripper
self.gripper = baxter_interface.Gripper(self.arm)
# Calibracion
self.gripper.calibrate()
# Declaracion del brazo
self.brazo = baxter_interface.Limb(self.arm)
self.pos_ini = [1] * 6
self.pos_fin = [1] * 6
# Declaracion de caja
self.caja = bandeja(0.160, 0.250, 3, 3, -0.2)
self.separacion = separacion
self.centros = [x[:] for x in self.caja.centros]
# Rotacion estandar
self.rot = [math.pi, 0, -math.pi / 2]
# Publicador a pantalla
self.pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True,
queue_size=10)
self.contador = [False] * self.caja.espacios
self.lugar = 0
self.fila_max = 12
self.fila = 0
self.mov_caja = movimiento_cajas('right')
self.definir_posicion()
self.mov_caja.definir_posicion()
self.publicador_a_pantalla('Simulador Produccion')
def __init__(self):
self._done = False
self._ur = 0.75
self._sub_ur = rospy.Subscriber('/trust/robot_states', RobotData, self._get_robot)
self._pub_ur = rospy.Publisher('trust/ur_manual', Float64, queue_size=1)
# Use navigator for start and stop of recording
self._nav_right = baxter_interface.Navigator('right')
self._nav_right.button0_changed.connect(self._nav_b0_pressed)
self._nav_right.wheel_changed.connect(self._nav_wheel_changed)
self._cvbridge = CvBridge()
self._image = numpy.zeros((1024, 600, 3), numpy.uint8)
self._pub_img = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
def __init__(self, limb, distance, loops, calibrate):
# Create baxter_interface limb instance
self._arm = limb
self._limb = baxter_interface.Limb(self._arm)
self._gripper = baxter_interface.Gripper(self._arm)
self._cam = baxter_interface.CameraController(self._arm +
'_hand_camera')
self._cam.gain = 1
self._ik_srv = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(self._ik_srv, SolvePositionIK)
self._ikreq = SolvePositionIKRequest()
self._hover_distance = distance
self._num_loops = loops
self._domino_source_approach = None
self._domino_source = None
self._domino_source_jp = None
self._calibration_point = None
# Recorded waypoints
self._waypoints = list()
self._source_point = None
# Recording state
self._is_recording = False
# Verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# Calibrate Gripper
if (calibrate):
self._gripper.calibrate()
if not (self._gripper.calibrated() or self._gripper.calibrate()):
rospy.logwarn("%s (%s) calibration failed.",
self._gripper.name.capitalize(),
self._gripper.type())
# Create Navigator I/O
self._navigator_io = baxter_interface.Navigator(self._arm)
self._overhead_orientation = Quaternion(x=-0.0249590815779,
y=0.999649402929,
z=0.00737916180073,
w=0.00486450832011)
def __init__(self, filename, rate):
"""
Records joint data to a file at a specified rate.
"""
self._filename = filename
self._raw_rate = rate
self._rate = rospy.Rate(rate)
self._start_time = rospy.get_time()
self._done = False
self._limb_left = baxter_interface.Limb("left")
self._limb_right = baxter_interface.Limb("right")
# forward kinematics
self._limb_kin_left = baxter_pykdl.baxter_kinematics('left')
self._limb_kin_right = baxter_pykdl.baxter_kinematics('right')
self._arc_length_left = 0.0
self._arc_length_right = 0.0
self._gripper_left = baxter_interface.Gripper("left", CHECK_VERSION)
self._gripper_right = baxter_interface.Gripper("right", CHECK_VERSION)
self._io_left_lower = baxter_interface.DigitalIO('left_lower_button')
self._io_left_upper = baxter_interface.DigitalIO('left_upper_button')
self._io_right_lower = baxter_interface.DigitalIO('right_lower_button')
self._io_right_upper = baxter_interface.DigitalIO('right_upper_button')
self._io_left_lower.state_changed.connect(self._left_open_action)
self._io_left_upper.state_changed.connect(self._left_close_action)
# Use navigator for start and stop of recording
self._nav_left = baxter_interface.Navigator('left')
self._nav_left.button0_changed.connect(self._nav_b0_pressed)
self._is_started = False
self._pre_pos_left = None
# Verify Grippers Have No Errors and are Calibrated
if self._gripper_left.error():
self._gripper_left.reset()
if self._gripper_right.error():
self._gripper_right.reset()
if (not self._gripper_left.calibrated()
and self._gripper_left.type() != 'custom'):
self._gripper_left.calibrate()
if (not self._gripper_right.calibrated()
and self._gripper_right.type() != 'custom'):
self._gripper_right.calibrate()
def __init__(self, arm, separacion):
self.arm = arm
# Declaracion del boton
self.boton_brazo = baxter_interface.Navigator(self.arm)
# Gripper
self.gripper = baxter_interface.Gripper(self.arm)
# Calibracion
self.gripper.calibrate()
# Declaracion del brazo
self.brazo = baxter_interface.Limb(self.arm)
self.pos_ini = [1] * 6
self.pos_fin = [1] * 6
# Largo de la pieza
self.separacion = 0.0315
# Tablero
self.caja = tablero_gato(0.38, 0.38, 3, 3)
self.centros = [x[:] for x in self.caja.centros]
# Rotacion estandar
self.rot = [math.pi, 0, -math.pi / 2]
# Publicador a pantalla
self.pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True,
queue_size=10)
self.fila_max = 5
self.fila = 0
self.contador = [False] * self.caja.espacios
self.lugar = 0
def __init__(self, arm, speed=0.3, accuracy=baxter_interface.JOINT_ANGLE_TOLERANCE):
self._arm = arm
self._speed = speed
self._accuracy = accuracy
self._limb = baxter_interface.Limb(self._arm)
self._path = list()
self._path_recording = True
self._provide_assist = True
# creating a navigator io object
self._navigator_io = baxter_interface.Navigator(self._arm)
# checking the robot state
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
# Enabling the robot if it not already enabled
if not self._init_state:
self._rs.enable()
def main():
def b0_pressed(v):
print ("Button 0: %s" % (v,))
def b1_pressed(v):
grip_left.open(block=True, timeout=2.0)
def b2_pressed(v):
grip_left.close(block=True, timeout=2.0)
def wheel_moved(v):
print ("Wheel Increment: %d, New Value: %s" % (v, nav.wheel))
angles = left.joint_angles()
if v == 1:
temp = angles['left_w2'] - 0.5
elif v == -1:
temp = angles['left_w2'] + 0.5
pos = {'left_w2': temp}
move_list(arm='left', p_list=[pos], timeout=0.05)
rospy.init_node("navigator_test")
left = baxter_interface.Limb('left')
left.set_joint_position_speed(1.0)
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
nav = baxter_interface.Navigator('left')
nav.button0_changed.connect(b0_pressed)
nav.button1_changed.connect(b1_pressed)
nav.button2_changed.connect(b2_pressed)
nav.wheel_changed.connect(wheel_moved)
while not rospy.is_shutdown():
rate = rospy.Rate(1)
rate.sleep()
def __init__(self, limb, speed, accuracy):
# Create baxter_interface limb instance
self._arm = limb
self._limb = baxter_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 = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# Create Navigator I/O
self._navigator_io = baxter_interface.Navigator(self._arm)
def main():
rospy.init_node("control", anonymous=True)
pub = rospy.Publisher('display_chatter', String,
queue_size=40) #Display chatter publisher defined.
# This variables fot helping that which action will be chosed
indexOfEmotion = 0
isWakeUp = False
isFollow = False
emotion = 0
isEnable = False
'''
Navigators are declared.
-> Navigators on the arms are used for changing the emotions.
Okay buttons of the arm navigators are used for enable or disable robot
-> Navigators on the torses are unsed for the arm following action.
Also they are used for two actions. (Sleep and wake up)
'''
navLeftArm = baxter_interface.Navigator('left')
navRightArm = baxter_interface.Navigator('right')
navLeftTorso = baxter_interface.Navigator('torso_left')
navRightTorso = baxter_interface.Navigator('torso_right')
print "Controller is enable..."
while not rospy.is_shutdown():
# Arm navigators okay button to enable and disable
if navLeftTorso._state.buttons[0] or navRightTorso._state.buttons[0]:
if not isEnable:
pub.publish("enable")
isEnable = True
print "enable"
else:
pub.publish("disable")
isEnable = False
print "disable"
#Left arm up button to wake up
elif navLeftArm._state.buttons[1]:
pub.publish("wake_up")
isWakeUp = True
print "wake_up"
#Left arm down button to sleep
elif navLeftArm._state.buttons[2]:
pub.publish("sleep")
isWakeUp = False
print "sleep"
#Right arm buttons to follow arms
elif navRightArm._state.buttons[1]:
if isFollow:
pub.publish("arm_follow_off")
isFollow = False
print "arm_follow_off"
else:
pub.publish("dynamic_left_arm_follow_on")
isFollow = True
print "dynamic_left_arm_follow_on"
elif navRightArm._state.buttons[2]:
if isFollow:
pub.publish("arm_follow_off")
isFollow = False
print "arm_follow_off"
else:
pub.publish("dynamic_right_arm_follow_on")
isFollow = True
print "dynamic_right_arm_follow_on"
else:
# Wheel Control
currentEmotion = getEmotion(navLeftArm._state.wheel)
if not emotion == currentEmotion and isWakeUp:
pub.publish(commands[currentEmotion])
emotion = currentEmotion
print commands[currentEmotion]
print currentEmotion
continue
print "Wait for 0.3 secs "
time.sleep(0.3)
print "Okay:"
def __init__(self):
print "Initializing Node"
rospy.init_node('baxter_peripherals')
self._running = True
self._valid_limb_names = {
'left': 'left',
'l': 'left',
'right': 'right',
'r': 'right'
}
# gripper initialization
self._grippers = {
'left': baxter_interface.Gripper('left'),
'right': baxter_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 = {
'left': baxter_interface.AnalogIO('left_hand_range'),
'right': baxter_interface.AnalogIO('right_hand_range')
}
# accelerometer initialization
self._accelerometers = {'left': [0.0] * 3, '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 = baxter_interface.Head()
# sonar initialization
self._sonar_pointcloud = RR.RobotRaconteurNode.s.NewStructure(
'BaxterPeripheral_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 = {'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._suppress_collision_avoidance = {'left': False, 'right': False}
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 = {'left': False, 'right': False}
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 = {'left': False, 'right': False}
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 = {'left': False, 'right': False}
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(baxter_interface.Limb('left').joint_names() + \
baxter_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 = {
'left': baxter_interface.Navigator('left'),
'right': baxter_interface.Navigator('right'),
'torso_left': baxter_interface.Navigator('torso_left'),
'torso_right': baxter_interface.Navigator('torso_right')
}
# initialize frame transform
self._listener = tf.TransformListener()
next_move = "PROBLEM"
def callbackHint(v):
global next_move, command, origin, destination
# if button is pressed
if v == True:
name_of_file = "movement" + str(origin) + "-" + str(
destination) + ".txt"
movementLAPub.publish(name_of_file)
feedbackPub.publish(command)
if __name__ == "__main__":
# create node
rospy.init_node('provideHintAlgorithm', anonymous=True)
# create subscriber to the valid move topic
movesSub = rospy.Subscriber('/hanoi/nextMove', Moves, callbackNextMove)
# create subscriber to the left arm button
leftArmNav = baxter_interface.Navigator('left')
# check for left arm button 1 press for YES HINT
leftArmNav.button1_changed.connect(callbackHint)
#prevents program from exiting, allowing subscribers and publishers to keep operating
#in our case that is the camera subscriber and the image processing callback function
rospy.spin()
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('baxter_peripherals')
self._running = True
self._valid_limb_names = {
'left': 'left',
'l': 'left',
'right': 'right',
'r': 'right'
}
# gripper initialization
self._grippers = {
'left': baxter_interface.Gripper('left'),
'right': baxter_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 = {
'left': baxter_interface.AnalogIO('left_hand_range'),
'right': baxter_interface.AnalogIO('right_hand_range')
}
# accelerometer initialization
self._accelerometers = {'left': [0.0] * 3, '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 = baxter_interface.Head()
# suppressions
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._suppress_collision_avoidance = {'left': False, 'right': False}
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 = {'left': False, 'right': False}
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 = {'left': False, 'right': False}
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 = {'left': False, 'right': False}
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(baxter_interface.Limb('left').joint_names() + \
baxter_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)
# hysteresis subscription
self._hyst_lock = threading.Lock()
self._hysteresis_torque = OrderedDict(
zip(baxter_interface.Limb('left').joint_names() + \
baxter_interface.Limb('right').joint_names(),
[0.0]*14))
rospy.Subscriber("/robot/limb/left/gravity_compensation_torques",
SEAJointState, self.hyst_callback)
rospy.Subscriber("/robot/limb/right/gravity_compensation_torques",
SEAJointState, self.hyst_callback)
#commanded position subscription
self._commandPos_lock = threading.Lock()
self._commanded_position = OrderedDict(
zip(baxter_interface.Limb('left').joint_names() + \
baxter_interface.Limb('right').joint_names(),
[0.0]*14))
rospy.Subscriber("/robot/limb/left/gravity_compensation_torques",
SEAJointState, self.commandPos_callback)
rospy.Subscriber("/robot/limb/right/gravity_compensation_torques",
SEAJointState, self.commandPos_callback)
#commanded effort subscription
self._commandTor_lock = threading.Lock()
self._commanded_torque = OrderedDict(
zip(baxter_interface.Limb('left').joint_names() + \
baxter_interface.Limb('right').joint_names(),
[0.0]*14))
rospy.Subscriber("/robot/limb/left/gravity_compensation_torques",
SEAJointState, self.commandTor_callback)
rospy.Subscriber("/robot/limb/right/gravity_compensation_torques",
SEAJointState, self.commandTor_callback)
#all gravity effort subscription
self._allGrav_lock = threading.Lock()
self._all_gravity_torque = OrderedDict(
zip(baxter_interface.Limb('left').joint_names() + \
baxter_interface.Limb('right').joint_names(),
[0.0]*14))
rospy.Subscriber("/robot/limb/left/gravity_compensation_torques",
SEAJointState, self.allGrav_callback)
rospy.Subscriber("/robot/limb/right/gravity_compensation_torques",
SEAJointState, self.allGrav_callback)
#crosstalk effort subscription
self._crossTor_lock = threading.Lock()
self._crosstalk_torque = OrderedDict(
zip(baxter_interface.Limb('left').joint_names() + \
baxter_interface.Limb('right').joint_names(),
[0.0]*14))
rospy.Subscriber("/robot/limb/left/gravity_compensation_torques",
SEAJointState, self.crossTor_callback)
rospy.Subscriber("/robot/limb/right/gravity_compensation_torques",
SEAJointState, self.crossTor_callback)
#hysteresis state subscription
# self._hystState_lock = threading.Lock()
#self._hysteresis_state = OrderedDict(
# zip('hystState',[0.0]))
#rospy.Subscriber("/robot/limb/left/gravity_compensation_torques",
# SEAJointState, self.hystState_callback)
#rospy.Subscriber("/robot/limb/right/gravity_compensation_torques",
# SEAJointState, self.hystState_callback)
# navigators
self._navigators = {
'left': baxter_interface.Navigator('left'),
'right': baxter_interface.Navigator('right'),
'torso_left': baxter_interface.Navigator('torso_left'),
'torso_right': baxter_interface.Navigator('torso_right')
}
"""
import argparse
import sys
import rospy
from std_msgs.msg import String
import baxter_interface
import time
rospy.init_node('secondweek')
#grab the left arm
Llimb = baxter_interface.Limb('left')
#set left limb to 'comfortable' position to get started with
Langles = {'left_w0': 2.0793109556030274, 'left_w1': -1.4522963092712404, 'left_w2': -3.0533887547973633, 'left_e0': -0.17832526638793947, 'left_e1': 2.1414371774414063, 'left_s0': 0.22472818516845705, 'left_s1': -0.683388440222168}
Llimb.move_to_joint_positions(Langles)
Lnav = (baxter_interface.Navigator('right'))
#grab the right arm
Rlimb = baxter_interface.Limb('right')
Rangles = {'right_s0': -0.12923788123168947, 'right_s1': -0.34361169609375003, 'right_w0': -1.1830826813049318, 'right_w1': 1.1347622865417482, 'right_w2': 2.5966459757263185, 'right_e0': 1.1581554935302736, 'right_e1': 1.3974564961669922}
Rlimb.move_to_joint_positions(Rangles)
Rnav = (baxter_interface.Navigator('right'))
#this should import the grippers left and right
right_gripper = baxter_interface.Gripper('right')
left_gripper = baxter_interface.Gripper('left')
#force calibration of left gripper
left_gripper.calibrate()
def __init__(self, arm):
self.boton_brazo = baxter_interface.Navigator(arm)
self.brazo = baxter_interface.Limb(arm)
