def __init__(self, arm, posiciones):
# Brazo a utilizar
self.limb = arm
self.limb_interface = baxter_interface.Limb(self.limb)
# set speed as a ratio of maximum speed
self.limb_interface.set_joint_position_speed(0.5)
#self.other_limb_interface.set_joint_position_speed(0.5)
# Gripper ("left" or "right")
self.gripper = baxter_interface.Gripper(self.limb)
# calibrate the gripper
self.gripper.calibrate()
# Laser
self.laser = baxter_interface.AnalogIO('left_hand_range')
self.dist = self.laser.state() / 1000 - 0.105
# Parametros de camara
self.cam_calibracion = 0.0025 # 0.0025 pixeles por metro a 1 metro de distancia. Factor de correccion
self.cam_x_offset = 0 # Correccion de camara por los gripper,
self.cam_y_offset = -0.025
self.resolution = 1
self.width = 1280 # 1280 640 960
self.height = 800 # 800 400 600
# Pose inicial
self.x = posiciones[0][0] #0.4 # La tengo que setear
self.y = posiciones[0][1] #0.4 # La tengo que setear
self.z = 0.0
self.roll = math.pi
self.pitch = 0.0
self.yaw = 0.0
self.pose = [self.x, self.y, self.z, self.roll, self.pitch, self.yaw]
self.mover_baxter('base', self.pose[:3], self.pose[3:6])
# Guardar cuadrilatero de busqueda
self.x_pos = self.x + abs(self.x - posiciones[4][0])
self.x_neg = self.x - abs(self.x - posiciones[3][0])
self.y_pos = self.y + abs(self.y - posiciones[1][1])
self.y_neg = self.y - abs(self.y - posiciones[2][1])
# Posiciones contenedores
self.Contenedor_1 = posiciones[5]
self.Contenedor_2 = posiciones[6]
self.Contenedor_3 = posiciones[7]
self.Contenedor_4 = posiciones[8]
self.Contenedor_5 = posiciones[9]
def __init__(self):
self.IK_srv = baxter_ik_srv()
self.left_arm = baxter_interface.Limb('left')
self.left_arm.set_joint_position_speed(0.2)
self.left_gripper = baxter_interface.Gripper('left')
if self.left_gripper.calibrated() == False:
self.left_gripper.calibrate()
self.left_gripper.open(block=True)
self.left_gripper.set_velocity(5)
self.left_gripper.set_moving_force(10)
self.left_gripper.set_holding_force(5)
self.left_range = baxter_interface.AnalogIO('left_hand_range')
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()
import rospkg
import geometry_msgs.msg
from geometry_msgs.msg import (
PoseStamped,
Pose,
Point,
Quaternion,
)
from jointcontroller_host import Baxter_impl
rospy.init_node('test_gripper', anonymous=True)
leftgripper = baxter_interface.Gripper('left')
leftgripper.stop()
rospy.sleep(1)
l_vacuum_sensor = baxter_interface.AnalogIO('left_vacuum_sensor_analog')
i = 0
while True:
break
leftgripper.command_suction(timeout=100)
# leftgripper.set_vacuum_threshold(100)
print "good"
# leftgripper.stop()
print l_vacuum_sensor.state()
# print leftgripper.type()
# print leftgripper.blowing()
# print leftgripper.sucking()
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')
}