class teleop(object): def __init__(self): self.lastpose = Pose() self.lasttime = time.time() self.need_plan = True self.move_group = MoveGroupInterface("arm_with_torso", "base_link") def callback_rosbridge(self, data): if data.pose != self.lastpose: self.lastpose = data.pose self.lasttime = time.time() self.need_plan = True def check_pose_updates(self): if (not (self.need_plan and time.time() - self.lasttime > move_delay)): return self.need_plan = False pose_goal = self.lastpose gripper_frame = 'wrist_roll_link' gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' gripper_pose_stamped.header.stamp = rospy.Time.now() gripper_pose_stamped.pose = pose_goal self.move_group.moveToPose(gripper_pose_stamped, gripper_frame) print('Moved To New Pose')
def main(args): rospy.init_node('simple_move') move_group = MoveGroupInterface('manipulator', "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ 'shoulder_1_joint', 'shoulder_2_joint', 'elbow_1_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] # This is the wrist link not the gripper itself gripper_frame = "wrist_3_link" poses, pose_name = get_pose(args.reset) # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = "base_link" while not rospy.is_shutdown(): for pose in poses: # Finish building the Pose_stamped message # If the message stampe is not current it could be ignored gripper_pose_stamped.header.stamp = rospy.Time.now() # Set the message pose gripper_pose_stamped.pose = pose move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: log_output = pose_name + " Done!" rospy.loginfo(log_output) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops move_group.get_move_action().cancel_all_goals()
class Arm: def __init__(self): self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link") self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6) def MoveToPose(self, X, Y, Z, Roll, Pitch, Yaw): orientation = tf.transformations.quaternion_from_euler( Roll, Pitch, Yaw) self.poseStamped = PoseStamped() self.poseStamped.header.frame_id = 'base_link' self.poseStamped.header.stamp = rospy.Time.now() self.poseStamped.pose.position.x = X self.poseStamped.pose.position.y = Y self.poseStamped.pose.position.z = Z self.poseStamped.pose.orientation.x = orientation[0] self.poseStamped.pose.orientation.y = orientation[1] self.poseStamped.pose.orientation.z = orientation[2] self.poseStamped.pose.orientation.w = orientation[3] self.moveGroup.moveToPose(self.poseStamped, 'gripper_link', 0.005, max_velocity_scaling_factor=0.2) self.result = self.moveGroup.get_move_action().get_result() def Tuck(self): joints = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] self.moveGroup.moveToJointPosition(joints, pose, 0.01, max_velocity_scaling_factor=0.2) while not rospy.is_shutdown(): self.result = self.moveGroup.get_move_action().get_result() if self.result.error_code.val == MoveItErrorCodes.SUCCESS: break def AddDice(self, name, X, Y, Z): self.planning_scene.addCube(name, 0.1, X, Y, Z) def RemoveDice(self, name): self.planning_scene.removeCollisionObject(name)
class TrajectorySimulator(): def __init__(self): self.group = MoveGroupInterface("arm", "base_link") # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(15) print "============ Starting planning" def plan_to_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "gripper_link", tolerance=0.3, plan_only=True) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" return result.planned_trajectory def plan_to_jointspace_goal(self, pose): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" return "Success"
class RobotArm: def __init__(self): self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link") self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.removeCollisionObject("table1") def MoveToPose(self,X,Y,Z,x,y,z,w): self.poseStamped = PoseStamped() self.poseStamped.header.frame_id = 'base_link' self.poseStamped.header.stamp = rospy.Time.now() self.poseStamped.pose.position.x = X self.poseStamped.pose.position.y = Y self.poseStamped.pose.position.z = Z self.poseStamped.pose.orientation.x = x self.poseStamped.pose.orientation.y = y self.poseStamped.pose.orientation.z = z self.poseStamped.pose.orientation.w = w self.moveGroup.moveToPose(self.poseStamped, 'gripper_link') self.result = self.moveGroup.get_move_action().get_result() def OriginReturn(self): #Move to the origin joints = ["torso_lift_joint","shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] self.moveGroup.moveToJointPosition(joints, pose, 0.02) while 1: self.result = self.moveGroup.get_move_action().get_result() if self.result.error_code.val == MoveItErrorCodes.SUCCESS: break
class TrajectorySimulator(): def __init__(self): self.group = MoveGroupInterface("arm", "base_link") # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(15) print "============ Starting planning" def plan_to_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "gripper_link", tolerance = 0.3, plan_only=True) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" return result.planned_trajectory def plan_to_jointspace_goal(self, pose): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" return "Success"
class Sensory_data_collection: def __init__(self, object_name, trialnum): rospy.init_node('pnp', anonymous=True) rospy.Subscriber('/joy', Joy, self.joycallback) print('starting...') self.calibrated = False self.both_arms_group = MoveGroupInterface("both_arms", "base") self.right_arm_group = MoveGroupInterface("right_arm", "base") self.left_arm_group = MoveGroupInterface("left_arm", "base") self.leftgripper = baxter_interface.Gripper('left') self.rightgripper = baxter_interface.Gripper('right') self.leftgripper.calibrate() self.leftgripper.open() self.left_arm = baxter_interface.Limb('left') self.right_arm = baxter_interface.Limb('right') self.rightgripper.calibrate() self.rightgripper.open() self.both_arm_joints = [ 'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0', 'left_s1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2', 'right_e0', 'right_e1' ] self.right_joints = [ 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2', 'right_e0', 'right_e1' ] self.left_joints = [ 'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0', 'left_s1' ] self.init_joint_angles = [ 0.2515728492132078, 1.58958759144626, 3.0418839023767754, -1.2862428906419194, 1.2916118233995184, 0.37735927381981177, -1.3203739631723699, -0.1514806028036846, -1.2908448330055757, 2.908044078633773, -1.5424176822187836, -0.34131072530450457, 0.984048675428493, 1.1949710337627373 ] self.preright = [ 0.27381557063754636, -0.7309418454273996, 2.269908070873441, -1.0181797479589434, 0.9177040063524488, 1.1213399559442374, 1.878359474765689 ] self.preleft = [ 0.7294078646395142, 1.1850001586414822, 1.957359485341788, -0.9821311994436361, 1.5397332158399841, -0.7742768026851626, -0.7566360236244803 ] self.prepush = [ 0.6914418401393503, 0.877820505867428, 0.5725583290782307, -0.7827136970185323, 1.7636944108712544, -0.6354515413815326, -0.7646894227608787 ] self.postpush1 = [ 0.5767767762449155, 1.0749370371107037, 0.5591359971842333, -0.68568941218478, 1.4166312576121796, -0.9744612955042091, -0.6653641667452982 ] self.postpush2 = [ 0.5556845404114912, 1.135912773429149, 0.4275971446230591, -0.656543777214957, 1.0020729496861465, -1.23255356306593, -0.4686311306989939 ] self.shake1 = [ 0.7232719414879726, 1.1455001533534328, 0.9177040063524488, -0.9652574107768966, 1.551621566946096, -0.7735098122912198, -0.7631554419729933 ] self.shake2 = [ 0.6753350418665534, 1.016645767171058, 2.992413021967471, -0.9269078910797612, 1.6712720684011584, -0.8011214664731573, -0.8084078752156131 ] self.shake3 = [ 0.6412039693361029, 0.34284470609239, 2.865476111769953, -1.007825377640717, 1.3219079439602552, -0.7554855380335662, -0.8199127311247536 ] self.shake4 = [ 1.4580487388850858, 1.326893381520883, 1.8634031620838063, -0.9882671225951778, 1.2517283229144975, -0.6830049458059805, -0.9782962474739226 ] self.prepoke = [ 0.6170437719269076, 0.8969952657159956, 2.0129662889026343, -0.9119515783978784, 1.9036701577657984, -0.5215534678810406, -0.9560535260495842 ] self.pickgoal = PoseStamped() self.pickgoal.header.frame_id = "base" self.pickgoal.header.stamp = rospy.Time.now() self.pickgoal.pose.position.x = 0.57 self.pickgoal.pose.position.y = -0.1 self.pickgoal.pose.position.z = -0.05 self.pickgoal.pose.orientation.x = 1.0 self.pickgoal.pose.orientation.y = 0.0 self.pickgoal.pose.orientation.z = 0.0 self.pickgoal.pose.orientation.w = 0.0 #data saving attributes self.current_image = None self.bridge = CvBridge() self.current_jointstate = {} self.current_endpointstate = {} self.current_accelerometer = {} self.keep_saving = False self.accel_counter = 0 self.joint_counter = 0 self.endpoint_counter = 0 self.pause_event = Event() rospy.Subscriber('/robot/accelerometer/left_accelerometer/state', Imu, self.accel_callback) rospy.Subscriber('/robot/limb/left/endpoint_state', EndpointState, self.endpoint_callback) rospy.Subscriber('/robot/joint_states', JointState, self.joint_callback) rospy.Subscriber('/camera/rgb/image_raw', Image, self.image_callback) self.object_name = object_name self.trialnum = trialnum if not os.path.exists(os.path.dirname(self.object_name)): try: os.mkdir(self.object_name) except OSError as exc: pass self.q = queue.Queue() self.should_record = False self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) rospy.sleep(5) self.perform_action_sequence('left') rospy.spin() def perform_action_sequence(self, arm): self.capture_image(self.trialnum, self.object_name) self.lift(arm) self.shake(arm) self.drop(arm) self.push(arm) rospy.sleep(6) self.poke(arm) self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) def joycallback(self, data): if data.buttons[3] == 1: #X arm = 'left' self.keep_saving = True self.capture_image('lift', self.object_name) self.start_recording_audio('lift', self.object_name) self.lift(arm) self.stop_recording_audio() self.save_data('lift', self.object_name) elif data.buttons[1] == 1: #B arm = 'left' self.keep_saving = True self.capture_image('push', self.object_name) self.start_recording_audio('push', self.object_name) self.push(arm) self.stop_recording_audio() self.save_data('push', self.object_name) elif data.buttons[4] == 1: #Y arm = 'left' self.keep_saving = True self.capture_image('drop', self.object_name) self.start_recording_audio('drop', self.object_name) self.drop(arm) self.stop_recording_audio() self.save_data('drop', self.object_name) elif data.buttons[0] == 1: #A arm = 'left' self.keep_saving = True self.capture_image('shake', self.object_name) self.start_recording_audio('shake', self.object_name) self.shake(arm) self.stop_recording_audio() self.save_data('shake', self.object_name) def record_callback(self, indata, frames, time, status): self.q.put(indata.copy()) def start_recording_audio(self, action, object_name): self.should_record = True thread.start_new_thread(self.record_audio, (action, object_name)) def stop_recording_audio(self): self.should_record = False def record_audio(self, action, object_name): try: name = object_name + '/baxter_' + action + "_audio_" + object_name + '_' + self.trialnum + '.wav' with sf.SoundFile(name, mode='x', samplerate=48000, channels=1) as file: with sd.InputStream(samplerate=48000, channels=1, callback=self.record_callback): print('#' * 80) print('recording audio...') print('#' * 80) while self.should_record: file.write(self.q.get()) self.q = queue.Queue() except KeyboardInterrupt: print('\nRecording finished: ' + repr('test.wav')) parser.exit(0) except Exception as e: print(e) def push(self, arm): # Move both arms to start state # self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) if arm == 'left': self.left_arm_group.moveToJointPosition(self.left_joints, self.prepush, plan_only=False) rospy.sleep(2) self.keep_saving = True self.start_recording_audio('push', self.object_name) self.left_arm_group.moveToJointPosition(self.left_joints, self.postpush1, plan_only=False) self.left_arm_group.moveToJointPosition(self.left_joints, self.postpush2, plan_only=False) rospy.sleep(2) self.stop_recording_audio() self.save_data('push', self.object_name) self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) rospy.sleep(5) def shake(self, arm): if arm == 'left': #lift #shake self.keep_saving = True self.start_recording_audio('shake', self.object_name) self.left_arm_group.moveToJointPosition(self.left_joints, self.shake1, plan_only=False) self.left_arm_group.moveToJointPosition(self.left_joints, self.shake2, plan_only=False) self.left_arm_group.moveToJointPosition(self.left_joints, self.shake3, plan_only=False) self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) self.left_arm_group.moveToJointPosition(self.left_joints, self.shake4, plan_only=False) self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) self.stop_recording_audio() self.save_data('shake', self.object_name) rospy.sleep(5) def drop(self, arm): self.keep_saving = True self.start_recording_audio('drop', self.object_name) self.leftgripper.open() rospy.sleep(5) self.stop_recording_audio() self.save_data('drop', self.object_name) # rospy.sleep(5) def poke(self, arm): self.leftgripper.close() self.left_arm_group.moveToJointPosition(self.left_joints, self.prepoke, plan_only=False) self.keep_saving = True self.start_recording_audio('poke', self.object_name) self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) rospy.sleep(2) self.stop_recording_audio() self.save_data('poke', self.object_name) rospy.sleep(5) def lift(self, arm): # Move both arms to start state # self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) #pick if arm == 'left': self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) rospy.sleep(1) self.keep_saving = True self.start_recording_audio('lift', self.object_name) self.left_arm_group.moveToPose(self.pickgoal, "left_gripper", plan_only=False) self.leftgripper.close() self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) self.stop_recording_audio() self.save_data('lift', self.object_name) else: self.right_arm_group.moveToJointPosition(self.right_joints, self.preright, plan_only=False) rospy.sleep(1) self.right_arm_group.moveToPose(self.pickgoal, "right_gripper", plan_only=False) self.rightgripper.close() rospy.sleep(5) #data saving methods def accel_callback(self, data): if self.keep_saving: self.current_accelerometer[ self.accel_counter] = mc.convert_ros_message_to_dictionary( data) self.accel_counter += 1 def endpoint_callback(self, data): if self.keep_saving: self.current_endpointstate[ self.endpoint_counter] = mc.convert_ros_message_to_dictionary( data) self.endpoint_counter += 1 def joint_callback(self, data): if self.keep_saving: self.current_jointstate[ self.joint_counter] = mc.convert_ros_message_to_dictionary( data) self.joint_counter += 1 def image_callback(self, image): self.current_image = image def capture_image(self, action, object_name): img = self.bridge.imgmsg_to_cv2(self.current_image, 'bgr8') shape = img.shape img = img[130:shape[0] - 230, 330:shape[1] - 200] #y,x name = object_name + "/baxter_image_" + object_name + '_' + self.trialnum + '.png' if cv2.imwrite(name, img): print('Image saved') else: print('Could not save image') def save_data(self, action, object_name): self.keep_saving = False accel_name = object_name + '/baxter_' + action + "_accel_" + object_name + '_' + self.trialnum + '.json' endpoint_name = object_name + '/baxter_' + action + "_endstate_" + object_name + '_' + self.trialnum + '.json' joint_name = object_name + '/baxter_' + action + "_jointstate_" + object_name + '_' + self.trialnum + '.json' rospy.sleep(1) with open(accel_name, "w") as accel_file: d = copy.deepcopy(self.current_accelerometer) json.dump(d, accel_file, indent=4) self.current_accelerometer = {} self.accel_counter = 0 with open(endpoint_name, "w") as endpoint_file: e = copy.deepcopy(self.current_endpointstate) json.dump(e, endpoint_file, indent=4) self.current_endpointstate = {} self.endpoint_counter = 0 with open(joint_name, "w") as joint_file: j = copy.deepcopy(self.current_jointstate) json.dump(j, joint_file, indent=4) self.current_jointstate = {} self.joint_counter = 0 print('Data saved')
def picknplace(): # Define initial parameters p = PlanningSceneInterface("base") g = MoveGroupInterface("both_arms", "base") gr = MoveGroupInterface("right_arm", "base") gl = MoveGroupInterface("left_arm", "base") leftgripper = baxter_interface.Gripper('left') leftgripper.calibrate() leftgripper.open() jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519] lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384] rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] # Clear planning scene p.clear() # Add table as attached object p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) # Move both arms to start state g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Loop to continue pick and place until all objects are cleared from table j=0 k=0 while locs_x: p.clear() # CLear planning scene of all collision objects # Attach table as attached collision object to base of baxter p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) xn = locs_x[0] yn = locs_y[0] zn = -0.06 thn = orien[0] sz = size[0] if thn > pi/4: thn = -1*(thn%(pi/4)) # Add collision objects into planning scene objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] for i in range(1,len(locs_x)): p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05) p.waitForSync() # Move both arms to pos2 (Right arm away and left arm on table) g.moveToJointPosition(jts_both, pos2, plan_only=False) # Move left arm to pick object and pick object pickgoal = PoseStamped() pickgoal.header.frame_id = "base" pickgoal.header.stamp = rospy.Time.now() pickgoal.pose.position.x = xn pickgoal.pose.position.y = yn pickgoal.pose.position.z = zn+0.1 pickgoal.pose.orientation.x = 1.0 pickgoal.pose.orientation.y = 0.0 pickgoal.pose.orientation.z = 0.0 pickgoal.pose.orientation.w = 0.0 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn) gl.moveToPose(pickgoal, "left_gripper", plan_only=False) pickgoal.pose.position.z = zn gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False) leftgripper.close() pickgoal.pose.position.z = zn+0.1 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Move both arms to pos1 g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Place object stleft = PoseStamped() stleft.header.frame_id = "base" stleft.header.stamp = rospy.Time.now() stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.55 stleft.pose.position.z = 0.1 stleft.pose.orientation.x = 1.0 stleft.pose.orientation.y = 0.0 stleft.pose.orientation.z = 0.0 stleft.pose.orientation.w = 0.0 # Stack objects (large cubes) if size if greater than some threshold if sz > 16.: stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.7 stleft.pose.position.z = -0.04+(k*0.05) k += 1 gl.moveToPose(stleft, "left_gripper", plan_only=False) leftgripper.open() stleft.pose.position.z = 0.3 gl.moveToPose(stleft, "left_gripper", plan_only=False)
class TrajectoryGenerator(): # Wrapper class that facilitates trajectory planning. # Includes methods for planning to arbitrary joint-space goals # and end-effector pose goals. def __init__(self): self.group = MoveGroupInterface("arm", "base_link") self.gripper = MoveGroupInterface("gripper", "base_link") self.virtual_state = RobotState() self.trajectory = None # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(5) print "============ Starting planning" def generate_trajectory(self, goal_poses): for goal_pose in goal_poses: if goal_pose.goal_type is "ee_pose": traj_result = self.plan_to_ee_pose_goal(goal_pose.pose) elif goal_pose.goal_type is "jointspace": traj_result = self.plan_to_jointspace_goal(goal_pose.state) elif goal_pose.goal_type is "gripper_posture": traj_result = self.plan_to_gripper_goal(goal_pose.state) else: rospy.loginfo("Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'") sys.exit() approved_trajectories.append(traj_result) return approved_trajectories def plan_to_ee_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "gripper_link", tolerance = 0.02, plan_only=True, start_state = self.virtual_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" self.trajectory = result.planned_trajectory.joint_trajectory return self.trajectory def plan_to_jointspace_goal(self, pose): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose, plan_only=True, start_state=self.virtual_state ) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" self.trajectory = result.planned_trajectory #.joint_trajectory return self.trajectory def plan_to_gripper_goal(self, posture): joints = ["gripper_link", "l_gripper_finger_link", "r_gripper_finger_link"] while not rospy.is_shutdown(): result = self.gripper.moveToJointPosition(joints, pose, plan_only=True, start_state=self.virtual_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Gripper Posture Plan Successful") self.trajectory = result.planned_trajectory.joint_trajectory return self.trajectory def update_virtual_state(self): # Sets joint names and sets position to final position of previous trajectory if self.trajectory == None: print "No trajectory available to provide a new virtual state." print "Update can only occur after trajectory has been planned." else: self.virtual_state.joint_state.name = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] self.virtual_state.joint_state.position = self.trajectory.points[-1].positions def clear_virtual_state(self): self.virtual_state.joint_state.joint_names = [] self.virtual_state.joint_state.position = [] def execute_trajectory(self, trajectory): execute_known_trajectory = rospy.ServiceProxy('execute_known_trajectory', ExecuteKnownTrajectory) result = execute_known_trajectory(trajectory)
class Robot(object): """ fetch max x = 0.96 (wrist roll joint) """ def __init__(self): rospy.loginfo("Waiting for Fetch ...") self.tuck_the_arm_joints_value = [ 0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0 ] self.stow_joints_value = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] self.move_group = MoveGroupInterface("arm_with_torso", "base_link") self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso") self.joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] def move_to_pose(self, postion, quaternion, frame='base_link'): target_frame = 'wrist_roll_link' pose_stamped = PoseStamped() pose_stamped.header.frame_id = frame pose_stamped.header.stamp = rospy.Time.now() pose_stamped.pose = Pose( Point(postion[0], postion[1], postion[2]), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) if not rospy.is_shutdown(): self.move_group.moveToPose(pose_stamped, target_frame) result = self.move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Move success!") return True else: rospy.logerr("in state: %s", self.move_group.get_move_action().get_state()) return False else: rospy.logerr("MoveIt! failure no result returned.") return False def move_to_joints(self, joints_value): # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, joints_value, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("move to joint Success!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", self.move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") def tuck_the_arm(self): self.move_to_joints(self.tuck_the_arm_joints_value) def stow(self): self.move_to_joints(self.stow_joints_value) def stop(self): """This stops all arm movement goals It should be called when a program is exiting so movement stops """ self.move_group.get_move_action().cancel_all_goals()
class FetchPose: def __init__(self): self.move_group = MoveGroupInterface("arm_with_torso", "base_link") self.pose_group = moveit_commander.MoveGroupCommander("arm_with_torso") self.joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # planning_scene = PlanningSceneInterface("base_link") # planning_scene.removeCollisionObject("my_front_ground") # planning_scene.removeCollisionObject("my_back_ground") # planning_scene.removeCollisionObject("my_right_ground") # planning_scene.removeCollisionObject("my_left_ground") # planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) # planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) # planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) # planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.tuck_the_arm_joints_value = [ 0, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0 ] self.stow_joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] # self.high_pose_value = [0.38380688428878784, -0.12386894226074219, -0.31408262252807617, 2.1759514808654785, 0.0061359405517578125, 0.9556700587272644, -1.921694278717041, -1.6908303499221802] self.high_pose_value = [ 0.4946120488643647, -0.19136428833007812, -0.4793691635131836, 0.009587380103766918, 0.1629854440689087, 0.2983585298061371, 1.8430781364440918, -1.6992675065994263 ] self.trans = Transformer() def excute(self, joints_value): if rospy.is_shutdown(): return False # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, joints_value, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("pose Success!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", self.move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") def tuck_the_arm(self): self.excute(self.tuck_the_arm_joints_value) def stow(self): self.excute(self.stow_joints_value) def hige_pose(self): self.excute(self.high_pose_value) def control_torso(self, offset=0.1): joints_values = self.pose_group.get_current_joint_values() joints_values[0] += offset self.excute(joints_values) def grasp_by_move_group(self, point_xyz): wrist_roll_link_gripper_offset = 0.20 q1 = self.trans.quaternion_from_direction((1, 0, 0), (0, 0, -1)) q2 = self.trans.quaternion_from_axis(90, (1, 0, 0)) quaternion = quaternion_multiply(q1, q2) pose = Pose( Point(point_xyz[0], point_xyz[1], point_xyz[2] + wrist_roll_link_gripper_offset), # offset Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' gripper_pose_stamped.header.stamp = rospy.Time.now() gripper_pose_stamped.pose = pose gripper_frame = 'wrist_roll_link' self.move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = self.move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: print("reach success!") return 'Success' else: rospy.logerr("reach fail") return 'Fail' else: rospy.logerr("MoveIt! failure no result returned.") return 'Fail'
class FetchArmController: def __init__(self): self.moving_mode = False self.plan_only = False self.prev_joy_buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] self.joy_subscriber = rospy.Subscriber("/fetch_joy", Joy, self.joy_callback) self.joints_subscriber = rospy.Subscriber("/joint_states", JointState, self.joints_states_callback) self.rgbimage_subscriber = rospy.Subscriber("/head_camera/depth/image", Image, self.depthimage_callback) self.depthimage_subscriber = rospy.Subscriber( "/head_camera/depth/image", Image, self.rgbimage_callback) self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_reset_pos", Empty, self.arm_reset_callback) self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_start_pos", Empty, self.arm_start_callback) self.arm_effort_pub = rospy.Publisher( "/arm_controller/weightless_torque/command", JointTrajectory, queue_size=2) self.gripper_client = actionlib.SimpleActionClient( "gripper_controller/gripper_action", GripperCommandAction) self.arm_cartesian_twist_pub = rospy.Publisher( "/arm_controller/cartesian_twist/command", Twist, queue_size=2) self.head_point_client = actionlib.SimpleActionClient( "head_controller/point_head", PointHeadAction) self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only=self.plan_only) planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) def joints_states_callback(self, data): if len(data.name) == 13: self.arm_joints = data.position[-7:] def depthimage_callback(self, image): self.depth_image = image def rgbimage_callback(self, image): self.rgb_image = image def joy_callback(self, joy): if not self.moving_mode: self.arm_control_action(joy) self.prev_joy_buttons = joy.buttons def arm_control_action(self, joy): twist = Twist() twist.linear.x = 0.2 * joy.axes[1] twist.linear.y = 0.2 * joy.axes[0] twist.linear.z = -0.2 * joy.axes[2] twist.angular.x = 0.2 * joy.axes[5] twist.angular.y = 0.2 * joy.axes[6] twist.angular.z = 0.2 * joy.axes[4] self.arm_cartesian_twist_pub.publish(twist) if joy.buttons[0] and not self.prev_joy_buttons[0]: goal = GripperCommandGoal() goal.command.max_effort = 60 goal.command.position = 0.0 self.gripper_client.send_goal(goal) elif not joy.buttons[0] and self.prev_joy_buttons[0]: goal = GripperCommandGoal() goal.command.max_effort = 60 goal.command.position = 0.1 self.gripper_client.send_goal(goal) def arm_reset_callback(self, data): reset_pose_stamped = PoseStamped() reset_pose_stamped.header.frame_id = "base_link" reset_pose_stamped.header.stamp = rospy.Time.now() reset_pose_stamped.pose = Pose(Point(0.055, -0.140, 0.623), Quaternion(0.460, -0.504, 0.511, 0.523)) self.arm_move_group.moveToPose(reset_pose_stamped, "wrist_roll_link") def arm_start_callback(self, data): reset_pose_stamped = PoseStamped() reset_pose_stamped.header.frame_id = "base_link" reset_pose_stamped.header.stamp = rospy.Time.now() reset_pose_stamped.pose = Pose(Point(0.213, -0.520, 0.605), Quaternion(0.545, 0.766, -0.197, 0.278)) self.arm_move_group.moveToPose(reset_pose_stamped, "wrist_roll_link")
class TrajectorySimulator(): def __init__(self): # Instantiate a RobotCommander object. # This object is an interface to the robot as a whole. #robot = moveit_commander.RobotCommander() # Instantiate a MoveGroupCommander object. # This object is an interface to one 'group' of joints, # in our case the joints of the arm. self.group = MoveGroupInterface("arm", "base_link") # Create DisplayTrajectory publisher to publish trajectories # for RVIZ to visualize. # self.display_trajectory_publisher = rospy.Publisher( #"/move_group/display_planned_path", #moveit_msgs.msg.DisplayTrajectory) # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(15) print "============ Starting tutorial " def get_state_info(self): # Let's get some basic information about the robot. # This info may be helpful for debugging. # Get name of reference frame for this robot print "============ Reference frame: %s" % self.group.get_planning_frame( ) # Get name of end-effector link for this group print "============ Reference frame: %s" % self.group.get_end_effector_link( ) # List all groups defined on the robot print "============ Robot Groups:" print self.robot.get_group_names() # Print entire state of the robot print "============ Printing robot state" print self.robot.get_current_state() print "============" def plan_to_pose_goal(self, goal): ### Planning to a Pose Goal # Let's plan a motion for this group to a desired pose for the # end-effector print "============ Generating plan 1" # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "gripper_link", tolerance=0.3, plan_only=False) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" return # Now we call the planner to compute the plan and visualize it if successful. # We are just planning here, not asking move_group to actually move the robot. print "============ Waiting while RVIZ displays plan1..." rospy.sleep(5) print "============ Goal achieved" # group.plan() automatically asks RVIZ to visualize the plan, # so we need not explicitly ask RVIZ to do this. # For completeness of understanding, however, we make this # explicit request below. The same trajectory should be displayed # a second time. print "============ Visualizing plan1" """ display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state() display_trajectory.trajectory.append(plan1) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while plan1 is visualized (again)..." rospy.sleep(5) """ def plan_to_jointspace_goal(self, pose): # Let's set a joint-space goal and visualize it in RVIZ # A joint-space goal differs from a pose goal in that # a goal for the state of each joint is specified, rather # than just a goal for the end-effector pose, with no # goal specified for the rest of the arm. joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose, 0.1) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" rospy.sleep(5) return "Success"
class GraspingClient(object): def __init__(self): self.arm = MoveGroupInterface('arm_with_torso', 'map') self.move_group = MoveGroupInterface('arm', 'base_link') def pick(self, target_pose): #print self.arm._fixed_frame, self.arm.planner_id #Taking coordinate values of the object object_x, object_y, object_z = round( target_pose.pose.position.x, 2), round(target_pose.pose.position.y, 2), round(target_pose.pose.position.z, 2) print object_x, object_y, object_z #Initializing offset values for translation offset_x, offset_y, offset_z = 0, 0, 0.05 #Initializing offset values for rotation roll, pitch, yaw = 0, 0, 0 #Applying rotation to the arm for i in range(10, 1, -1): #Adding translation along x-axis final_x = object_x + offset_x #Adding translation along y-axis final_y = object_y + offset_y #Adding translation along z-axis final_z = object_z + offset_z + i * 0.1 print final_x, final_y, final_z, i * 0.1 final_quaternion = quaternion_from_euler(radians(roll), radians(pitch), radians(yaw), 'sxyz') pose_goal = PoseStamped() pose_goal.header.frame_id = 'map' pose_goal.pose.position.x = final_x pose_goal.pose.position.y = final_y pose_goal.pose.position.z = final_z pose_goal.pose.orientation.x = final_quaternion[0] pose_goal.pose.orientation.y = final_quaternion[1] pose_goal.pose.orientation.z = final_quaternion[2] pose_goal.pose.orientation.w = final_quaternion[3] pose_goal.header.stamp = rospy.Time.now() self.arm.moveToPose(pose_goal, 'wrist_roll_link') result = self.arm.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo('Moved arm') rospy.sleep(10) else: rospy.logerr('Cannot move arm') else: rospy.logerr('No result') def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo('Tucking done')
class moveit_example: def __init__(self): # initialize ROS node self.picking = False rospy.init_node('eexxaammple', anonymous=True) # create variables for the arms self.left_arm = baxter_interface.Limb('left') self.right_arm = baxter_interface.Limb('right') # print self.left_arm.joint_angles() # i = 0; # while (i == 0): # print(self.left_arm.joint_efforts()) # rospy.sleep(.5) #release_object(self) rospy.Subscriber('/object_poses', PoseArray, self.object_poses_callback) self.calibrated = False # initialize arm group variables for MoveIt self.both_arms_group = MoveGroupInterface("both_arms", "base") self.right_arm_group = MoveGroupInterface("right_arm", "base") self.left_arm_group = MoveGroupInterface("left_arm", "base") # create variables for the grippers self.leftgripper = baxter_interface.Gripper('left') self.rightgripper = baxter_interface.Gripper('right') # calibrate left gripper and keep it open self.leftgripper.calibrate() self.leftgripper.open() # calibrate right gripper and keep it open self.rightgripper.calibrate() self.rightgripper.open() # names of the left and right joints self.both_arm_joints = [ 'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0', 'left_s1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2', 'right_e0', 'right_e1' ] self.right_joints = [ 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2', 'right_e0', 'right_e1' ] self.left_joints = [ 'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0', 'left_s1' ] self.object_poses = None # joint angles for initial position of arms # these were manually recorded. I essentially just moved the arms to the positions # I desired and looked up their joint angles. self.init_joint_angles = [ 0.2515728492132078, 1.58958759144626, 3.0418839023767754, -1.2862428906419194, 1.2916118233995184, 0.37735927381981177, -1.3203739631723699, -0.1514806028036846, -1.2908448330055757, 2.908044078633773, -1.5424176822187836, -0.34131072530450457, 0.984048675428493, 1.1949710337627373 ] self.tug = [ 0.7294078646395142, 1.1850001586414822, 1.957359485341788, -0.9821311994436361, 1.5397332158399841, -0.7742768026851626, -0.7566360236244803 ] # joint angles for pre-grasping poses of the left and right arms. # These were manually retrieved. I essentially just moved the arms to the desired # positions and read off their joint angles self.preright = [ 0.27381557063754636, -0.7309418454273996, 2.269908070873441, -1.0181797479589434, 0.9177040063524488, 1.1213399559442374, 1.878359474765689 ] self.preleft = [ 0.7294078646395142, 1.1850001586414822, 1.957359485341788, -0.9821311994436361, 1.5397332158399841, -0.7742768026851626, -0.7566360236244803 ] self.pregrasp_pose = [ 0.3996019952441503, 1.7648448964621686, 2.2507333110248733, -1.0599807244288209, 1.1293933550806359, -0.6764855274574675, -1.113286556807839 ] # Here, I manually specify the 3D position and orientation of the object. # I hard-code the position here. # This is possibly the only functionality yout'd modify once the vision system is running. # Once the vision system is running, it will publish the poses of candidate objects to # a ROS topic. Your job would be to to read pose messages from the topic, select a pose # from the list and feed it to moveit. # self.pickgoal = PoseStamped() self.pickgoal.header.frame_id = "base" self.pickgoal.header.stamp = rospy.Time.now() self.pickgoal.pose.position.x = 0.57 self.pickgoal.pose.position.y = -0.1 self.pickgoal.pose.position.z = -0.05 self.pickgoal.pose.orientation.x = 1.0 self.pickgoal.pose.orientation.y = 0.0 self.pickgoal.pose.orientation.z = 0.0 self.pickgoal.pose.orientation.w = 0.0 ######### THIS IS WHERE THE ARM MOTION HAPPENS ############## # Move both arms to their initial positions. Then wait for 5 seconds (rospy.sleep(5)) self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) # rospy.sleep(5) # # Move the left arm to the pre-grasping position so that the grippers hover over # # the object # self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) # # Move arm to the object # self.left_arm_group.moveToPose(self.pickgoal, "left_gripper", plan_only=False) # # close the left gripper # self.leftgripper.close() # self.preleft = [0.41302432713814763, 1.419315723990979, 2.2227381616459647, -0.724805922275858, # 1.2198982215658754, -0.947233136519243, -0.9100341024130217] # Move arm back to the pre-grasping position. At this point the object should be # between the grippers # self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) # self.preleft = [0.336325287743877, 1.1060001480653834, 2.5651993725413833, -0.7156020375485456, # 1.7215099392044055, -0.45099035163831164, -1.2241166687325602] # self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) # self.leftgripper.open() rospy.sleep(3) for i in range(5): self.pick_arbitrary_object() rospy.sleep(2) rospy.spin() def object_poses_callback(self, data): if not self.picking: N = len(data.poses) if N > 0: self.object_poses = data.poses else: self.object_poses = None def pick_arbitrary_object(self): if self.object_poses is not None: N = len(self.object_poses) target = random.sample(self.object_poses, 1)[0] self.pick_up_and_put_back(target) def pick_up_and_put_back(self, pose): stamped = PoseStamped() stamped.header.frame_id = "base" stamped.header.stamp = rospy.Time.now() stamped.pose = pose self.picking = True #pick up self.left_arm_group.moveToJointPosition(self.left_joints, self.pregrasp_pose, plan_only=False) self.left_arm_group.moveToPose(stamped, "left_gripper", plan_only=False) self.leftgripper.close() self.left_arm_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) rospy.sleep(2) #put back self.left_arm_group.moveToJointPosition(self.left_joints, self.pregrasp_pose, plan_only=False) self.left_arm_group.moveToPose(stamped, "left_gripper", plan_only=False) self.leftgripper.open() self.left_arm_group.moveToJointPosition(self.left_joints, self.pregrasp_pose, plan_only=False) self.left_arm_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) self.picking = False rospy.sleep(2)
def main(): rospy.init_node('move_group_python_path_follow', anonymous=True) planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) moveit_commander.roscpp_initialize(sys.argv) move_group = MoveGroupInterface("arm", "base_link") robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("arm") print "============ Robot Groups:" print robot.get_group_names() print "============ Printing robot state" print robot.get_current_state() print "============" gripper_frame = 'wrist_roll_link' gripper_poses = list() radius = 0.2 for theta in np.arange(-pi, pi, 0.5 * pi): gripper_poses.append( Pose( Point(0.75, radius * np.sin(theta), 0.6 + radius * np.cos(theta)), Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)))) gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' gripper_pose_stamped.header.stamp = rospy.Time.now() gripper_pose_stamped.pose = gripper_poses[0] move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Success!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) trajectory = RobotTrajectory() (plan, fraction) = group.compute_cartesian_path(gripper_poses, 0.01, 0.0) group.execute(plan) move_group.get_move_action().cancel_all_goals()
def move_corner(self, x, y): position = self.cal_position.get_base_position_from_pix(x, y) position[0] = position[0] - 0.20 move_group = MoveGroupInterface("arm_with_torso", "base_link") planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # This is the wrist link not the gripper itself gripper_frame = 'wrist_roll_link' pose = Pose(Point(position[0], position[1], position[2]), Quaternion(0, 0, 0, 1)) # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' # Finish building the Pose_stamped message # If the message stamp is not current it could be ignored gripper_pose_stamped.header.stamp = rospy.Time.now() # Set the message pose gripper_pose_stamped.pose = pose # Move gripper frame to the pose specified move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Hello there!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") time.sleep(1) joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] move_group.moveToJointPosition(joint_names, joints_value, wait=False) # Since we passed in wait=False above we need to wait here move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("pose Success!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", self.move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.")
class TrajectoryGenerator(): # Wrapper class that facilitates trajectory planning. # Includes methods for planning to arbitrary joint-space goals # and end-effector pose goals. def __init__(self): self.group = MoveGroupInterface("arm", "base_link", plan_only=True) self.gripper = MoveGroupInterface("gripper", "base_link", plan_only=True) self.virtual_arm_state = RobotState() self.virtual_gripper_state = RobotState() # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(5) print "============ Starting planning" def generate_trajectories(self, motion_goals, **kwargs): approved_trajectories = list() supported_args = ("get_approval") for arg in kwargs.keys(): if arg not in supported_args: rospy.loginfo( "generate_trajectories: unsupported argument: %s", arg) if type(motion_goals) is not type(list()): motion_goals = [motion_goals] for motion_goal in motion_goals: if motion_goal.goal_type is 'ee_pose': print "==== Virtual Start State: ", self.virtual_arm_state trajectory = self.plan_to_ee_pose_goal(motion_goal.pose) elif motion_goal.goal_type is "jointspace": trajectory = self.plan_to_jointspace_goal(motion_goal.state) elif motion_goal.goal_type is "gripper_posture": trajectory = self.plan_to_gripper_goal(motion_goal.posture) else: rospy.loginfo( "Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'" ) sys.exit() try: kwargs["get_approval"] approved = self.get_user_approval() if approved: approved_trajectories.append(trajectory) self.update_virtual_state(trajectory) else: # Recur approved_trajectories += self.generate_trajectories( motion_goal, get_approval=True) except KeyError: pass returnable_approved_trajectories = copy.deepcopy(approved_trajectories) return returnable_approved_trajectories def get_user_approval(self): choice = 'not_set' options = {'y': True, 'r': False} while choice.lower() not in options.keys(): choice = raw_input("Add plan to execution queue? Answer 'y' for yes "\ " or 'r' for replan: ") return options[choice] def plan_to_ee_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "wrist_roll_link", tolerance=0.02, plan_only=True, start_state=self.virtual_arm_state, planner_id="PRMkConfigDefault") if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" self.trajectory = result.planned_trajectory.joint_trajectory return result.planned_trajectory def plan_to_jointspace_goal(self, state): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] while not rospy.is_shutdown(): result = self.group.moveToJointPosition( joints, state, plan_only=True, start_state=self.virtual_arm_state, planner_id="PRMkConfigDefault") if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" self.trajectory = result.planned_trajectory.joint_trajectory return result.planned_trajectory def plan_to_gripper_goal(self, posture): joints = ["l_gripper_finger_joint", "r_gripper_finger_joint"] while not rospy.is_shutdown(): result = self.gripper.moveToJointPosition( joints, posture, plan_only=True, start_state=self.virtual_gripper_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Gripper Posture Plan Successful") self.trajectory = result.planned_trajectory.joint_trajectory return result.planned_trajectory def update_virtual_state(self, trajectory): # Sets joint names and sets position to final position of previous trajectory if len(trajectory.joint_trajectory.points[-1].positions) == 7: self.virtual_arm_state.joint_state.name = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] self.virtual_arm_state.joint_state.position = trajectory.joint_trajectory.points[ -1].positions rospy.loginfo("Virtual Arm State Updated") print self.virtual_arm_state.joint_state.position elif len(trajectory.joint_trajectory.points[-1].positions) == 2: self.virtual_gripper_state.joint_state.name = [ "l_gripper_finger_joint", "r_gripper_finger_joint" ] self.virtual_gripper_state.joint_state.position = trajectory.joint_trajectory.points[ -1].positions rospy.loginfo("Virtual Gripper State Updated") print self.virtual_gripper_state.joint_state.position def clear_virtual_arm_state(self): self.virtual_arm_state.joint_state.name = [] self.virtual_arm_state.joint_state.position = [] def clear_virtual_gripper_state(self): self.virtual_gripper_state.joint_state.names = [] self.virtual_gripper_state.joint_state.position = [] def get_virtual_arm_state(self): virtual_arm_state = copy.deepcopy(self.virtual_arm_state) return virtual_arm_state def get_virtual_gripper_state(self): virtual_gripper_state = copy.deepcopy(self.virtual_gripper_state) return virtual_gripper_state
# gripper_pose_grasp = PoseStamped() # gripper_pose_grasp.header.frame_id = "base_link" # gripper_pose_grasp.pose.position = object_pose.g_pos.position # gripper_pose_grasp.pose.orientation = object_pose.g_pos.orientation # gripper_pose_grasp.pose.position.x = g_pos_x - Lwrist2gripper # print(object_pose) print('created msgs') print(gripper_pose_pregrasp) print(gripper_pose_grasp) print(gripper_pose_up) # Pregrasp gripper_pose_pregrasp.header.stamp = rospy.Time.now() move_group.moveToPose(gripper_pose_pregrasp, gripper_frame) result = move_group.get_move_action().get_result() print('moving to pregrasp') if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Moved to Pregrasp") print() else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else:
class TrajectoryGenerator(): # Wrapper class that facilitates trajectory planning. # Includes methods for planning to arbitrary joint-space goals # and end-effector pose goals. def __init__(self): self.group = MoveGroupInterface("arm", "base_link", plan_only=True) self.gripper = MoveGroupInterface("gripper", "base_link", plan_only=True) self.virtual_arm_state = RobotState() self.virtual_gripper_state = RobotState() # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(5) print "============ Starting planning" def generate_trajectories(self, motion_goals, **kwargs): approved_trajectories = list() supported_args = ("get_approval") for arg in kwargs.keys(): if arg not in supported_args: rospy.loginfo("generate_trajectories: unsupported argument: %s", arg) if type(motion_goals) is not type(list()): motion_goals = [motion_goals] for motion_goal in motion_goals: if motion_goal.goal_type is 'ee_pose': print "==== Virtual Start State: ", self.virtual_arm_state trajectory = self.plan_to_ee_pose_goal(motion_goal.pose) elif motion_goal.goal_type is "jointspace": trajectory = self.plan_to_jointspace_goal(motion_goal.state) elif motion_goal.goal_type is "gripper_posture": trajectory = self.plan_to_gripper_goal(motion_goal.posture) else: rospy.loginfo("Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'") sys.exit() try: kwargs["get_approval"] approved = self.get_user_approval() if approved: approved_trajectories.append(trajectory) self.update_virtual_state(trajectory) else: # Recur approved_trajectories += self.generate_trajectories(motion_goal, get_approval=True) except KeyError: pass returnable_approved_trajectories = copy.deepcopy(approved_trajectories) return returnable_approved_trajectories def get_user_approval(self): choice = 'not_set' options = {'y':True, 'r':False} while choice.lower() not in options.keys(): choice = raw_input("Add plan to execution queue? Answer 'y' for yes "\ " or 'r' for replan: ") return options[choice] def plan_to_ee_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "wrist_roll_link", tolerance = 0.02, plan_only=True, start_state = self.virtual_arm_state, planner_id = "PRMkConfigDefault") if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" self.trajectory = result.planned_trajectory.joint_trajectory return result.planned_trajectory def plan_to_jointspace_goal(self, state): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, state, plan_only=True, start_state=self.virtual_arm_state, planner_id = "PRMkConfigDefault") if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" self.trajectory = result.planned_trajectory.joint_trajectory return result.planned_trajectory def plan_to_gripper_goal(self, posture): joints = ["l_gripper_finger_joint", "r_gripper_finger_joint"] while not rospy.is_shutdown(): result = self.gripper.moveToJointPosition(joints, posture, plan_only=True, start_state=self.virtual_gripper_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Gripper Posture Plan Successful") self.trajectory = result.planned_trajectory.joint_trajectory return result.planned_trajectory def update_virtual_state(self, trajectory): # Sets joint names and sets position to final position of previous trajectory if len(trajectory.joint_trajectory.points[-1].positions) == 7: self.virtual_arm_state.joint_state.name = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] self.virtual_arm_state.joint_state.position = trajectory.joint_trajectory.points[-1].positions rospy.loginfo("Virtual Arm State Updated") print self.virtual_arm_state.joint_state.position elif len(trajectory.joint_trajectory.points[-1].positions) == 2: self.virtual_gripper_state.joint_state.name = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.virtual_gripper_state.joint_state.position = trajectory.joint_trajectory.points[-1].positions rospy.loginfo("Virtual Gripper State Updated") print self.virtual_gripper_state.joint_state.position def clear_virtual_arm_state(self): self.virtual_arm_state.joint_state.name = [] self.virtual_arm_state.joint_state.position = [] def clear_virtual_gripper_state(self): self.virtual_gripper_state.joint_state.names = [] self.virtual_gripper_state.joint_state.position = [] def get_virtual_arm_state(self): virtual_arm_state = copy.deepcopy(self.virtual_arm_state) return virtual_arm_state def get_virtual_gripper_state(self): virtual_gripper_state = copy.deepcopy(self.virtual_gripper_state) return virtual_gripper_state
class GraspingClient(object): def __init__(self, sim=False): self.scene = PlanningSceneInterface("base_link") self.dof = rospy.get_param('~jaco_dof') self.robot = moveit_commander.RobotCommander() self._listener = tf.TransformListener() # self.frame = self._listener.lookupTransform('/left_ee_link', '/base_link', rospy.Time(0)) # rospy.loginfo("========== Left_ee to base_link is: ") # rospy.loginfo(self.frame) # curr_pos = self._listener.lookupTransform('/base_link', '/left_ee_link', rospy.Time(0)) # rospy.loginfo(curr_pos) # self.move_group = moveit_commander.MoveGroupCommander("upper_body") # rospy.loginfo("======upper_body_group connected =========") # rospy.loginfo(self.move_group.get_joints()) # self.lmove_group = moveit_commander.MoveGroupCommander("left_arm") # rospy.loginfo("======left_arm_group connected =========") # self.rmove_group = moveit_commander.MoveGroupCommander("right_arm") # rospy.loginfo("======right_arm_group connected =========") self.move_group = MoveGroupInterface("upper_body", "base_link") self.lmove_group = MoveGroupInterface("left_arm", "base_link") self.rmove_group = MoveGroupInterface("right_arm", "base_link") self.move_group.setPlannerId("RRTConnectkConfigDefault") self.lmove_group.setPlannerId("RRTConnectkConfigDefault") self.rmove_group.setPlannerId("RRTConnectkConfigDefault") if "6dof" == self.dof: rospy.logwarn( "======= Please change launch param to 7DoF =========") elif "7dof" == self.dof: self._upper_body_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint" ] self.tucked = [ -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7, 0.0, -0.5, 1.7, 0.04, 0, 0 ] # self.constrained_stow =[-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0] # flipping pose self.constrained_stow = [ -0.23, -0.71, -1.02, -1.0, 0.9, 1.89, -2.41, 0.44, 0.71, 1.12, 1.21, -1.02, -1.84, 2.61, 0.20, 0, 0 ] # self.constrained_stow_2 = [-0.34, -0.66, -0.92, -1.18, 0.94, 1.75, -2.49, 0.54, 0.65, 1.0, 1.35, -1.07, -1.75, 2.67, 0.20, 0, 0] self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0] self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0] self.tableDist = 0.8 else: rospy.logerr("DoF needs to be set 6 or 7, aborting demo") return ''' pickplace = [lg_itf, rg_itf] pick_result = [None, None] ''' self.pickplace = [None] * 2 self.pickplace[0] = PickPlaceInterface("left_side", "left_gripper", verbose=True) self.pickplace[0].planner_id = "RRTConnectkConfigDefault" self.pickplace[1] = PickPlaceInterface("right_side", "right_gripper", verbose=True) self.pickplace[1].planner_id = "RRTConnectkConfigDefault" self.pick_result = [None] * 2 self._last_gripper_picked = 0 self.place_result = [None] * 2 self._last_gripper_placed = 0 self._objs_to_keep = [] self._lgripper = GripperActionClient('left') self._rgripper = GripperActionClient('right') find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() rospy.loginfo( "============= FindGraspableObjectsAction connected! ============") self.scene.clear() # This is a simulation so need to adjust gripper parameters if sim: self._gripper_closed = 0.96 self._gripper_open = 0.00 else: self._gripper_closed = 0.01 self._gripper_open = 0.165 def add_objects_to_keep(self, obj): self._objs_to_keep.append(obj) def clearScene(self): self.scene.clear() def getPickCoordinates(self): self.updateScene(0, False) nut, grasps = self.getGraspableNut(False) bolt, grasps = self.getGraspableBolt(False) if (None == nut) or (None == bolt): rospy.loginfo("======= No nut or bolt found ========") return None center_objects = (nut.primitive_poses[0].position.y + bolt.primitive_poses[0].position.y) / 2 surface = self.getSupportSurface(nut.support_surface) tmp1 = surface.primitive_poses[ 0].position.x - surface.primitives[0].dimensions[0] / 2 surface = self.getSupportSurface(bolt.support_surface) tmp2 = surface.primitive_poses[ 0].position.x - surface.primitives[0].dimensions[0] / 2 front_edge = (tmp1 + tmp2) / 2 coords = Pose2D(x=(front_edge - self.tableDist), y=center_objects, theta=0.0) return coords def getGBCoordinates(self): ''' Get gearbox pos from arv marker ''' pass def updateScene(self, gripper=0, plan=True): # find objects rospy.loginfo("Updating scene...") goal = FindGraspableObjectsGoal() goal.plan_grasps = plan goal.gripper = gripper # send gripper to FindGraspableObject self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) # return graspable object: objects, grasps(moveit) find_result = self.find_client.get_result() rospy.loginfo("=========== before updating, find_result.objects is: ") rospy.loginfo(find_result.objects) # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, True) # insert objects to scene idx = -1 # result.object = GraspableObject = object, grasps # obj.object = grasp_msgs.object for obj in find_result.objects: if obj.object.primitive_poses[ 0].position.z < 0.5 or obj.object.primitive_poses[ 0].position.x > 2.0 or obj.object.primitive_poses[ 0].position.y > 0.5: continue idx += 1 obj.object.name = "object%d_%d" % (idx, gripper) self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=True) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view if obj.primitive_poses[0].position.z < 0.5: continue height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], obj.primitives[0].dimensions[1], # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=True) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableNut(self, planned=True): if self.objects == None: rospy.loginfo("============= No self.objects ===========") for obj in self.objects: # need grasps if len(obj.grasps) < 1 and planned: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5 or \ obj.object.primitive_poses[0].position.z > 1.0 or \ obj.object.primitive_poses[0].position.x > 2.0: continue elif (obj.object.primitives[0].type == sp.CYLINDER): if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.102 or \ obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.142: continue elif (obj.object.primitives[0].type == sp.BOX): if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.102 or \ obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.142: continue else: continue print "nut: ", obj.object.primitive_poses[0] return obj.object, obj.grasps # nothing detected return None, None def getGraspableBolt(self, planned=True): for obj in self.objects: # need grasps if len(obj.grasps) < 1 and planned: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5 or \ obj.object.primitive_poses[0].position.z > 1.0 or \ obj.object.primitive_poses[0].position.x > 2.0: continue elif (obj.object.primitives[0].type == sp.CYLINDER): if obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] < 0.21 or \ obj.object.primitives[0].dimensions[sp.CYLINDER_HEIGHT] > 0.28: continue elif (obj.object.primitives[0].type == sp.BOX): if obj.object.primitives[0].dimensions[sp.BOX_Z] < 0.21 or \ obj.object.primitives[0].dimensions[sp.BOX_Z] > 0.28: continue else: continue return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps, gripper=0): success, pick_result = self.pickplace[gripper].pick_with_retry( block.name, grasps, retries=1, support_name=block.support_surface, scene=self.scene) self.pick_result[gripper] = pick_result self._last_gripper_picked = gripper return success def place(self, block, pose_stamped, gripper=0): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result[ gripper].grasp.pre_grasp_posture l.pre_place_approach = self.pick_result[ gripper].grasp.pre_grasp_approach l.post_place_retreat = self.pick_result[ gripper].grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace[gripper].place_with_retry( block.name, places, retries=1, scene=self.scene) self.place_result[gripper] = place_result self._last_gripper_placed = gripper return success def goto_tuck(self): # remove previous objects while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition( self._upper_body_joints, self.tucked, 0.05, max_velocity_scaling_factor=0.3) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def goto_plan_grasp(self): while not rospy.is_shutdown(): try: result = self.move_group.moveToJointPosition( self._upper_body_joints, self.constrained_stow, 0.01, max_velocity_scaling_factor=0.3) except: continue if result.error_code.val == MoveItErrorCodes.SUCCESS: return def goto_plan_grasp_lx(self): plan_lx = PoseStamped() plan_lx.header.frame_id = 'base_link' plan_lx.pose.position.x = 0.71 plan_lx.pose.position.y = 0.435 plan_lx.pose.position.z = 1.34 while not rospy.is_shutdown(): try: result = self.lmove_group.moveToPose(plan_lx, 'left_ee_link', 0.05) except: continue if result.error_code.val == MoveItErrorCodes.SUCCESS: return def left_arm_constrained_stow(self): c1 = Constraints() c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.stamp = rospy.get_rostime() c1.orientation_constraints[0].header.frame_id = "base_link" c1.orientation_constraints[0].link_name = "left_ee_link" c1.orientation_constraints[0].orientation.w = 1.0 c1.orientation_constraints[ 0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2 c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28 c1.orientation_constraints[0].weight = 1.0 while not rospy.is_shutdown(): result = self.lmove_group.moveToJointPosition( self._left_arm_joints, self.larm_const_stow, 0.05, path_constraints=c1, planning_time=120.0) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def right_arm_constrained_stow(self): c1 = Constraints() c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.stamp = rospy.get_rostime() c1.orientation_constraints[0].header.frame_id = "base_link" c1.orientation_constraints[0].link_name = "right_ee_link" c1.orientation_constraints[0].orientation.w = 1.0 c1.orientation_constraints[ 0].absolute_x_axis_tolerance = 0.2 #x axis is pointed up for wrist link c1.orientation_constraints[0].absolute_y_axis_tolerance = 0.2 c1.orientation_constraints[0].absolute_z_axis_tolerance = 6.28 c1.orientation_constraints[0].weight = 1.0 while not rospy.is_shutdown(): result = self.rmove_group.moveToJointPosition( self._right_arm_joints, self.rarm_const_stow, 0.05, path_constraints=c1, planning_time=120.0) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def open_gripper(self): self._lgripper.command(self._gripper_open, block=True) self._rgripper.command(self._gripper_open, block=True) def close_gripper(self): self._lgripper.command(self._gripper_closed, block=True) self._rgripper.command(self._gripper_closed, block=True)
class GripperController(object): def __init__(self): self.client = actionlib.SimpleActionClient( "gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_action...") self.move_group = MoveGroupInterface("arm_with_torso", "base_link") self.goal = GripperCommandGoal() self.client.wait_for_server() def close(self): '''Close the gripper''' self.goal.command.position = 0 self.goal.command.max_effort = 100 self.client.send_goal(self.goal) self.client.wait_for_result() def release(self): '''Release the gripper''' self.goal.command.position = 1 self.goal.command.max_effort = 100 self.client.send_goal(self.goal) self.client.wait_for_result() def tuck(self): '''Tuck the arm of fetch''' position = (0.0555, -0.138, 0.571) quaternion = (0.45848, -0.50438, 0.5078, 0.5268) self.move_to_pose(position, quaternion) def move_to_pose(self, postion, quaternion, frame='base_link'): target_frame = 'wrist_roll_link' pose_stamped = PoseStamped() pose_stamped.header.frame_id = frame pose_stamped.header.stamp = rospy.Time.now() pose_stamped.pose = Pose( Point(postion[0], postion[1], postion[2]), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) if not rospy.is_shutdown(): self.move_group.moveToPose(pose_stamped, target_frame) result = self.move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Move success!") else: rospy.logerr("in state: %s", self.move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # self.move_group.get_move_action().cancel_all_goals() def move_to_some_pose(self, postion, direction, frame='base_link'): target_frame = 'wrist_roll_link' quatrenion = Transformer().quaternion_from_direction((1, 0, 0), direction) pose_stamped = PoseStamped() pose_stamped.header.frame_id = frame pose_stamped.header.stamp = rospy.Time.now() pose_stamped.pose = Pose(Point(postion[0], postion[1], postion[2]), quatrenion) if not rospy.is_shutdown(): self.move_group.moveToPose(pose_stamped, target_frame) result = self.move_group.get_move_action().get_result() if result: if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("See!") else: rospy.logerr("in state: %s", self.move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") self.move_group.get_move_action().cancel_all_goals() def wave(self): '''Just for fun''' gripper_poses = [ ((0.347, 0.045, 1.022), (-0.174, -0.301, 0.273, 0.635)), ((0.347, 0.245, 1.022), (-0.274, -0.701, 0.173, 0.635)) ] for i in range(5): for pose in gripper_poses: self.move_to_pose(pose[0], pose[1]) self.stop() def stop(self): """This stops all arm movement goals It should be called when a program is exiting so movement stops """ self.move_group.get_move_action().cancel_all_goals()
class Sensory_data_collection: def __init__(self): rospy.init_node('pnp', anonymous=True) self.both_arms_group = MoveGroupInterface("both_arms", "base") self.right_arm_group = MoveGroupInterface("right_arm", "base") self.left_arm_group = MoveGroupInterface("left_arm", "base") self.leftgripper = baxter_interface.Gripper('left') self.rightgripper = baxter_interface.Gripper('right') self.leftgripper.calibrate() self.leftgripper.open() self.rightgripper.calibrate() self.rightgripper.open() self.both_arm_joints = [ 'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0', 'left_s1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2', 'right_e0', 'right_e1' ] self.right_joints = [ 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2', 'right_e0', 'right_e1' ] self.left_joints = [ 'left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0', 'left_s1' ] self.init_joint_angles = [ 0.2515728492132078, 1.58958759144626, 3.0418839023767754, -1.2862428906419194, 1.2916118233995184, 0.37735927381981177, -1.3203739631723699, -0.1514806028036846, -1.2908448330055757, 2.908044078633773, -1.5424176822187836, -0.34131072530450457, 0.984048675428493, 1.1949710337627373 ] self.preright = [ 0.27381557063754636, -0.7309418454273996, 2.269908070873441, -1.0181797479589434, 0.9177040063524488, 1.1213399559442374, 1.878359474765689 ] self.preleft = [ 0.7294078646395142, 1.1850001586414822, 1.957359485341788, -0.9821311994436361, 1.5397332158399841, -0.7742768026851626, -0.7566360236244803 ] self.pickgoal = PoseStamped() self.pickgoal.header.frame_id = "base" self.pickgoal.header.stamp = rospy.Time.now() self.pickgoal.pose.position.x = 0.57 self.pickgoal.pose.position.y = -0.1 self.pickgoal.pose.position.z = -0.05 self.pickgoal.pose.orientation.x = 1.0 self.pickgoal.pose.orientation.y = 0.0 self.pickgoal.pose.orientation.z = 0.0 self.pickgoal.pose.orientation.w = 0.0 rospy.Subscriber('/joy', Joy, self.joystick_callback) def joystick_callback(self, data): if data.buttons[3] == 1: arm = 'left' self.pick_and_place(arm) elif data.buttons[1] == 1: arm = 'left' #shake elif data.buttons[4] == 1: arm = 'left' self.pick_and_drop(arm) elif data.buttons[0] == 1: arm = 'left' #push def pick_and_drop(self, arm): # leftLimb = baxter_interface.Limb('left') # rightLimb = baxter_interface.Limb('right') # print leftLimb.joint_angles() # print " " # print rightLimb.joint_angles() # ''' # Move both arms to start state self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) #pick and drop if arm == 'left': self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) rospy.sleep(1) self.left_arm_group.moveToPose(self.pickgoal, "left_gripper", plan_only=False) self.leftgripper.close() rospy.sleep(3) self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) rospy.sleep(1) self.leftgripper.open() else: self.right_arm_group.moveToJointPosition(self.right_joints, self.preright, plan_only=False) rospy.sleep(1) self.right_arm_group.moveToPose(self.pickgoal, "right_gripper", plan_only=False) self.rightgripper.close() rospy.sleep(3) self.right_arm_group.moveToJointPosition(self.right_joints, self.preright, plan_only=False) rospy.sleep(1) self.rightgripper.open() rospy.sleep(1) self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) def pick_and_place(self, arm): # leftLimb = baxter_interface.Limb('left') # rightLimb = baxter_interface.Limb('right') # print leftLimb.joint_angles() # print " " # print rightLimb.joint_angles() # ''' # Move both arms to start state self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) #pick if arm == 'left': self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) rospy.sleep(1) self.left_arm_group.moveToPose(self.pickgoal, "left_gripper", plan_only=False) self.leftgripper.close() else: self.right_arm_group.moveToJointPosition(self.right_joints, self.preright, plan_only=False) rospy.sleep(1) self.right_arm_group.moveToPose(self.pickgoal, "right_gripper", plan_only=False) self.rightgripper.close() rospy.sleep(1) self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) rospy.sleep(1) #place if arm == 'left': self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) self.left_arm_group.moveToPose(self.pickgoal, "left_gripper", plan_only=False) self.leftgripper.open() self.left_arm_group.moveToJointPosition(self.left_joints, self.preleft, plan_only=False) self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False) else: self.right_arm_group.moveToJointPosition(self.right_joints, self.preright, plan_only=False) self.right_arm_group.moveToPose(self.pickgoal, "right_gripper", plan_only=False) self.rightgripper.open() self.right_arm_group.moveToJointPosition(self.right_joints, self.preright, plan_only=False) self.both_arms_group.moveToJointPosition(self.both_arm_joints, self.init_joint_angles, plan_only=False)
def clasificar(): adaptative = True pr_b = False precision = 0.7 # Can free memory? pub = rospy.Publisher("finished", String, queue_size=10) # Initialize MoveIt! scene p = PlanningSceneInterface("base") arms_group = MoveGroupInterface("both_arms", "base") rightarm_group = MoveGroupInterface("right_arm", "base") leftarm_group = MoveGroupInterface("left_arm", "base") # Create right arm instance right_arm = baxter_interface.limb.Limb('right') # Create right gripper instance right_gripper = baxter_interface.Gripper('right') right_gripper.calibrate() right_gripper.open() right_gripper.close() offset_zero_point = 0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 table_distance_from_gripper = -offset_zero_point + table_size_z + 0.0275 / 2 j = 0 k = 0 # Initialize object list objlist = [ 'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11' ] p.clear() p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() # Move both arms to start state. # Initial pos rpos = PoseStamped() rpos.header.frame_id = "base" rpos.header.stamp = rospy.Time.now() rpos.pose.position.x = 0.555 rpos.pose.position.y = 0.0 rpos.pose.position.z = 0.206 rpos.pose.orientation.x = 1.0 rpos.pose.orientation.y = 0.0 rpos.pose.orientation.z = 0.0 rpos.pose.orientation.w = 0.0 lpos = PoseStamped() lpos.header.frame_id = "base" lpos.header.stamp = rospy.Time.now() lpos.pose.position.x = 0.65 lpos.pose.position.y = 0.6 lpos.pose.position.z = 0.206 lpos.pose.orientation.x = 1.0 lpos.pose.orientation.y = 0.0 lpos.pose.orientation.z = 0.0 lpos.pose.orientation.w = 0.0 while True: # Move to initial position rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False) # Get the middle point locs = PoseArray() locs = rospy.wait_for_message("clasificacion", PoseArray) if (len(locs.poses) != 0): punto_medio = PoseStamped() punto_medio.header.frame_id = "base" punto_medio.header.stamp = rospy.Time.now() punto_medio.pose.position.x = locs.poses[0].position.x punto_medio.pose.position.y = locs.poses[0].position.y punto_medio.pose.position.z = locs.poses[0].position.z punto_medio.pose.orientation.x = locs.poses[0].orientation.x punto_medio.pose.orientation.y = locs.poses[0].orientation.y punto_medio.pose.orientation.z = locs.poses[0].orientation.z punto_medio.pose.orientation.w = locs.poses[0].orientation.w alfa = 0.1 # Two parameters: # When pr_b == 1, the program will try to adapt the trajectory for getting the precision established in the parameter of the same name # When adaptative == 1, the program will try to get the best precision based on the precision of the last execution. if (pr_b and not adaptative): if precision >= 1: punto_medio.pose.position.x += 0.01 else: punto_medio.pose.position.x -= alfa * (1 - precision) else: print("If it is the first time executing, write -1") precision_value = input() if precision_value != -1.0: punto_medio.pose.position.x += alfa * (1 - precision_value) elif precision_value == 1.0: adaptative = False # Get the normal orientation for separating the objects orient = (-1) * (1 / punto_medio.pose.orientation.x) punto_medio.pose = rotate_pose_msg_by_euler_angles( punto_medio.pose, 0.0, 0.0, orient) rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) orient = 1.57 - orient punto_medio.pose = rotate_pose_msg_by_euler_angles( punto_medio.pose, 0.0, 0.0, orient) rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) punto_medio.pose.position.x += 0.15 rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) time.sleep(1) punto_medio.pose.position.x -= 0.3 rightarm_group.moveToPose(punto_medio, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) time.sleep(1) rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) # Free the memory when finished freemem = "free" if not adaptative: pub.publish(freemem) else: sys.exit("Cannot identify any object")
def dyn_wedge(): pub = rospy.Publisher("finished", String, queue_size=10) # Initialize MoveIt scene p = PlanningSceneInterface("base") arms_group = MoveGroupInterface("both_arms", "base") rightarm_group = MoveGroupInterface("right_arm", "base") leftarm_group = MoveGroupInterface("left_arm", "base") # Create right arm instance right_arm = baxter_interface.limb.Limb('right') # Create right gripper instance right_gripper = baxter_interface.Gripper('right') right_gripper.calibrate() right_gripper.open() right_gripper.close() offset_zero_point=0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 table_distance_from_gripper = -offset_zero_point+table_size_z+0.0275/2 j=0 k=0 # Initialize object list objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] p.clear() p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) p.waitForSync() # Move both arms to start state. # Initial pose rpos = PoseStamped() rpos.header.frame_id = "base" rpos.header.stamp = rospy.Time.now() rpos.pose.position.x = 0.555 rpos.pose.position.y = 0.0 rpos.pose.position.z = 0.206 #0.18 rpos.pose.orientation.x = 1.0 rpos.pose.orientation.y = 0.0 rpos.pose.orientation.z = 0.0 rpos.pose.orientation.w = 0.0 lpos = PoseStamped() lpos.header.frame_id = "base" lpos.header.stamp = rospy.Time.now() lpos.pose.position.x = 0.555 lpos.pose.position.y = 0.65 lpos.pose.position.z = 0.206#0.35 lpos.pose.orientation.x = 1.0 lpos.pose.orientation.y = 0.0 lpos.pose.orientation.z = 0.0 lpos.pose.orientation.w = 0.0 rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) leftarm_group.moveToPose(lpos, "left_gripper", max_velocity_scaling_factor=1, plan_only=False) # Get the middle point between the two centroids locs = rospy.wait_for_message("clasificacion", PoseArray) if(len(locs.poses)!=0): punto_medio1 = PoseStamped() punto_medio1.header.frame_id = "base" punto_medio1.header.stamp = rospy.Time.now() punto_medio1.pose.position.x = locs.poses[0].position.x punto_medio1.pose.position.y = locs.poses[0].position.y punto_medio1.pose.position.z = -0.08 punto_medio1.pose.orientation.x = locs.poses[0].orientation.x punto_medio1.pose.orientation.y = locs.poses[0].orientation.y punto_medio1.pose.orientation.z = locs.poses[0].orientation.z punto_medio1.pose.orientation.w = locs.poses[0].orientation.w else: sys.exit("No se encuentran los puntos") # Get the middle point again after a few seconds tiempo = 3 time.sleep(tiempo) locs = rospy.wait_for_message("clasificacion", PoseArray) if(len(locs.poses)!=0): punto_medio2 = PoseStamped() punto_medio2.header.frame_id = "base" punto_medio2.header.stamp = rospy.Time.now() punto_medio2.pose.position.x = locs.poses[0].position.x punto_medio2.pose.position.y = locs.poses[0].position.y punto_medio2.pose.position.z = -0.08 punto_medio2.pose.orientation.x = locs.poses[0].orientation.x punto_medio2.pose.orientation.y = locs.poses[0].orientation.y punto_medio2.pose.orientation.z = locs.poses[0].orientation.z punto_medio2.pose.orientation.w = locs.poses[0].orientation.w else: sys.exit("No se encuentran los puntos") # Calculate speed of objects vel = (punto_medio2.pose.position.y - punto_medio1.pose.position.y) / tiempo # Predict position within a certain time nuevo_tiempo = 2.1 posicion = nuevo_tiempo * vel * 2 if(posicion>=0): nueva_y = punto_medio2.pose.position.y - posicion else: nueva_y = punto_medio2.pose.position.y + posicion orientacion = (-1) * 1/punto_medio2.pose.orientation.x new_or = 0.26 if orientacion > 0: new_or = -0.26 # If there is time enough to sweep the objects if vel > -0.08: start = time.time() punto_medio2.pose.position.y = nueva_y - nueva_y/2 punto_medio2.pose.position.x = punto_medio1.pose.position.x - 0.03 punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion - new_or) rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) end = time.time() tiempo = end - start while(nuevo_tiempo - tiempo >= 1): end = time.time() nuevo_tiempo = end - start else: punto_medio2.pose.position.y = nueva_y print(punto_medio2.pose.position.y) punto_medio2.pose.position.x = punto_medio1.pose.position.x punto_medio2.pose.position.y += 0.05 punto_medio2.pose = rotate_pose_msg_by_euler_angles(punto_medio2.pose, 0.0, 0.0, orientacion) rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) obj = punto_medio2.pose.position.y + 0.16 neg = 1 while punto_medio2.pose.position.y < obj: punto_medio2.pose.position.y += 0.03 # Shake arm punto_medio2.pose.position.x += neg * 0.09 rightarm_group.moveToPose(punto_medio2, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) neg = (-1)*neg time.sleep(2) rightarm_group.moveToPose(rpos, "right_gripper", max_velocity_scaling_factor=1, plan_only=False) freemem = "free" pub.publish(freemem)
try: t = listener.getLatestCommonTime(frame, "/map") trans, rot = listener.lookupTransform(frame, "/map", t) pos.append(trans) ori.append(rot) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): break if len(pos) == 8: pos = np.array(pos) acc = MotionPolicy(oldStates, pos, obstacle, goal) oldStates = pos acc = np.multiply(acc, ((rospy.get_time() - time)**2) / 2.0) time = rospy.get_time() poses = np.add(pos, acc) pose = poses[-2] rot = ori[-2] gripper_pose = Pose(Point(pose[0], pose[1], pose[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = "/map" gripper_pose_stamped.header.stamp = t gripper_pose_stamped.pose = gripper_pose move_group.moveToPose(gripper_pose_stamped, frames[-2][1:]) rate.sleep() move_group.get_move_action().cancel_all_goals()
class screw_1(object): def __init__(self): # self.scene = PlanningSceneInterface("base_link") self.dof = "7dof" self.robot = moveit_commander.RobotCommander() self._listener = tf.TransformListener() self.move_group = MoveGroupInterface("upper_body", "base_link") self.lmove_group = MoveGroupInterface("left_arm", "base_link") self.rmove_group = MoveGroupInterface("right_arm", "base_link") self.move_group.setPlannerId("RRTConnectkConfigDefault") self.lmove_group.setPlannerId("RRTConnectkConfigDefault") self.rmove_group.setPlannerId("RRTConnectkConfigDefault") if "7dof" == self.dof: self._upper_body_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint" ] self.tucked = [ -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7, 0.0, -0.5, 1.7, 0.04, 0, 0 ] # self.constrained_stow =[-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0] self.constrained_stow = [ -2.037, 1.046, -2.877, -1.423, -1.143, 1.679, 1.690, 2.037, -1.046, 2.877, 1.423, 1.143, -1.679, -1.690, 0.4, 0.288, 0 ] else: rospy.logerr("DoF needs to be set at 7, aborting demo") return self._lgripper = GripperActionClient('left') self._rgripper = GripperActionClient('right') # self.scene.clear() def goto_tuck(self): # remove previous objects raw_input("========== Press Enter to goto_tuck ========") while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition( self._upper_body_joints, self.tucked, 0.05, max_velocity_scaling_factor=0.4) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def goto_plan_grasp(self): raw_input("========== Press Enter to goto_plan_grasp ========") while not rospy.is_shutdown(): try: result = self.move_group.moveToJointPosition( self._upper_body_joints, self.constrained_stow, 0.05, max_velocity_scaling_factor=0.3) except: continue if result.error_code.val == MoveItErrorCodes.SUCCESS: return def insert_bolt(self): # right arm # base_link y+ pose = self.curr_pose('right_ee_link') pose.pose.position.y += 0.05 raw_input("=========== Press Enter to insert bolt ========") # rospy.sleep(2.0) while not rospy.is_shutdown(): try: result = self.rmove_group.moveToPose( pose, "right_ee_link", tolerance=0.02, max_velocity_scaling_factor=0.1) except: continue if result.error_code.val == MoveItErrorCodes.SUCCESS: return def insert_nut(self): pass def start(self): pass def retreat(self): pass def curr_pose(self, eef): pose = PoseStamped() pose.header.frame_id = eef curr_pose = self._listener.transformPose('/base_link', pose) return curr_pose
def picknplace(): # Define initial parameters p = PlanningSceneInterface("base") g = MoveGroupInterface("both_arms", "base") gr = MoveGroupInterface("right_arm", "base") gl = MoveGroupInterface("left_arm", "base") leftgripper = baxter_interface.Gripper('left') leftgripper.calibrate() leftgripper.open() # jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] # jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] # jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] # pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] # pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] # lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519] # lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384] # rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] # rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] # Clear planning scene # p.clear() # # Add table as attached object # p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) # Move both arms to start state # g.moveToJointPosition(jts_both, pos1, plan_only=False) # # Get obj locations # temp = rospy.wait_for_message("Dpos", PoseArray) # locs = temp.poses # locs_x = [] # locs_y = [] # orien = [] # size = [] # for i in range(len(locs)): # locs_x.append(0.57 + locs[i].position.x) # locs_y.append(-0.011 + locs[i].position.y) # orien.append(locs[i].position.z*pi/180) # size.append(locs[i].orientation.x) # # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene # ind_rmv = [] # for i in range(0,len(locs)): # if (locs_y[i] > 0.42): # ind_rmv.append(i) # continue # for j in range(i,len(locs)): # if not (i == j): # if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: # ind_rmv.append(i) # locs_x = del_meth(locs_x, ind_rmv) # locs_y = del_meth(locs_y, ind_rmv) # orien = del_meth(orien, ind_rmv) # size = del_meth(size, ind_rmv) # # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes # if locs_x: # ig0 = itemgetter(0) # sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) # locs_x = list(sorted_lists[1]) # locs_y = list(sorted_lists[2]) # orien = list(sorted_lists[3]) # size = list(sorted_lists[0]) # # Loop to continue pick and place until all objects are cleared from table # j=0 # k=0 # while locs_x: # p.clear() # CLear planning scene of all collision objects # # Attach table as attached collision object to base of baxter # p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) # xn = locs_x[0] # yn = locs_y[0] # zn = -0.06 # thn = orien[0] # sz = size[0] # if thn > pi/4: # thn = -1*(thn%(pi/4)) # # Add collision objects into planning scene # objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] # for i in range(1,len(locs_x)): # p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05) # p.waitForSync() # # Move both arms to pos2 (Right arm away and left arm on table) # g.moveToJointPosition(jts_both, pos2, plan_only=False) # Move left arm to pick object and pick object # pickgoal = PoseStamped() # pickgoal.header.frame_id = "base" # pickgoal.header.stamp = rospy.Time.now() # pickgoal.pose.position.x = xn # pickgoal.pose.position.y = yn # pickgoal.pose.position.z = zn+0.1 # pickgoal.pose.orientation.x = 1.0 # pickgoal.pose.orientation.y = 0.0 # pickgoal.pose.orientation.z = 0.0 # pickgoal.pose.orientation.w = 0.0 goal_pose = PoseStamped() limb = baxter_interface.Limb('left') gripper_pose = limb.endpoint_pose() goal_pose.pose.position.x = gripper_pose['position'][0] #0.63 goal_pose.pose.position.y = gripper_pose['position'][1] #0.19519 goal_pose.pose.position.z = gripper_pose['position'][2] + 0.1 #0.05 goal_pose.pose.orientation.x = gripper_pose['orientation'][0] goal_pose.pose.orientation.y = gripper_pose['orientation'][1] goal_pose.pose.orientation.z = gripper_pose['orientation'][2] goal_pose.pose.orientation.w = gripper_pose['orientation'][3] gl.moveToPose(goal_pose, "left_gripper", plan_only=False)
def picknplace(): # Initialize the planning scene interface. p = PlanningSceneInterface("base") # Connect the arms to the move group. g = MoveGroupInterface("both_arms", "base") gr = MoveGroupInterface("right_arm", "base") gl = MoveGroupInterface("left_arm", "base") # Create baxter_interface limb instance. leftarm = baxter_interface.limb.Limb('left') # Create baxter_interface gripper instance. leftgripper = baxter_interface.Gripper('left') leftgripper.calibrate() leftgripper.open() # Define the joints for the positions. jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] pos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] lpos1 = [-1.4580487388850858, 1.627553615946424, 0.07669903939427068, -0.3539660668045592, -1.9155585088719105, -1.4124128104454947, -0.6438884357149024] lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384] rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] placegoal = PoseStamped() placegoal.header.frame_id = "base" placegoal.header.stamp = rospy.Time.now() placegoal.pose.position.x = 0.55 placegoal.pose.position.y = 0.28 placegoal.pose.position.z = 0 placegoal.pose.orientation.x = 1.0 placegoal.pose.orientation.y = 0.0 placegoal.pose.orientation.z = 0.0 placegoal.pose.orientation.w = 0.0 # Define variables. offset_zero_point = 0.903 table_size_x = 0.714655654394 table_size_y = 1.05043717328 table_size_z = 0.729766045265 center_x = 0.457327827197 center_y = 0.145765166941 center_z = -0.538116977368 # The distance from the zero point in Moveit to the ground is 0.903 m. # The value is not allways the same. (look in Readme) center_z_cube= -offset_zero_point+table_size_z+0.0275/2 pressure_ok=0 j=0 k=0 start=1 locs_x = [] # Initialize a list for the objects and the stacked cubes. objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] boxlist= ['box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08', 'box09', 'box10', 'box11'] # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) # Move both arms to start state where the right camera looks for objects. g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False) time.sleep(0.5) # cProfile to measure the performance (time) of the task. pr = cProfile.Profile() pr.enable() # Loop to continue pick and place until all objects are cleared from table. while locs_x or start: # Only for the start. if start: start = 0 # Receive the data from all objects from the topic "detected_objects". temp = rospy.wait_for_message("detected_objects", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] # Adds the data from the objects. for i in range(len(locs)): locs_x.append(locs[i].position.x) locs_y.append(locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected locations for same objects. ind_rmv = [] for i in range(0,len(locs) ): if (locs_y[i] > 0.24 or locs_x[i] > 0.75): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Do the task only if there are still objects on the table. if locs_x: # Clear planning scene. p.clear() # Add table as attached object. p.attachBox('table', table_size_x, table_size_y, table_size_z, center_x, center_y, center_z, 'base', touch_links=['pedestal']) # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes. ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Initialize the data of the biggest object on the table. xn = locs_x[0] yn = locs_y[0] zn = -0.16 thn = orien[0] sz = size[0] if thn > pi/4: thn = -1*(thn%(pi/4)) # Add the detected objects into the planning scene. #for i in range(1,len(locs_x)): #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube) # Add the stacked objects as collision objects into the planning scene to avoid moving against them. #for e in range(0, k): #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes']) if k>0: p.attachBox(boxlist[0], 0.07, 0.07, 0.0275*k, placegoal.pose.position.x, placegoal.pose.position.y, center_z_cube, 'base', touch_links=['cubes']) p.waitForSync() # Move both arms to the second position. g.moveToJointPosition(jts_both, pos2, max_velocity_scaling_factor = 1, plan_only=False) # Initialize the pickgoal. pickgoal = PoseStamped() pickgoal.header.frame_id = "base" pickgoal.header.stamp = rospy.Time.now() pickgoal.pose.position.x = xn pickgoal.pose.position.y = yn pickgoal.pose.position.z = zn pickgoal.pose.orientation.x = 1.0 pickgoal.pose.orientation.y = 0.0 pickgoal.pose.orientation.z = 0.0 pickgoal.pose.orientation.w = 0.0 # Move to the approach pickgoal. (5 cm to pickgoal) pickgoal.pose.position.z = zn+0.05 # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles. pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn) gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) pickgoal.pose.position.z = zn # Move to the pickgoal. gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.3, plan_only=False) # Read the force in z direction. b=leftarm.endpoint_effort() z_= b['force'] z_force= z_.z # Search again for objects, if the gripper isn't at the right position and presses on an object. #print("----->force in z direction:", z_force) if z_force>-4: leftgripper.close() attempts=0 pressure_ok=1 # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects. while(leftgripper.force()<25 and pressure_ok==1): time.sleep(0.04) attempts+=1 if(attempts>50): leftgripper.open() pressure_ok=0 print("----->pressure is to low<-----") else: print("----->gripper presses on an object<-----") # Move back to the approach pickgoal. pickgoal.pose.position.z = zn+0.05 gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) # Move both arms to position 1. g.moveToJointPosition(jts_both, pos1, max_velocity_scaling_factor = 1, plan_only=False) if pressure_ok and z_force>-4: # Define the approach placegoal. # Increase the height of the tower every time by 2.75 cm. placegoal.pose.position.z =-0.145+(k*0.0275)+0.08 # Move to the approach pickgoal. gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) # Define the placegoal. placegoal.pose.position.z =-0.145+(k*0.0275) gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) leftgripper.open() k += 1 # Move to the approach placegoal. placegoal.pose.position.z = -0.145+(k*0.0275)+0.08 gl.moveToPose(placegoal, "left_gripper", max_velocity_scaling_factor = 1, plan_only=False) # Move left arm to start position pos1. gl.moveToJointPosition(jts_left, lpos1, max_velocity_scaling_factor = 1, plan_only=False) pr.disable() sortby = 'cumulative' ps=pstats.Stats(pr).sort_stats(sortby).print_stats(0.0) p.clear()
Quaternion(-0.374, -0.701, 0.173, 0.635)) # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' while not rospy.is_shutdown(): # Finish building the Pose_stamped message # If the message stamp is not current it could be ignored gripper_pose_stamped.header.stamp = rospy.Time.now() # Set the message pose gripper_pose_stamped.pose = pose # Move gripper frame to the pose specified move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Hello there!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals
class TrajectorySimulator(): def __init__(self): # Instantiate a RobotCommander object. # This object is an interface to the robot as a whole. #robot = moveit_commander.RobotCommander() # Instantiate a MoveGroupCommander object. # This object is an interface to one 'group' of joints, # in our case the joints of the arm. self.group = MoveGroupInterface("arm", "base_link") # Create DisplayTrajectory publisher to publish trajectories # for RVIZ to visualize. # self.display_trajectory_publisher = rospy.Publisher( #"/move_group/display_planned_path", #moveit_msgs.msg.DisplayTrajectory) # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(15) print "============ Starting tutorial " def get_state_info(self): # Let's get some basic information about the robot. # This info may be helpful for debugging. # Get name of reference frame for this robot print "============ Reference frame: %s" % self.group.get_planning_frame() # Get name of end-effector link for this group print "============ Reference frame: %s" % self.group.get_end_effector_link() # List all groups defined on the robot print "============ Robot Groups:" print self.robot.get_group_names() # Print entire state of the robot print "============ Printing robot state" print self.robot.get_current_state() print "============" def plan_to_pose_goal(self, goal): ### Planning to a Pose Goal # Let's plan a motion for this group to a desired pose for the # end-effector print "============ Generating plan 1" # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose(pose_target, "gripper_link", tolerance = 0.3, plan_only=False) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" return # Now we call the planner to compute the plan and visualize it if successful. # We are just planning here, not asking move_group to actually move the robot. print "============ Waiting while RVIZ displays plan1..." rospy.sleep(5) print "============ Goal achieved" # group.plan() automatically asks RVIZ to visualize the plan, # so we need not explicitly ask RVIZ to do this. # For completeness of understanding, however, we make this # explicit request below. The same trajectory should be displayed # a second time. print "============ Visualizing plan1" """ display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state() display_trajectory.trajectory.append(plan1) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while plan1 is visualized (again)..." rospy.sleep(5) """ def plan_to_jointspace_goal(self, pose): # Let's set a joint-space goal and visualize it in RVIZ # A joint-space goal differs from a pose goal in that # a goal for the state of each joint is specified, rather # than just a goal for the end-effector pose, with no # goal specified for the rest of the arm. joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose, 0.1) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" rospy.sleep(5) return "Success"
class Grasping(object): def __init__(self): self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.scene = PlanningSceneInterface("base_link") self.move_group = MoveGroupInterface("arm", "base_link") find_objects = "basic_grasping_perception/find_objects" self.find_client = actionlib.SimpleActionClient( find_objects, FindGraspableObjectsAction) self.find_client.wait_for_server() def pickup(self, block, grasps): rospy.sleep(1) success, pick_result = self.pickplace.pick_with_retry( block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success #swaps two blocks positions. def swapBlockPos(self, block1Pos, block2Pos): #intermediate point for movement self.armIntermediatePose() #intermediate positon on the table, used for short term storage of the first block manipulated in the sorting. posIntermediate = np.array([0.725, 0]) self.armIntermediatePose() # Get block to pick while not rospy.is_shutdown(): rospy.loginfo("Picking object...") self.updateScene() cube, grasps = self.getGraspableObject(block1Pos) if cube == None: rospy.logwarn("Perception failed.") continue # Pick the block if self.pickup(cube, grasps): break rospy.logwarn("Grasping failed.") #self.tuck() #Place the block while not rospy.is_shutdown(): rospy.loginfo("Placing object...") pose = PoseStamped() pose.pose = cube.primitive_poses[0] pose.pose.position.z += 0.05 pose.header.frame_id = cube.header.frame_id if self.place(cube, pose, posIntermediate): break rospy.logwarn("Placing failed.") #place block 2 in block 1's self.armIntermediatePose() # Get block to pick while not rospy.is_shutdown(): rospy.loginfo("Picking object...") self.updateScene() cube, grasps = self.getGraspableObject(block2Pos) if cube == None: rospy.logwarn("Perception failed.") continue # Pick the block if self.pickup(cube, grasps): break rospy.logwarn("Grasping failed.") while not rospy.is_shutdown(): rospy.loginfo("Placing object...") pose = PoseStamped() pose.pose = cube.primitive_poses[0] pose.pose.position.z += 0.05 pose.header.frame_id = cube.header.frame_id if self.place(cube, pose, block1Pos): break rospy.logwarn("Placing failed.") #place block1 in block 2's spot self.armIntermediatePose() while not rospy.is_shutdown(): rospy.loginfo("Picking object...") self.updateScene() cube, grasps = self.getGraspableObject(posIntermediate) if cube == None: rospy.logwarn("Perception failed.") continue # Pick the block if self.pickup(cube, grasps): break rospy.logwarn("Grasping failed.") #self.tuck() self.armIntermediatePose() while not rospy.is_shutdown(): rospy.loginfo("Placing object...") pose = PoseStamped() pose.pose = cube.primitive_poses[0] pose.pose.position.z += 0.05 pose.header.frame_id = cube.header.frame_id if self.place(cube, pose, block2Pos): break rospy.logwarn("Placing failed.") return def place(self, block, pose_stamped, placePos): rospy.sleep(1) #creates a list of place positons for the block, with a specified x, y, and z. places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = "base_link" l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat #this x and y value are input as placePos through the function call. l.place_pose.pose.position.x = placePos[0] l.place_pose.pose.position.y = placePos[1] places.append(copy.deepcopy(l)) #success = self.pickplace.place_with_retry(block.name, places, scene = self.scene) #return success ## create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success def updateScene(self): rospy.sleep(1) # find objectsw goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], use_service=False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], use_service=True) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def getGraspableObject(self, pos): graspable = None for obj in self.objects: if len(obj.grasps) < 1: continue if (obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[1] < 0.05 or \ obj.object.primitives[0].dimensions[1] > 0.07 or \ obj.object.primitives[0].dimensions[2] < 0.05 or \ obj.object.primitives[0].dimensions[2] > 0.07): continue if (obj.object.primitive_poses[0].position.z < 0.3) or \ (obj.object.primitive_poses[0].position.y > pos[1]+0.05) or \ (obj.object.primitive_poses[0].position.y < pos[1]-0.05) or \ (obj.object.primitive_poses[0].position.x > pos[0]+0.05) or \ (obj.object.primitive_poses[0].position.x < pos[0]-0.05): continue return obj.object, obj.grasps return None, None def armToXYZ(self, x, y, z): rospy.sleep(1) #new pose_stamped of the end effector that moves the arm out of the way of the vision for planning. intermediatePose = PoseStamped() intermediatePose.header.frame_id = 'base_link' #position intermediatePose.pose.position.x = x intermediatePose.pose.position.y = y intermediatePose.pose.position.z = z #quaternion for the end position intermediatePose.pose.orientation.w = 1 while not rospy.is_shutdown(): result = self.move_group.moveToPose(intermediatePose, "wrist_roll_link") if result.error_code.val == MoveItErrorCodes.SUCCESS: return def armIntermediatePose(self): self.armToXYZ(0.1, -0.7, 0.9) def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class TrajectoryGenerator: # Wrapper class that facilitates trajectory planning. # Includes methods for planning to arbitrary joint-space goals # and end-effector pose goals. def __init__(self): self.group = MoveGroupInterface("arm", "base_link") self.gripper = MoveGroupInterface("gripper", "base_link") self.virtual_state = RobotState() self.trajectory = None # Sleep to allow RVIZ time to start up. print "============ Waiting for RVIZ..." rospy.sleep(5) print "============ Starting planning" def generate_trajectory(self, goal_poses): for goal_pose in goal_poses: if goal_pose.goal_type is "ee_pose": traj_result = self.plan_to_ee_pose_goal(goal_pose.pose) elif goal_pose.goal_type is "jointspace": traj_result = self.plan_to_jointspace_goal(goal_pose.state) elif goal_pose.goal_type is "gripper_posture": traj_result = self.plan_to_gripper_goal(goal_pose.state) else: rospy.loginfo("Goal type improperly specified. Please specify as either 'ee_pose' or 'jointspace'") sys.exit() approved_trajectories.append(traj_result) return approved_trajectories def plan_to_ee_pose_goal(self, goal): # Extract goal position pose_x = goal[0] pose_y = goal[1] pose_z = goal[2] # Extract goal orientation--provided in Euler angles roll = goal[3] pitch = goal[4] yaw = goal[5] # Convert Euler angles to quaternion, create PoseStamped message quaternion = quaternion_from_euler(roll, pitch, yaw) pose_target = PoseStamped() pose_target.pose.position.x = pose_x pose_target.pose.position.y = pose_y pose_target.pose.position.z = pose_z pose_target.pose.orientation.x = quaternion[0] pose_target.pose.orientation.y = quaternion[1] pose_target.pose.orientation.z = quaternion[2] pose_target.pose.orientation.w = quaternion[3] pose_target.header.frame_id = "base_link" pose_target.header.stamp = rospy.Time.now() print "============= PoseStamped message filled out" # Execute motion while not rospy.is_shutdown(): result = self.group.moveToPose( pose_target, "gripper_link", tolerance=0.02, plan_only=True, start_state=self.virtual_state ) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "================ Pose Achieved" self.trajectory = result.planned_trajectory.joint_trajectory return self.trajectory def plan_to_jointspace_goal(self, pose): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint", ] while not rospy.is_shutdown(): result = self.group.moveToJointPosition(joints, pose, plan_only=True, start_state=self.virtual_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: print "============ Pose Achieved" self.trajectory = result.planned_trajectory # .joint_trajectory return self.trajectory def plan_to_gripper_goal(self, posture): joints = ["gripper_link", "l_gripper_finger_link", "r_gripper_finger_link"] while not rospy.is_shutdown(): result = self.gripper.moveToJointPosition(joints, pose, plan_only=True, start_state=self.virtual_state) if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Gripper Posture Plan Successful") self.trajectory = result.planned_trajectory.joint_trajectory return self.trajectory def update_virtual_state(self): # Sets joint names and sets position to final position of previous trajectory if self.trajectory == None: print "No trajectory available to provide a new virtual state." print "Update can only occur after trajectory has been planned." else: self.virtual_state.joint_state.name = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint", ] self.virtual_state.joint_state.position = self.trajectory.points[-1].positions def clear_virtual_state(self): self.virtual_state.joint_state.joint_names = [] self.virtual_state.joint_state.position = [] def execute_trajectory(self, trajectory): execute_known_trajectory = rospy.ServiceProxy("execute_known_trajectory", ExecuteKnownTrajectory) result = execute_known_trajectory(trajectory)
def picknplace(): # Define initial parameters p = PlanningSceneInterface("base") g = MoveGroupInterface("both_arms", "base") gr = MoveGroupInterface("right_arm", "base") gl = MoveGroupInterface("left_arm", "base") leftgripper = baxter_interface.Gripper('left') leftgripper.calibrate() leftgripper.open() jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2'] jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2'] pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519] lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384] rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719] rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323] # Clear planning scene p.clear() # Add table as attached object p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) # Move both arms to start state g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Loop to continue pick and place until all objects are cleared from table j=0 k=0 while locs_x: p.clear() # CLear planning scene of all collision objects # Attach table as attached collision object to base of baxter p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal']) xn = locs_x[0] yn = locs_y[0] zn = -0.06 thn = orien[0] sz = size[0] if thn > pi/4: thn = -1*(thn%(pi/4)) # Add collision objects into planning scene objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11'] for i in range(1,len(locs_x)): p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05) p.waitForSync() # Move both arms to pos2 (Right arm away and left arm on table) g.moveToJointPosition(jts_both, pos2, plan_only=False) # Move left arm to pick object and pick object pickgoal = PoseStamped() pickgoal.header.frame_id = "base" pickgoal.header.stamp = rospy.Time.now() pickgoal.pose.position.x = xn pickgoal.pose.position.y = yn pickgoal.pose.position.z = zn+0.1 pickgoal.pose.orientation.x = 1.0 pickgoal.pose.orientation.y = 0.0 pickgoal.pose.orientation.z = 0.0 pickgoal.pose.orientation.w = 0.0 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn) gl.moveToPose(pickgoal, "left_gripper", plan_only=False) pickgoal.pose.position.z = zn gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False) leftgripper.close() pickgoal.pose.position.z = zn+0.1 gl.moveToPose(pickgoal, "left_gripper", plan_only=False) # Move both arms to pos1 g.moveToJointPosition(jts_both, pos1, plan_only=False) # Get obj locations temp = rospy.wait_for_message("Dpos", PoseArray) locs = temp.poses locs_x = [] locs_y = [] orien = [] size = [] for i in range(len(locs)): locs_x.append(0.57 + locs[i].position.x) locs_y.append(-0.011 + locs[i].position.y) orien.append(locs[i].position.z*pi/180) size.append(locs[i].orientation.x) # Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene ind_rmv = [] for i in range(0,len(locs)): if (locs_y[i] > 0.42): ind_rmv.append(i) continue for j in range(i,len(locs)): if not (i == j): if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018: ind_rmv.append(i) locs_x = del_meth(locs_x, ind_rmv) locs_y = del_meth(locs_y, ind_rmv) orien = del_meth(orien, ind_rmv) size = del_meth(size, ind_rmv) # Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes if locs_x: ig0 = itemgetter(0) sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0)) locs_x = list(sorted_lists[1]) locs_y = list(sorted_lists[2]) orien = list(sorted_lists[3]) size = list(sorted_lists[0]) # Place object stleft = PoseStamped() stleft.header.frame_id = "base" stleft.header.stamp = rospy.Time.now() stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.55 stleft.pose.position.z = 0.1 stleft.pose.orientation.x = 1.0 stleft.pose.orientation.y = 0.0 stleft.pose.orientation.z = 0.0 stleft.pose.orientation.w = 0.0 # Stack objects (large cubes) if size if greater than some threshold if sz > 16.: stleft.pose.position.x = 0.65 stleft.pose.position.y = 0.7 stleft.pose.position.z = -0.04+(k*0.05) k += 1 gl.moveToPose(stleft, "left_gripper", plan_only=False) leftgripper.open() stleft.pose.position.z = 0.3 gl.moveToPose(stleft, "left_gripper", plan_only=False)
class FetchClient(object): def __init__(self): self.bfp = True self.cx = 320 self.cy = 240 self.ax = 57.5 self.ay = 45 self.fx = self.cx / math.tan(self.ax * math.pi / 180.0 / 2) self.fy = self.cy / math.tan(self.ay * math.pi / 180.0 / 2) self.robot = robot_interface.Robot_Interface() self.br = tf.TransformBroadcaster() self.move_group = MoveGroupInterface("arm", "base_link") self.pose_group = moveit_commander.MoveGroupCommander("arm") self.cam = camera.RGBD() while True: try: cam_info = self.cam.read_info_data() if cam_info is not None: break except: rospy.logerr('info not recieved') self.pcm = PCM() self.pcm.fromCameraInfo(cam_info) def broadcast_position(self, position, name): pass def calc_3D_position(self, u, v): dep_data = self.robot.get_img_data()[1] z = self.robot.get_depth((u, v), dep_data) if z == 0: return None, None, None x = (u - self.cx) * z / self.fx y = (v - self.cy) * z / self.fy return x, y, z def broadcast_from_pix(self, point, name): while self.bfp: # dep = self.robot.get_depth(point, dep_data) # while dep == 0: # print("depth info error") # dep = self.robot.get_depth(point, dep_data) # # td_points = self.pcm.projectPixelTo3dRay(point) # norm_pose = np.array(td_points) # norm_pose = norm_pose / norm_pose[2] # norm_pose = norm_pose * (dep) norm_pose = self.calc_3D_position(point[0], point[1]) a = tf.transformations.quaternion_from_euler(ai=-2.355, aj=-3.14, ak=0.0) b = tf.transformations.quaternion_from_euler(ai=0.0, aj=0.0, ak=1.57) base_rot = tf.transformations.quaternion_multiply(a, b) self.br.sendTransform(norm_pose, base_rot, rospy.Time.now(), name, 'head_camera_rgb_optical_frame') def get_position_of_baselink(self, name): listener = tf.TransformListener() rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: pose, rot = listener.lookupTransform('base_link', name, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue rate.sleep() if pose: # rot = tf.transformations.quaternion_from_euler(0*DEG_TO_RAD, 90*DEG_TO_RAD, 0*DEG_TO_RAD) return pose def create_tf_of_base_link(self, name, position, rot): quat = tf.transformations.quaternion_from_euler(ai=rot[0], aj=rot[1], ak=rot[2]) while True: self.br.sendTransform(position, quat, rospy.Time.now(), name, 'base_link') def show_image_with_point(self, im): plt.imshow(im) pylab.show() def reach(self, point_xyz): pose = Pose(Point(point_xyz[0], point_xyz[1], point_xyz[2]), Quaternion(0, 0.707106781187, 0, 0.707106781187)) # pose = Pose(Point(0.57503179279, 0.170263536187, 0.97855338914), Quaternion(0, 0.707106781187, 0, 0.707106781187)) print(pose) gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' gripper_pose_stamped.header.stamp = rospy.Time.now() gripper_pose_stamped.pose = pose gripper_frame = 'wrist_roll_link' # gripper_frame = 'l_gripper_finger_link' self.move_group.moveToPose(gripper_pose_stamped, gripper_frame, plan_only=True) result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode print("plan0 success!") height = result.planned_trajectory.joint_trajectory.points[0] gripper_pose_stamped.pose.position.z += height self.move_group.moveToPose(gripper_pose_stamped, gripper_frame, plan_only=True) result1 = self.move_group.get_move_action().get_result() if result1: if result1.error_code.val == MoveItErrorCodes.SUCCESS: print("plan1 success!") joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] joint_pose = result1.planned_trajectory.joint_trajectory[ 0].position[6:13] print joint_pose self.move_group.moveToJointPosition(joint_names, joint_pose, wait=False) result2 = self.move_group.get_move_action().get_result() if result2: # Checking the MoveItErrorCode if result2.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("reach!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("reach fail") else: rospy.logerr("MoveIt! failure no result returned.") else: rospy.logerr("plan1 fail") else: rospy.logerr("MoveIt! failure no result returned.") else: rospy.logerr("MoveIt! failure no result returned.")