def __init__(self, moveit=False, tolerance=0.008726646):
baxter_interface.JOINT_ANGLE_TOLERANCE = tolerance
self._left_limb = baxter_interface.Limb('left')
self._right_limb = baxter_interface.Limb('right')
self._left_gripper = baxter_interface.Gripper('left')
self._left_gripper.set_holding_force(100)
self._left_gripper.set_moving_force(100)
self._right_gripper = baxter_interface.Gripper('right')
self._right_gripper.set_holding_force(100)
self._right_gripper.set_moving_force(100)
left_ns = "ExternalTools/" + 'left' + "/PositionKinematicsNode/IKService"
self.left_iksvc = rospy.ServiceProxy(left_ns, SolvePositionIK)
rospy.wait_for_service(left_ns, 5.0)
right_ns = "ExternalTools/" + 'right' + "/PositionKinematicsNode/IKService"
self.right_iksvc = rospy.ServiceProxy(right_ns, SolvePositionIK)
rospy.wait_for_service(right_ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.server_pid_path = None
if moveit:
moveit_commander.roscpp_initialize(sys.argv)
self.robot_commander = moveit_commander.RobotCommander()
self.left_commander = moveit_commander.MoveGroupCommander(
'left_arm')
self.right_commander = moveit_commander.MoveGroupCommander(
'right_arm')
self.collision_proxy = rospy.ServiceProxy('/check_state_validity',
GetStateValidity)
self.collision_proxy.wait_for_service()
def __init__(self, limb, hover_distance=0.15, verbose=True):
self._limb_name = limb # string
self._hover_distance = hover_distance # in meters
self._verbose = verbose # bool
self._limb = baxter_interface.Limb(limb)
self._gripper = baxter_interface.Gripper(limb)
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 2.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def __init__(self, limb, verbose=True):
# Subscribers
# Publishers
# Initialize parameters
self._limb_name = limb # string
self._verbose = verbose # bool
self._limb = baxter_interface.Limb(limb)
self._gripper = baxter_interface.Gripper(limb)
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
left = baxter_interface.Gripper('left', baxter_interface.CHECK_VERSION)
print("Enabling robot... ")
self._rs.enable()
print("Calibrating gripper...")
left.calibrate()
# Node cycle rate (in Hz).
self.loop_rate = rospy.Rate(15)
def target_pose_listener():
rospy.init_node('target_pose_listener', anonymous=True)
#Subscribe to Baxter's left endpoint state and Visp autotracker messages
rospy.Subscriber("/robot/limb/left/endpoint_state", EndpointState,
getPoseEE)
rospy.Subscriber("/visp_auto_tracker/object_position", PoseStamped,
getPoseTag)
rospy.Subscriber("/opencv/center_of_object", Point,
getPositionObjectfromOpenCV)
#Wait for left gripper's end effector pose
while not first_flag:
pass
print "In main loop"
#Move to home position
move_to_home_pose = PoseStamped()
move_to_home_pose.header = Header(stamp=rospy.Time.now(), frame_id='base')
move_to_home_pose.pose.position = Point(
x=0.8,
y=0.3,
z=0.3,
)
move_to_home_pose.pose.orientation = Quaternion(
x=0,
y=1,
z=0,
w=0,
)
BaxterMovement(move_to_home_pose)
#Calibrate left gripper
baxterleft = baxter_interface.Gripper('left')
baxterleft.calibrate()
#Run main part of loop while rospy is not shutdown
while not rospy.is_shutdown():
#If no message is being published from QR code, moves towards object seen from OpenCV node
while tag_msg.pose.position.x == 0:
BaxterMovement(NewPoseUsingOpenCV(position_object_opencv))
#Otherwise, moves toward QR code
BaxterMovement(NewPoseUsingQRcode(tag_msg))
rospy.spin()
def __init__(self,limb="right"):
#create our action server clients
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#verify joint trajectory action servers are available
r_server_up = self._right_client.wait_for_server(rospy.Duration(10.0))
if not r_server_up:
msg = ("Action server not available."
" Verify action server availability.")
rospy.logerr(msg)
rospy.signal_shutdown(msg)
sys.exit(1)
#create our goal request
self._limb_name = limb
self._gripper = baxter_interface.Gripper(limb)
self._r_goal = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._r_arm = baxter_interface.Limb('right')
#gripper interface - for gripper command playback
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
# Verify Grippers Have No Errors and are Calibrated
#gripper goal trajectories
# Timing offset to prevent gripper playback before trajectory has started
self._slow_move_offset = 0.0
self._trajectory_start_offset = rospy.Duration(0.0)
self._trajectory_actual_offset = rospy.Duration(0.0)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#gripper control rate
self._gripper_rate = 20.0 # Hz
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('right_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 #0.04 # Correccion de camara por los gripper,
self.cam_y_offset = -0.025 #-0.015
self.resolution = 1
self.width = 960 # 1280 640 960
self.height = 600 # 800 400 600
# Pose inicial
self.x = posiciones[0][0] #0.4
self.y = posiciones[0][1] #-0.4
self.z = 0.0 # -0.1
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]
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 ChangeGripper(action):
if rospy.get_param('/gripper') == action:
return True
else:
rospy.set_param('/gripper', action)
gripper = bi.Gripper(rospy.get_param('/hand_mode'))
rospy.sleep(1)
if action == "open":
gripper.open()
return True
elif action == "close":
gripper.close()
return True
else:
print("Please enter a valid gripper action.")
return False
def __init__(self):
super(MoveItPyPlanner_left, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.group_names = self.robot.get_group_names()
self.scene = moveit_commander.PlanningSceneInterface()
self.group_name = "left_arm"
self.group = moveit_commander.MoveGroupCommander(self.group_name)
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.left_gripper = baxter_interface.Gripper('left')
self.box_name = ''
self.eef_link = self.group.get_end_effector_link()
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, limb, hover_distance=0.15, verbose=True):
self._limb_name = limb # string
self._hover_distance = hover_distance # in meters
self._verbose = verbose # bool
self._limb = baxter_interface.Limb(limb)
self._gripper = baxter_interface.Gripper(limb)
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._robot = moveit_commander.RobotCommander()
# This is an interface to one group of joints. In our case, we want to use the "right_arm".
#We will use this to plan and execute motions
self._group = moveit_commander.MoveGroupCommander(limb + "_arm")
self._group.set_max_velocity_scaling_factor(
0.2) # This is to make Baxter move slower
def main():
global right_command_pub
rospy.init_node("rsdk_ik_service_client")
right_command_pub = rospy.Publisher("/robot/limb/right/joint_command", JointCommand, queue_size=10)
limb = baxter_interface.Limb("right")
gripper = baxter_interface.Gripper("right")
#Calibrate your gripper here
gripper.calibrate()
point1 = Point( x=0.7,
y=-0.15,
z=-0.129)
point2 = Point( x=0.7,
y=-0.15,
z=0.1)
point3 = Point( x=0.7,
y=-0.5,
z=0.1)
point4 = Point( x=0.7,
y=-0.5,
z=-0.129)
gripper.open()
rospy.sleep(.2)
while(not rospy.is_shutdown()):
move_joint(limb, point1)
rospy.sleep(.2)
gripper.close()
rospy.sleep(.1)
move_joint(limb, point2)
rospy.sleep(.2)
move_joint(limb, point3)
rospy.sleep(.2)
move_joint(limb, point4)
rospy.sleep(.2)
gripper.open()
rospy.sleep(.1)
move_joint(limb, point3)
rospy.sleep(.2)
move_joint(limb, point2)
rospy.sleep(.2)
def __init__(self, limb, side):
# TODO: Replace limb motion with moveit motions
self.side = side
# TODO: JB Added
# self._limb = baxter_interface.Limb(limb)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self._limb = moveit_commander.MoveGroupCommander(limb)
self._limb.set_max_velocity_scaling_factor(0.4)
circle_io = baxter_interface.DigitalIO(side + '_lower_button')
dash_io = baxter_interface.DigitalIO(side + '_upper_button')
self.calibrating = False
self.object_calib = -1
self.objects = [
"teddy_bear", "orange", "book", "clock", "bottle", "scissors",
"cup", "bowl"
]
# self.objects = ['neutral', 'Cup', 'Tea', 'Sugar', 'Left_Bread', 'Right_Bread', 'Lettuce', 'Meat']
# self.objects = ['neutral', 'placemat', 'cup', 'plate', 'fork', 'spoon', 'knife', 'bowl', 'soda', 'wineglass']
# self.objects = ['neutral', 'cup', 'plate', 'bowl']
# self.objects = ['neutral', 'bowl']
# TODO: JB Added
# self.object_pick_joint_angles = dict()
self.object_pick_poses = dict()
self.object_approach_poses = dict()
# TODO: JB Added
# self.object_place_joint_angles = dict()
self.object_place_poses = dict()
self._gripper = baxter_interface.Gripper(side)
self._gripper.calibrate()
self._gripper.set_holding_force(100.0)
ik_srv = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK)
self._ikreq = SolvePositionIKRequest()
circle_io.state_changed.connect(self.CirclePressed_)
# Variables to manage threads
self.work_thread = None
self.stop = False
# state variables
self.state = STATE.IDLE
def __init__(self, limb, pick_hover = 0.15, place_hover = 0.25, verbose=True):
self._limb_name = limb # string
self._pick_hover = pick_hover # in meters
self._place_hover = place_hover # in meters
#how far to hover above brick before pick to prevent knock over
self._verbose = verbose # bool
self._limb = baxter_interface.Limb(limb)
self._gripper = baxter_interface.Gripper(limb)
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def __init__(self):
rospy.loginfo("Creating HandControl class")
# Instatiating objects
self.kb = kbhit.KBHit()
rospy.on_shutdown(self.kb.set_normal_term)
self.rh = baxter_interface.Gripper("right")
rospy.sleep(1.0)
self.run_state = ON # Always on
self.gripper_state = OPEN
self.gripper_state = CLOSED
# Subscribers
self.run_flag_sub = rospy.Subscriber('/run_status', Bool, self.run_cb)
self.omni_white_sub = rospy.Subscriber('omni1_button', PhantomButtonEvent, self.omni_white_cb, queue_size=1)
return
def __init__(self, gripper, scale_pos, reconfig_server):
self._scale_pos = scale_pos
self._dyn = reconfig_server
self._ee = gripper + '_gripper'
self._ns = 'robot/end_effector/' + self._ee + '/gripper_action'
self._gripper = baxter_interface.Gripper(gripper, CHECK_VERSION)
# Store Gripper Type
self._type = self._gripper.type()
if self._type == 'custom':
msg = ("Stopping %s action server - %s gripper not capable of "
"gripper actions" % (self._gripper.name, self._type))
rospy.logerr(msg)
return
# Verify Grippers Have No Errors and are Calibrated
if self._gripper.error():
self._gripper.reset()
if self._gripper.error():
msg = ("Stopping %s action server - Unable to clear error" %
self._gripper.name)
rospy.logerr(msg)
return
if not self._gripper.calibrated():
self._gripper.calibrate()
if not self._gripper.calibrated():
msg = ("Stopping %s action server - Unable to calibrate" %
self._gripper.name)
rospy.logerr(msg)
return
# Action Server
self._server = actionlib.SimpleActionServer(
self._ns,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False)
self._action_name = rospy.get_name()
self._server.start()
# Action Feedback/Result
self._fdbk = GripperCommandFeedback()
self._result = GripperCommandResult()
# Initialize Parameters
self._prm = self._gripper.parameters()
self._timeout = 5.0
def __init__(self, limb, speed = 1.5, hover_distance = 0.15, verbose=True): # fix the damned speed
self._limb_name = limb # string
self._hover_distance = hover_distance # in meters above
self._verbose = verbose # bool
self._limb = baxter_interface.Limb(limb) # selects limb
self._limb.set_joint_position_speed(speed) # sets overall speed
self._gripper = baxter_interface.Gripper(limb) # sets gripper left/right
self._gripper.set_moving_force(100) # sets max gripper moving force
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
def __init__(self, limb, name, ID):
# Initialize Node
rospy.init_node('grabber', anonymous=True)
# Subscribe to sensor data
rospy.Subscriber("/block_location", BlockArray, self.model_callback)
# Contains a list of block locations for blocks visible by Baxter
self._name = name
self._ID = ID
self._block_locations = []
self._check_for_blocks = False
# Select limb (left or right)
self._limb = baxter_interface.Limb(limb)
# Set movement speed
self._limb.set_joint_position_speed(0.05)
# Initialize Gripper
self._gripper = baxter_interface.Gripper(limb)
# Initialize inverse kinematics service
self._ik = rospy.ServiceProxy('/Baxter_IK_left/', BaxterIK)
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# An orientation for gripper fingers to be overhead and parallel to an object - use these values for orientation when you query the IK service
self._overhead_orientation = Quaternion(
x=-0.0249590815779,
y=0.999649402929,
z=0.00737916180073,
w=0.00486450832011)
# Start position - use these angles for the robot's start position
self._start_position = {'left_w0': 0.6699952259595108,
'left_w1': 1.030009435085784,
'left_w2': -0.4999997247485215,
'left_e0': -1.189968899785275,
'left_e1': 1.9400238130755056,
'left_s0': -0.08000397926829805,
'left_s1': -0.9999781166910306}
# Position over the trash can - use these angles for the robot's position over the trash can
self._trashcan_position = {'left_w0': 0.3699952259595108,
'left_w1': 1.030009435085784,
'left_w2': -0.999997247485215,
'left_e0': -1.189968899785275,
'left_e1': 1.9400238130755056,
'left_s0': 1.78000397926829805,
'left_s1': -0.9999781166910306}
def __init__(self, limb='right', hover_distance = 0.15, verbose=True, sequence=False):
self._limb_name = limb
self._hover_distance = hover_distance
self._verbose = verbose # print debug messages?
self._sequence = sequence # will run full demo once enabled
self._limb = baxter_interface.Limb(limb)
self._gripper = baxter_interface.Gripper(limb)
self._gripper.set_moving_force(90) # use 90% of gripper force when moving
self._gripper.set_holding_force(90) # use 90% of gripper force when holding
self._iteration = 0 # which brick is being manipulated: begin at zero
self._start_angles = { 'right_s0': 0.5448384680435581,
'right_s1': -1.3477369935496055,
'right_w0': 0.05326913893685781,
'right_w1': 1.5810761462006662,
'right_w2': -0.5013009260859533,
'right_e0': -0.2556930704776139,
'right_e1': 1.3379418806340722 }
# Hardcoded pass angles for horizontal brick placement
self._h_pass_angles = { 'right_s0': -0.46232466308834486,
'right_s1': -0.17581907048286283,
'right_w0': 0.05859510625342512,
'right_w1': 0.7853226620254743,
'right_w2': 1.8815493071424605,
'right_e0': 1.884863261937074,
'right_e1': 2.0108387683180178 }
# Hardcoded pass angles for vertical brick placement
self._v_pass_angles = { 'right_s0': 0.17441293379622724,
'right_s1': -0.9520368012619453,
'right_w0': -0.623071260093031,
'right_w1': 1.825515060165495,
'right_w2': 2.6252551125456414,
'right_e0': 1.7985084287013606,
'right_e1': 1.421872443080793 }
self._hover_angles = None
self._cpose = Pose() # calibration pose: empty until calibration function run
self._cpose_angles = {}
ns = 'ExternalTools/' + limb + '/PositionKinematicsNode/IKService'
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
# verify robot is enabled
print('Getting robot state... ')
self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print('Enabling robot...')
self._rs.enable()
def __init__(self, tab):
self.MAX_PLY = 32
self.pv_length = [0 for x in range(self.MAX_PLY)]
self.INFINITY = 32000
self.init()
self.tab = tab
self.gripper = baxter_interface.Gripper("right")
self.gripper.calibrate()
self.gripper.open()
self.limb = baxter_interface.Limb('right')
self.limb2 = baxter_interface.Limb('left')
self.piecePrise = Piece()
self.castle_pos1 = Piece()
self.castle_pos2 = Piece()
self.en_passant = False
self.boolc = False
def toggle_grip(self):
gripper = baxter_interface.Gripper(self.limb_side)
if self.grip_state == 0:
print("Opening gripper")
self.grip_state = 100
gripper.command_position(self.grip_state)
elif self.grip_state == 100:
print("Closing gripper")
self.grip_state = 0
gripper.command_position(self.grip_state)
time.sleep(0.5)
position = gripper.position()
print(' ' + str(position))
if position > 5:
print(" Picked up object")
else:
print(" Didn't pick up object")
def __init__(self):
self._limb = baxter_interface.limb.Limb("right")
self._angles = self._limb.joint_angles()
# open left gripper
#self.leftGripper =baxter_interface.Gripper('left')
#self.leftGripper.calibrate()
#self.leftGripper.open()
# open Right gripper
self.rightGripper = baxter_interface.Gripper('right')
self.rightGripper.calibrate()
self.rightGripper.open()
# set control params
print("Getting robot state...")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot..")
self._rs.enable()
def __init__(self, sim=False):
"""Hardware abstraction of the Baxter robot using the BaxterSDK
interface.
:param sim: Whether in Gazebo (True) or on real Baxter (False).
"""
name = 'main.baxter'
self._logger = logging.getLogger(name)
self._arms = ['left', 'right']
self._limbs = {a: baxter_interface.Limb(a)
for a in self._arms}
self._grippers = {a: baxter_interface.Gripper(a)
for a in self._arms}
self._grippers_pars = self._grippers['left'].valid_parameters()
self._grippers_pars['moving_force'] = 40.0
self._grippers_pars['holding_force'] = 30.0
self._sensors = {a: baxter_interface.analog_io.AnalogIO('%s_hand_range' % a)
for a in self._arms}
# Cameras on the Baxter robot are tricky. Due to limited bandwidth
# only two cameras can be operating at a time.
# http://sdk.rethinkrobotics.com/wiki/Camera_Control_Tool
# Default behavior on Baxter startup is for both of the hand cameras
# to be in operation at a resolution of 320x200 at a frame rate of
# 25 fps. We get their CameraControllers using the Baxter SDK ...
self.cameras_d = {a: baxter_interface.CameraController('%s_hand_camera' % a, sim=sim)
for a in self._arms}
# ... and set their resolution to 1280x800 @ 14 fps.
for arm in self._arms:
self.cameras_d[arm].resolution = (1280, 800)
self.cameras_d[arm].fps = 14.0
self.cameras_d[arm].exposure = settings.baxter_cam_exposure
# We don't need the CameraControllers any more. Our own module will
# do the remaining camera handling for us.
self.cameras = {a: Camera(topic='/cameras/{}_hand_camera/image'.format(a),
prefix=name)
for a in self._arms}
self._planner = SimplePlanner()
self._rs = None
self._init_state = None
self.cam_offset = None
self.range_offset = None
self.z_table = None
def __init__(self, verbose=False):
if not ProxyTrajectoryClient._is_initialized:
ProxyTrajectoryClient._is_initialized = True
Logger.loginfo(
"Initializing proxy FollowJointTrajectory client...")
ProxyTrajectoryClient._client = ProxyActionClient({
ProxyTrajectoryClient._action_topic:
FollowJointTrajectoryAction
})
ProxyTrajectoryClient._verbose = verbose
# enable the IK Service
ik_ns = "ExternalTools/" + ProxyTrajectoryClient._limb + "/PositionKinematicsNode/IKService"
ProxyTrajectoryClient._iksvc = rospy.ServiceProxy(
ik_ns, SolvePositionIK)
rospy.wait_for_service(ik_ns, 5.0)
# enable the Baxter
ProxyTrajectoryClient._rs = baxter_interface.RobotEnable(
baxter_interface.CHECK_VERSION)
ProxyTrajectoryClient._init_state = ProxyTrajectoryClient._rs.state(
).enabled
print("Enabling robot... ")
ProxyTrajectoryClient._rs.enable()
# enable baxter limb interface
ProxyTrajectoryClient._baxter_limb_interface = baxter_interface.limb.Limb(
ProxyTrajectoryClient._limb)
# Get the names from joints
ProxyTrajectoryClient._joint_name = [
joint for joint in
ProxyTrajectoryClient._baxter_limb_interface.joint_names()
]
ProxyTrajectoryClient._gripper = baxter_interface.Gripper(
ProxyTrajectoryClient._limb)
self._goal = FollowJointTrajectoryGoal()
self._result = None
self._goal_time_tolerance = rospy.Time(0.1)
self._goal.goal_time_tolerance = self._goal_time_tolerance
def __init__(self, arm, distance):
global image_directory
baxter_interface.RobotEnable().enable()
# arm ("left" or "right")
self.limb = arm
self.distance = distance
self.limb_interface = baxter_interface.Limb(self.limb)
self.gripper = baxter_interface.Gripper(arm)
self.gripper.calibrate()
self.other_limb_interface = baxter_interface.Limb("left")
self.baxter_ik_move("left", (0.25, 0.50, 0.2, math.pi, 0.0, 0.0))
self.limb_interface.set_joint_position_speed(0.1)
self.other_limb_interface.set_joint_position_speed(0.1)
#self.open_camera(arm)
self.bridge = CvBridge()
# image directory
self.image_dir = image_directory
# flag to control saving of analysis images
self.save_images = True
# required position accuracy in metres
# self.ball_tolerance = 0.005
# self.tray_tolerance = 0.05
# number of balls found
# self.balls_found = 0
# start positions
self.origin_x = 0.6 # x = front back 0.50 0.70(good)
self.origin_y = -0.1 # y = left right 0.30 -0.1(good)
self.origin_z = 0 # z = up down 0.15
self.roll = -1.0 * math.pi # roll = horizontal
self.pitch = 0.0 * math.pi # pitch = vertical
self.yaw = 0.0 * math.pi # yaw = rotation
self.pose = [self.origin_x, self.origin_y, self.origin_z, \
self.roll, self.pitch, self.yaw]
def __init__(self, arm):
self.limb = arm
self.limb_interface = baxter_interface.Limb(arm)
self.gripper = baxter_interface.Gripper(arm)
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.tolerance = 0.005
self.pos_x = 0.50
self.pos_y = 0.00
self.pos_z = 0.05
self.or_x = 0.0
self.or_y = -1.0
self.or_z = 0.0
self.or_w = 0.0
self.pose = [self.pos_x, self.pos_y, self.pos_z, self.or_x, self.or_y, self.or_z, self.or_w]
self.hough_accumulator = 30
self.hough_min_radius = 35
self.hough_max_radius = 70
self.cam_calib = 0.0004
self.cam_x_offset = 0.045
self.cam_y_offset = -0.01
self.width = 960
self.height = 600
self.cv_image = cv.CreateImage((self.width, self.height), 8, 3)
baxter_interface.RobotEnable().enable()
self.limb_interface.set_joint_position_speed(0.5)
self.gripper.calibrate()
self.reset_camera(self.limb, self.width, self.height)
self.subscribe_to_camera(self.limb)
self.distance = 0
def main():
# arg_fmt = argparse.RawDescriptionHelpFormatter
# parser = argparse.ArgumentParser(formatter_class=arg_fmt,
# description=main.__doc__)
# parser.add_argument(
# '-l', '--limb', choices=['left', 'right'], required=True,
# help="the limb to test"
# )
# args = parser.parse_args(rospy.myargv()[1:])
print("Getting robot state... ")
rospy.init_node("IKSolver")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
limb = baxter_interface.Limb("right")
right = baxter_interface.Gripper('right', CHECK_VERSION)
# return ik_test(args.limb)
goToOrigin(limb)
current_pose = limb.endpoint_pose()
print current_pose
print limb.joint_angles()
running = True
while (running):
x = raw_input("X Value?\n")
y = raw_input("Y Value?\n")
z = raw_input("Z Value?\n")
joints = solveIK('right', float(x), float(y), float(z),
current_pose['orientation'].x,
current_pose['orientation'].y,
current_pose['orientation'].z,
current_pose['orientation'].w)
if (x == " " or y == " " or z == " "):
running = False
else:
if (joints):
print "moving"
limb.move_to_joint_positions(joints)
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 main():
# arg_fmt = argparse.RawDescriptionHelpFormatter
# parser = argparse.ArgumentParser(formatter_class=arg_fmt,
# description=main.__doc__)
# parser.add_argument(
# '-l', '--limb', choices=['left', 'right'], required=True,
# help="the limb to test"
# )
# args = parser.parse_args(rospy.myargv()[1:])
print("Getting robot state... ")
rospy.init_node("IKSolver")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
limb = baxter_interface.Limb("right")
right = baxter_interface.Gripper('right', CHECK_VERSION)
# return ik_test(args.limb)
goToOrigin(limb)
current_pose = limb.endpoint_pose()
print current_pose
print limb.joint_angles()
x = input("X Value?\n")
y = input("Y Value?\n")
z = input("Z Value?\n")
# q1 = input("Q1 Value?\n")
# q2 = input("Q2 Value?\n")
# q3 = input("Q3 Value?\n")
# q4 = input("Q4 Value?\n")
# limb.move_to_neutral()
# joints = solveIK('right', float(x), float(y), float(z), float(q1), float(q2), float(q3), float(q4));
joints = solveIK('right', x, y, z, current_pose['orientation'].x,
current_pose['orientation'].y,
current_pose['orientation'].z,
current_pose['orientation'].w)
if (joints):
limb.move_to_joint_positions(joints)
def __init__(self):
left = baxter_interface.Gripper('left')
left.reboot()
self.left_arm = baxter_interface.Limb('left')
self.pub_rate = rospy.Publisher('/robot/joint_state_publish_rate',
UInt16)
self.pub_rate.publish(500)
"""
The following 6 dictionaries are positions for Baxter's left arm built such that
each will put Baxter's arm over (for overposition dictionaries) one of the three
bowls, or into (for inposition dictionaries) one of the bowls.
"""
self.overposition1 = dict(
zip()) #We need to write all of these dictionaries.
self.overposition2 = dict(zip())
self.overposition3 = dict(zip())
self.inposition1 = dict(zip())
self.inposition2 = dict(zip())
self.inposition3 = dict(zip())
