def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument( '-l', '--limb', required=True, choices=['left', 'right'], help='send joint trajectory to which limb' ) args = parser.parse_args(rospy.myargv()[1:]) limb = args.limb rospy.init_node("visual_servo") #Bit of cheating: move the robot's arms to a neutral position before running dominant_joints=[1.187301128466797, 1.942403170440674, 0.08206797205810547, -0.996704015789795, -0.6734175651123048, 1.0266166411193849, 0.4985437554931641] off_joints = [-1.1255584018249511, 1.4522963092712404, 0.6354515406555176, -0.8843399232055664, 0.6327670742797852, 1.2751215284729005, -0.4084223843078614, ] names = ['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2'] off_limb = "right" if limb == "right": off_limb = "left" off_names = [off_limb+"_"+name for name in names] jointlists = {} jointlists[off_limb] = off_joints jointlists[limb] = dominant_joints for side in [limb, off_limb]: limb_if = baxter_interface.Limb(side) limb_names = [side+"_"+name for name in names] joint_commands = dict(zip(limb_names, jointlists[side] )) limb_if.move_to_joint_positions(joint_commands) #Calibrate gripper gripper_if = baxter_interface.Gripper(limb) if not gripper_if.calibrated(): print "Calibrating gripper" gripper_if.calibrate() iksvc, ns = ik_command.connect_service(limb) command = VisualCommand(iksvc, limb) command.subscribe() command.publish() while not rospy.is_shutdown(): command.handler_pub.publish(command.done) command.pub_rate.sleep()
def map_keyboard(): left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) #These are from PyKDL and are needed for the Jacobian left_kin = baxter_kinematics('left') right_kin = baxter_kinematics('right') #Connect with IK service right_iksvc, right_ns = ik_command.connect_service('right') left_iksvc, left_ns = ik_command.connect_service('left') def command_jacobian(side, direction): #UNUSED if side == 'left': limb = left kin = left_kin elif side == 'right': limb = right kin = right_kin else: raise Exception("Got wrong side in command_jacobian") # current is the current position of end effector current_p = np.array(limb.endpoint_pose()['position'] + limb.endpoint_pose()['orientation']) current_p = current_q.reshape((7, 1)) current = np.zeros((6, 1)) quaternion_to_euler(current_p, current) # We need to convert the direction from the world frame to the hand frame # Rotate direction by the inverse of the rotation matrix from the angular pose in current R = tf.transformations.quaternion_matrix( current_p[3:7].flatten().tolist()) R_inv = tf.transformations.inverse_matrix(R) print R_inv[0:3, 0:3] direction = np.array(direction).reshape((6, 1)) print direction[0:3] dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3]) print dir_rot #direction[0:3] = dir_rot # Goal is the goal position, found by adding the requested direction from the user goal = current + direction current_angles = np.array( [limb.joint_angles()[name] for name in limb.joint_names()]) # Get the Jacobian inverse and solve for the necessary change in joint angles jacobian_inv = kin.jacobian_transpose() delta = jacobian_inv * (goal - current) commands = current_angles.flatten() + delta.flatten() # Command the joints command_list = commands.tolist() joint_command = dict(zip(limb.joint_names(), command_list[0])) limb.set_joint_positions(joint_command) def command_ik(side, direction): """Use the Rethink IK service to figure out a desired joint position""" if side == 'left': limb = left iksvc = left_iksvc ns = left_ns else: limb = right iksvc = right_iksvc ns = right_ns current_p = np.array(limb.endpoint_pose()['position'] + limb.endpoint_pose()['orientation']) direction = np.array(direction) desired_p = current_p + direction ik_command.service_request(iksvc, desired_p, side, False) zeros = [0] * 4 inc = 0.1 bindings = { 'q': (command_ik, ['right', [inc, 0, 0] + zeros], "increase right x"), 'a': (command_ik, ['right', [-inc, 0, 0] + zeros], "decrease right x"), 'w': (command_ik, ['right', [0, inc, 0] + zeros], "increase right y"), 's': (command_ik, ['right', [0, -inc, 0] + zeros], "decrease right y"), 'e': (command_ik, ['right', [0, 0, inc] + zeros], "increase right z"), 'd': (command_ik, ['right', [0, 0, -inc] + zeros], "decrease right z"), 'u': (command_ik, ['left', [inc, 0, 0] + zeros], "increase left x"), 'j': (command_ik, ['left', [-inc, 0, 0] + zeros], "decrease left x"), 'i': (command_ik, ['left', [0, inc, 0] + zeros], "increase left y"), 'k': (command_ik, ['left', [0, -inc, 0] + zeros], "decrease left y"), 'o': (command_ik, ['left', [0, 0, inc] + zeros], "increase left z"), 'l': (command_ik, ['left', [0, 0, -inc] + zeros], "decrease left z"), 'z': (grip_right.close, [], "right: gripper close"), 'x': (grip_right.open, [], "right: gripper open"), 'c': (grip_right.calibrate, [], "right: gripper calibrate") } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c in bindings: cmd = bindings[c] #expand binding to something like "command_jacobian(left, [0.1, 0, 0])" cmd[0](*cmd[1]) print("command: %s" % (cmd[2], )) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument( '-l', '--limb', required=True, choices=['left', 'right'], help='send joint trajectory to which limb' ) args = parser.parse_args(rospy.myargv()[1:]) limb = args.limb rospy.init_node('stackit') iksvc, ns = ik_command.connect_service(limb) moveit_commander.roscpp_initialize(sys.argv) rate = rospy.Rate(1) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander(limb+"_arm") group.allow_replanning(True) print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") rs.enable() #Calibrate gripper gripper_if = baxter_interface.Gripper(limb) if not gripper_if.calibrated(): print "Calibrating gripper" gripper_if.calibrate() obj_manager = ObjectManager() while len(obj_manager.collision_objects) <= 0: rate.sleep() objects = obj_manager.collision_objects object_height = params['object_height'] """if len(objects) > 1: stack_pose = projectPose(objects[len(objects)-1].primitive_poses[0]) stack_pose = incrementPoseMsgZ(stack_pose, object_height*2.0) objects.pop(len(objects)-1) elif len(objects) == 1: stack_pose = projectPose(objects[0].primitive_poses[0])""" stack_pose = Pose(position=Point(0.593, -0.212, object_height-0.130), orientation=Quaternion(0.6509160466, 0.758886809948, -0.0180992582839, -0.0084573527776) ) processed_win = "Processed image" raw_win = "Hand camera" cv2.namedWindow(processed_win) #cv2.namedWindow(raw_win) rospy.on_shutdown(obj_manager.remove_known_objects) for obj in objects: obj_pose = obj.primitive_poses[0] #group.pick(obj.id) #group.place(obj.id, stack_pose) print "Got pose:", obj.primitive_poses[0] pose = projectPose(obj.primitive_poses[0]) pose = incrementPoseMsgZ(pose, object_height*1.7) print "Modified pose:", pose #if obj.id in obj_manager.id_operations: # obj_manager.id_operations[obj.id] = CollisionObject.REMOVE #obj_manager.publish(obj) print "setting target to pose" # Move to the next block group.clear_pose_targets() group.set_start_state_to_current_state() group.set_pose_target(pose) plan = group.plan() # is there a better way of checking this? plan_found = len(plan.joint_trajectory.points) > 0 if plan_found: #print "============ Waiting while RVIZ displays plan1..." #rospy.sleep(3) group.go(wait=True) imgproc = ObjectFinder("star", None, None) imgproc.subscribe("/cameras/"+limb+"_hand_camera/image") imgproc.publish(limb) vc = VisualCommand(iksvc, limb) vc.subscribe() while (not vc.done) and (not rospy.is_shutdown()): blobArray = [] for centroid, axis in zip(imgproc.centroids, imgproc.axes): blob = BlobInfo() centroid = Point(*centroid) blob.centroid = centroid if axis is None: axis = -1*numpy.ones(6) blob.axis = Polygon([Point(*axis[0:3].tolist()), Point(*axis[3:6].tolist())]) blobArray.append(blob) msg = BlobInfoArray() msg.blobs = blobArray imgproc.handler_pub.publish(msg) if imgproc.processed is not None: cv2.imshow(processed_win, imgproc.processed) cv2.waitKey(10) vc.unsubscribe() imgproc.unsubscribe() print "Adding attached message" #Add attached message #obj.primitive_poses[0] = incrementPoseMsgZ(obj.primitive_poses[0], *object_height) #this is in the base frame...? obj_manager.publish_attached(obj, limb) #touch_links = [limb+"_gripper", limb+"_gripper_base", limb+"_hand_camera", limb+"_hand_range"] #group.attach_object(obj.id, limb+"_gripper", touch_links = touch_links) # Carefully rise away from the object before we plan another path pose = incrementPoseMsgZ(pose, 2*object_height) # test this ik_command.service_request_pose(iksvc, pose, limb, blocking = True) else: print "Unable to plan path" continue # what to do? # Move to the stacking position group.clear_pose_targets() group.set_start_state_to_current_state() group.set_pose_target(stack_pose) plan = group.plan() plan_found = len(plan.joint_trajectory.points) > 0 if plan_found: #print "============ Waiting while RVIZ displays plan2..." #rospy.sleep(3) group.go(wait=True) gripper_if.open(block=True) group.detach_object(obj.id) # Carefully rise away from the object before we plan another path pose = incrementPoseMsgZ(stack_pose, 2*object_height) ik_command.service_request_pose(iksvc, pose, limb, blocking = True) obj.operation = CollisionObject.REMOVE obj_manager.publish(obj) # Get the next stack pose stack_pose = incrementPoseMsgZ(stack_pose, object_height*3/4) """if obj.id in obj_manager.id_operations:
def map_keyboard(): left = baxter_interface.Limb('left') right = baxter_interface.Limb('right') grip_left = baxter_interface.Gripper('left', CHECK_VERSION) grip_right = baxter_interface.Gripper('right', CHECK_VERSION) #These are from PyKDL and are needed for the Jacobian left_kin = baxter_kinematics('left') right_kin = baxter_kinematics('right') #Connect with IK service right_iksvc, right_ns = ik_command.connect_service('right') left_iksvc, left_ns = ik_command.connect_service('left') def command_jacobian(side, direction): #UNUSED if side == 'left': limb = left kin = left_kin elif side == 'right': limb = right kin = right_kin else: raise Exception("Got wrong side in command_jacobian") # current is the current position of end effector current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation']) current_p = current_q.reshape((7, 1)) current = np.zeros((6, 1)) quaternion_to_euler(current_p, current) # We need to convert the direction from the world frame to the hand frame # Rotate direction by the inverse of the rotation matrix from the angular pose in current R = tf.transformations.quaternion_matrix(current_p[3:7].flatten().tolist()) R_inv = tf.transformations.inverse_matrix(R) print R_inv[0:3, 0:3] direction = np.array(direction).reshape((6, 1)) print direction[0:3] dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3]) print dir_rot #direction[0:3] = dir_rot # Goal is the goal position, found by adding the requested direction from the user goal = current + direction current_angles = np.array([limb.joint_angles()[name] for name in limb.joint_names()]) # Get the Jacobian inverse and solve for the necessary change in joint angles jacobian_inv = kin.jacobian_transpose() delta = jacobian_inv*(goal-current) commands = current_angles.flatten() + delta.flatten() # Command the joints command_list = commands.tolist() joint_command = dict(zip(limb.joint_names(), command_list[0])) limb.set_joint_positions(joint_command) def command_ik(side, direction): """Use the Rethink IK service to figure out a desired joint position""" if side == 'left': limb = left iksvc = left_iksvc ns = left_ns else: limb = right iksvc = right_iksvc ns = right_ns current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation']) direction = np.array(direction) desired_p = current_p + direction ik_command.service_request(iksvc, desired_p, side) zeros = [0]*4 inc = 0.1 bindings = { 'q': (command_ik, ['right', [inc, 0, 0]+zeros], "increase right x"), 'a': (command_ik, ['right', [-inc, 0, 0]+zeros], "decrease right x"), 'w': (command_ik, ['right', [0, inc, 0]+zeros], "increase right y"), 's': (command_ik, ['right', [0, -inc, 0]+zeros], "decrease right y"), 'e': (command_ik, ['right', [0, 0, inc]+zeros], "increase right z"), 'd': (command_ik, ['right', [0, 0, -inc]+zeros], "decrease right z"), 'u': (command_ik, ['left', [inc, 0, 0]+zeros], "increase left x"), 'j': (command_ik, ['left', [-inc, 0, 0]+zeros], "decrease left x"), 'i': (command_ik, ['left', [0, inc, 0]+zeros], "increase left y"), 'k': (command_ik, ['left', [0, -inc, 0]+zeros], "decrease left y"), 'o': (command_ik, ['left', [0, 0, inc]+zeros], "increase left z"), 'l': (command_ik, ['left', [0, 0, -inc]+zeros], "decrease left z"), 'z': (grip_right.close, [], "right: gripper close"), 'x': (grip_right.open, [], "right: gripper open"), 'c': (grip_right.calibrate, [], "right: gripper calibrate") } done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c in bindings: cmd = bindings[c] #expand binding to something like "command_jacobian(left, [0.1, 0, 0])" cmd[0](*cmd[1]) print("command: %s" % (cmd[2],)) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument('-l', '--limb', required=True, choices=['left', 'right'], help='send joint trajectory to which limb') args = parser.parse_args(rospy.myargv()[1:]) limb = args.limb rospy.init_node("visual_servo") #Bit of cheating: move the robot's arms to a neutral position before running dominant_joints = [ 1.187301128466797, 1.942403170440674, 0.08206797205810547, -0.996704015789795, -0.6734175651123048, 1.0266166411193849, 0.4985437554931641 ] off_joints = [ -1.1255584018249511, 1.4522963092712404, 0.6354515406555176, -0.8843399232055664, 0.6327670742797852, 1.2751215284729005, -0.4084223843078614, ] names = ['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2'] off_limb = "right" if limb == "right": off_limb = "left" off_names = [off_limb + "_" + name for name in names] jointlists = {} jointlists[off_limb] = off_joints jointlists[limb] = dominant_joints for side in [limb, off_limb]: limb_if = baxter_interface.Limb(side) limb_names = [side + "_" + name for name in names] joint_commands = dict(zip(limb_names, jointlists[side])) limb_if.move_to_joint_positions(joint_commands) #Calibrate gripper gripper_if = baxter_interface.Gripper(limb) if not gripper_if.calibrated(): print "Calibrating gripper" gripper_if.calibrate() iksvc, ns = ik_command.connect_service(limb) command = VisualCommand(iksvc, limb) command.subscribe() command.publish() while not rospy.is_shutdown(): command.handler_pub.publish(command.done) command.pub_rate.sleep()
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument( '-l', '--limb', required=True, choices=['left', 'right'], help='send joint trajectory to which limb' ) args = parser.parse_args(rospy.myargv()[1:]) print args limb = args.limb print("Initializing node... ") rospy.init_node("super_stacker_%s" % (limb,)) print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") rs.enable() #Calibrate gripper gripper_if = baxter_interface.Gripper(limb) if not gripper_if.calibrated(): print "Calibrating gripper" gripper_if.calibrate() limbInterface = baxter_interface.Limb(limb) iksvc, ns = ik_command.connect_service(limb) rate = rospy.Rate(100) # Get goal poses of each object in the scene dc = DepthCaller(limb) # Subscribe to estimate_depth # Move to pose published by estimate_depth while (not dc.done) and (not rospy.is_shutdown()): rate.sleep() #pass # Subscribe to object_finder and start visual servoing/grasping # The stack pose is the pose of the smallest object with a z-offset # accounting for the height of the object (this assumption makes # it really important that the segmentation is accurate) stack_pose = [0.594676466827, -0.296644499519, -0.0322744943164, 0.971805911045, -0.22637170004, 0.065946440385, 0.000437813100735] #stack_pose = dc.object_poses[-1] #stack_pose = incrementPoseZ(stack_pose, object_height) #dc.object_poses.pop(len(dc.object_poses)-1) for pose in dc.object_poses: ik_command.service_request(iksvc, pose, limb, blocking=True) vc = VisualCommand(iksvc, limb) vc.subscribe() while (not vc.done) and (not rospy.is_shutdown()): rate.sleep() ik_command.service_request(iksvc, stack_pose, limb, blocking=True) #limbInterface.move_to_joint_positions(stack_pose) # Let go gripper_if.open(block=True) stack_pose = incrementPoseZ(pose, object_height)
def map_keyboard(): global desired_angles, inc, angle_inc left = baxter_interface.Limb('left') grip_left = baxter_interface.Gripper('left', CHECK_VERSION)qqqqqqqqqqqa #These are from PyKDL and are needed for the Jacobian left_kin = baxter_kinematics('left') #Connect with IK service left_iksvc, left_ns = ik_command.connect_service('left') #Initialize desired angles from current angles init_orient = left.endpoint_pose()['orientation'] quat = [0,0,0,0] quat[3] = init_orient.w quat[0] = init_orient.x quat[1] = init_orient.y quat[2] = init_orient.z desired_angles = tf.transformations.euler_from_quaternion(quat) #print init_orient def command_jacobian(side, direction): #UNUSED if side == 'left': limb = left kin = left_kin else: raise Exception("Got wrong side in command_jacobian") # current is the current position of end effector current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation']) current_p = current_q.reshape((7, 1)) current = np.zeros((6, 1)) quaternion_to_euler(current_p, current) # We need to convert the direction from the world frame to the hand frame # Rotate direction by the inverse of the rotation matrix from the angular pose in current R = tf.transformations.quaternion_matrix(current_p[3:7].flatten().tolist()) R_inv = tf.transformations.inverse_matrix(R) print R_inv[0:3, 0:3] direction = np.array(direction).reshape((6, 1)) print direction[0:3] dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3]) print dir_rot #direction[0:3] = dir_rot # Goal is the goal position, found by adding the requested direction from the user goal = current + direction current_angles = np.array([limb.joint_angles()[name] for name in limb.joint_names()]) # Get the Jacobian inverse and solve for the necessary change in joint angles jacobian_inv = kin.jacobian_transpose() delta = jacobian_inv*(goal-current) commands = current_angles.flatten() + delta.flatten() # Command the joints command_list = commands.tolist() joint_command = dict(zip(limb.joint_names(), command_list[0])) limb.set_joint_positions(joint_command) def command_ik(side, direction): """Use the Rethink IK service to figure out a desired joint position""" if side == 'left': limb = left iksvc = left_iksvc ns = left_ns current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation']) direction = np.array(direction) print direction desired_p = current_p + direction ik_command.service_request(iksvc, desired_p, side) zeros = [0]*4 def update_angles(change_angles): global desired_angles current_angles = np.array(desired_angles) change_angles = np.array(change_angles) #Note that adding np arrays does element-wise summation, but adding python lists concatenates them new_angles = current_angles + change_angles #Check that the angles are in bounds, angles are in radians #for value in new_angles: #Wrap crossing zero towards 2*pi # if value < 0: # value = (2* np.pi) + value #Wrap crossing 2*pi towards zero # if value > (2 * np.pi): # value = value - (2*np.pi) #Convert to a python array and store in the desired angles desired_angles = new_angles.tolist() #Convert the desired angles to a quaternion quat = tf.transformations.quaternion_from_euler(desired_angles[0], desired_angles[1], desired_angles[2]) #This only does the left side, because the scooter only has a left arm limb = left current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation']) current_p[3:7] = quat ik_command.service_request(left_iksvc, current_p, "left") def update_inc(isSmall = True): global inc, angle_inc if isSmall: inc = 0.1 angle_inc = 0.1 else: inc = 0.5 angle_inc = 0.2 def toggle_gripper(): if grip_left.position() > 95: #gripper is open, so close it. Can be confused by a big object, though. print "Closing gripper" grip_left.close() elif grip_left.gripping() or grip_left.position() < 5: print "Opening gripper" #gripper is closed on something, or on nothing grip_left.open() else: print "Unknown state, opening gripper" #Unknown state, so open the gripper grip_left.open() done = False print("Controlling joints. Press ? for help, Esc to quit.") while not done and not rospy.is_shutdown(): #This had to be moved into the loop so the increment could be rebound when #the -/= keys are used to change the increment between small and large bindings = { 'u': (command_ik, ['left', [inc, 0, 0]+zeros], "increase left x"), 'j': (command_ik, ['left', [-inc, 0, 0]+zeros], "decrease left x"), 'i': (command_ik, ['left', [0, inc, 0]+zeros], "increase left y"), 'k': (command_ik, ['left', [0, -inc, 0]+zeros], "decrease left y"), 'o': (command_ik, ['left', [0, 0, inc]+zeros], "increase left z"), 'l': (command_ik, ['left', [0, 0, -inc]+zeros], "decrease left z"), 'q': (update_angles, [[angle_inc, 0, 0]], "increase left roll"), 'a': (update_angles, [[-angle_inc, 0, 0]], "decrease left roll"), 'w': (update_angles, [[0,angle_inc,0]], "increase left pitch"), 's': (update_angles, [[0,-angle_inc,0]], "decrease left pitch"), 'e': (update_angles, [[0,0,angle_inc]], "increase left yaw"), 'd': (update_angles, [[0,0,-angle_inc]], "decrease left yaw"), '-': (update_inc, [True], "small motion increments"), '=': (update_inc, [False], "large motion increments"), ' ': (toggle_gripper, [], "toggle gripper state") } c = baxter_external_devices.getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c in bindings: cmd = bindings[c] #expand binding to something like "command_jacobian(left, [0.1, 0, 0])" cmd[0](*cmd[1]) print("command: %s with params %s" % (cmd[2], cmd[1])) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(bindings.items(), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument( '-l', '--limb', required=True, choices=['left', 'right'], help='send joint trajectory to which limb' ) required.add_argument( '-f', '--folder', help='path to assets/ folder containing help images' ) args = parser.parse_args(rospy.myargv()[1:]) print args limb = args.limb print("Initializing node... ") rospy.init_node("servo_to_object_%s" % (limb,)) print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") rs.enable() #Calibrate gripper gripper_if = baxter_interface.Gripper(limb) if not gripper_if.calibrated(): print "Calibrating gripper" gripper_if.calibrate() if args.folder is None: args.folder = "/home/jackie/ros_ws/src/baxter_demos/assets/" # First, get start and end configurations from the user filenames = ["getpoint1.png", "getpoint2.png", "executing_grasp.png"] filenames = [args.folder+filename for filename in filenames] for filename in filenames: if not os.access(filename, os.R_OK): rospy.logerr("Cannot read file at '%s'" % (filename,)) return 1 limbInterface = baxter_interface.Limb(limb) #rospy.on_shutdown(display) common.send_image(filenames[0]) #Get the current position buttonpress = common.ButtonListener() buttonpress.subscribe("/robot/digital_io/"+limb+"_lower_button/state") points = [] while not buttonpress.pressed: rospy.sleep(params['button_rate']) points.append( buttonpress.getButtonPress(limbInterface)) print "Got first position:" print points[0] buttonpress.pressed = False common.send_image(filenames[1]) while not buttonpress.pressed: rospy.sleep(params['button_rate']) points.append( buttonpress.getButtonPress(limbInterface)) print "Got second position:" print points[1] common.send_image(filenames[2]) # Command start configuration limbInterface.move_to_joint_positions(points[0]) iksvc, ns = ik_command.connect_service(limb) rate = rospy.Rate(params['rate']) dc = DepthCaller(limb, iksvc) # Subscribe to estimate_depth # Move to pose published by estimate_depth while (not dc.done) and (not rospy.is_shutdown()): rate.sleep() print "Starting visual servoing" # Subscribe to object_finder and start visual servoing/grasping vc = VisualCommand(iksvc, limb) vc.subscribe() while (not vc.done) and (not rospy.is_shutdown()): rate.sleep() limbInterface.move_to_joint_positions(points[1]) # Let go gripper_if.open()
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument('-l', '--limb', required=True, choices=['left', 'right'], help='send joint trajectory to which limb') args = parser.parse_args(rospy.myargv()[1:]) limb = args.limb rospy.init_node('stackit') iksvc, ns = ik_command.connect_service(limb) moveit_commander.roscpp_initialize(sys.argv) rate = rospy.Rate(1) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander(limb + "_arm") group.allow_replanning(True) print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") rs.enable() #Calibrate gripper gripper_if = baxter_interface.Gripper(limb) if not gripper_if.calibrated(): print "Calibrating gripper" gripper_if.calibrate() obj_manager = ObjectManager() while len(obj_manager.collision_objects) <= 0: rate.sleep() objects = obj_manager.collision_objects object_height = params['object_height'] """if len(objects) > 1: stack_pose = projectPose(objects[len(objects)-1].primitive_poses[0]) stack_pose = incrementPoseMsgZ(stack_pose, object_height*2.0) objects.pop(len(objects)-1) elif len(objects) == 1: stack_pose = projectPose(objects[0].primitive_poses[0])""" stack_pose = Pose(position=Point(0.593, -0.212, object_height - 0.130), orientation=Quaternion(0.6509160466, 0.758886809948, -0.0180992582839, -0.0084573527776)) processed_win = "Processed image" raw_win = "Hand camera" cv2.namedWindow(processed_win) #cv2.namedWindow(raw_win) rospy.on_shutdown(obj_manager.remove_known_objects) for obj in objects: obj_pose = obj.primitive_poses[0] #group.pick(obj.id) #group.place(obj.id, stack_pose) print "Got pose:", obj.primitive_poses[0] pose = projectPose(obj.primitive_poses[0]) pose = incrementPoseMsgZ(pose, object_height * 1.7) print "Modified pose:", pose #if obj.id in obj_manager.id_operations: # obj_manager.id_operations[obj.id] = CollisionObject.REMOVE #obj_manager.publish(obj) print "setting target to pose" # Move to the next block group.clear_pose_targets() group.set_start_state_to_current_state() group.set_pose_target(pose) plan = group.plan() # is there a better way of checking this? plan_found = len(plan.joint_trajectory.points) > 0 if plan_found: #print "============ Waiting while RVIZ displays plan1..." #rospy.sleep(3) group.go(wait=True) imgproc = ObjectFinder("star", None, None) imgproc.subscribe("/cameras/" + limb + "_hand_camera/image") imgproc.publish(limb) vc = VisualCommand(iksvc, limb) vc.subscribe() while (not vc.done) and (not rospy.is_shutdown()): blobArray = [] for centroid, axis in zip(imgproc.centroids, imgproc.axes): blob = BlobInfo() centroid = Point(*centroid) blob.centroid = centroid if axis is None: axis = -1 * numpy.ones(6) blob.axis = Polygon([ Point(*axis[0:3].tolist()), Point(*axis[3:6].tolist()) ]) blobArray.append(blob) msg = BlobInfoArray() msg.blobs = blobArray imgproc.handler_pub.publish(msg) if imgproc.processed is not None: cv2.imshow(processed_win, imgproc.processed) cv2.waitKey(10) vc.unsubscribe() imgproc.unsubscribe() print "Adding attached message" #Add attached message #obj.primitive_poses[0] = incrementPoseMsgZ(obj.primitive_poses[0], *object_height) #this is in the base frame...? obj_manager.publish_attached(obj, limb) #touch_links = [limb+"_gripper", limb+"_gripper_base", limb+"_hand_camera", limb+"_hand_range"] #group.attach_object(obj.id, limb+"_gripper", touch_links = touch_links) # Carefully rise away from the object before we plan another path pose = incrementPoseMsgZ(pose, 2 * object_height) # test this ik_command.service_request_pose(iksvc, pose, limb, blocking=True) else: print "Unable to plan path" continue # what to do? # Move to the stacking position group.clear_pose_targets() group.set_start_state_to_current_state() group.set_pose_target(stack_pose) plan = group.plan() plan_found = len(plan.joint_trajectory.points) > 0 if plan_found: #print "============ Waiting while RVIZ displays plan2..." #rospy.sleep(3) group.go(wait=True) gripper_if.open(block=True) group.detach_object(obj.id) # Carefully rise away from the object before we plan another path pose = incrementPoseMsgZ(stack_pose, 2 * object_height) ik_command.service_request_pose(iksvc, pose, limb, blocking=True) obj.operation = CollisionObject.REMOVE obj_manager.publish(obj) # Get the next stack pose stack_pose = incrementPoseMsgZ(stack_pose, object_height * 3 / 4) """if obj.id in obj_manager.id_operations:
#!/usr/bin/env python import baxter_interface import rospy import copy from baxter_interface import CHECK_VERSION import ik_command import baxter_coordinates right_iksvc, right_ns = ik_command.connect_service('right') left_iksvc, left_ns = ik_command.connect_service('left') delta_z = 0.3 def pick(side, pos): if side not in ["left", "right"]: raise Exception("Invalid side " + side) pos_map = baxter_coordinates.coords[side] if pos not in pos_map: raise Exception(side + " can not reach " + str(pos)) gripper = baxter_interface.Gripper(side, CHECK_VERSION) coords = pos_map[pos] pre_grasp_coords = {} pre_grasp_coords['position'] = baxter_interface.limb.Limb.Point( x=coords['position'].x, y=coords['position'].y, z=coords['position'].z + delta_z) pre_grasp_coords['orientation'] = coords['orientation']