class GraspExecuter(object): def __init__(self, max_vscale=1.0): self.grasp_queue = Queue() initialized = False while not initialized: try: moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.left_arm = moveit_commander.MoveGroupCommander('left_arm') self.left_arm.set_max_velocity_scaling_factor(max_vscale) self.go_to_pose('left', L_HOME_STATE) self.right_arm = moveit_commander.MoveGroupCommander( 'right_arm') self.right_arm.set_max_velocity_scaling_factor(max_vscale) self.go_to_pose('right', R_HOME_STATE) self.left_gripper = Gripper('left') self.left_gripper.set_holding_force(GRIPPER_CLOSE_FORCE) self.left_gripper.open() self.right_gripper = Gripper('right') self.left_gripper.set_holding_force(GRIPPER_CLOSE_FORCE) self.right_gripper.open() initialized = True except rospy.ServiceException as e: rospy.logerr(e) def queue_grasp(self, req): grasp = req.grasp rotation_quaternion = np.asarray([ grasp.pose.orientation.w, grasp.pose.orientation.x, grasp.pose.orientation.y, grasp.pose.orientation.z ]) translation = np.asarray([ grasp.pose.position.x, grasp.pose.position.y, grasp.pose.position.z ]) T_grasp_camera = RigidTransform(rotation_quaternion, translation, 'grasp', T_camera_world.from_frame) T_gripper_world = T_camera_world * T_grasp_camera * T_gripper_grasp self.grasp_queue.put(T_gripper_world.pose_msg) return True def execute_grasp(self, grasp): if grasp.position.z < TABLE_DEPTH + 0.02: grasp.position.z = TABLE_DEPTH + 0.02 approach = deepcopy(grasp) approach.position.z = grasp.position.z + GRASP_APPROACH_DIST # perform grasp on the robot, up until the point of lifting rospy.loginfo('Approaching') self.go_to_pose('left', approach) # grasp rospy.loginfo('Grasping') self.go_to_pose('left', grasp) self.left_gripper.close(block=True) #lift object rospy.loginfo('Lifting') self.go_to_pose('left', approach) # Drop in bin rospy.loginfo('Going Home') self.go_to_pose('left', L_PREGRASP_POSE) self.left_gripper.open() lift_gripper_width = self.left_gripper.position() # a percentage # check drops lifted_object = lift_gripper_width > 0.0 return lifted_object, lift_gripper_width def go_to_pose(self, arm, pose): """Uses Moveit to go to the pose specified Parameters ---------- pose : :obj:`geometry_msgs.msg.Pose` or RigidTransform The pose to move to max_velocity : fraction of max possible velocity """ if arm == 'left': arm = self.left_arm else: arm = self.right_arm arm.set_start_state_to_current_state() arm.set_pose_target(pose) arm.plan() arm.go()
def listener(): global watch_timesteps global firt_pack_sync seq_len = 25 rospy.init_node('auto_grasping', anonymous=True, disable_signals=True) model = load_model(pkg_dir + '/model/my_model25-94.h5') zscore_data = np.loadtxt(pkg_dir + '/model/mean_std_zscore', delimiter=',', ndmin=2) left_arm = Limb('left') left_gripper = Gripper('left') left_gripper.calibrate() rate = rospy.Rate(50) # rate rospy.Subscriber('/imu_giver', ImuPackage, watchCallback) with firt_pack_sync: firt_pack_sync.wait() opened_timeout = 0 pre_res = 0 watch_buffer = [] bax_timesteps = [] # bax_timesteps and watch_buffer are two buffers to manage sequences while not rospy.is_shutdown(): rate.sleep() l_ang = list(left_arm.joint_angles().values()) l_vel = list(left_arm.joint_velocities().values()) l_eff = list(left_arm.joint_efforts().values()) bax_step = l_ang + l_vel + l_eff bax_timesteps.append(bax_step) if (watch_timesteps[1]): watch_buffer.extend(watch_timesteps[0]) watch_timesteps[1] = 0 if (len(bax_timesteps) >= seq_len and len(watch_buffer) >= seq_len): watch_buffer = watch_buffer[len(watch_buffer) - (seq_len):] bax_timesteps = bax_timesteps[len(bax_timesteps) - (seq_len):] sequence = [] for i in range(0, math.floor(seq_len * 0.3)): step = watch_buffer.pop(0) + bax_timesteps.pop(0) sequence.append(step) for i in range(0, math.ceil(seq_len * 0.7)): step = watch_buffer[i] + bax_timesteps[i] sequence.append(step) sequence = np.array(sequence) sequence = sequence - zscore_data[0, :] sequence = sequence / zscore_data[1, :] seq = np.ndarray((1, seq_len, sequence.shape[1])) seq[0] = sequence res = model.predict(seq) res = res[0][0] rospy.loginfo(left_gripper.position()) if (left_gripper.position() > 94.0): opened_timeout = opened_timeout + 1 if (res > 0.7 and pre_res > 0.7 and opened_timeout > 25): left_gripper.command_position(0.0) opened_timeout = 0 pre_res = res
def listener(): rospy.init_node('record_data', anonymous=True, disable_signals=True) global BAX_COLUMNS global WATCH_COLUMNS global NANOS_TO_MILLIS global bax_file_name global bax_row global watch_rows global command global sequence_counter resetNode() rospy.loginfo("Commands :\ns to stop\nr to remove the last file\nn to start new sequence\nc TWICE to shutdown the node\n") rate = rospy.Rate(120) # rate watch_sub = rospy.Subscriber('/imu_giver', ImuPackage, watchCallback) rospy.loginfo("START RECORDING SEQUENCE " + str(sequence_counter)) getkey_thread = Thread(target = getKey) getkey_thread.start() left_arm = Limb('left') left_gripper = Gripper('left') while not rospy.is_shutdown(): rate.sleep() t = round(rospy.get_rostime().to_nsec()*NANOS_TO_MILLIS) l_ang = list(left_arm.joint_angles().values()) l_vel = list(left_arm.joint_velocities().values()) l_eff = list(left_arm.joint_efforts().values()) l_grip_pos = str(left_gripper.position()) bax_row = l_ang + l_vel + l_eff bax_row.append(l_grip_pos) bax_row.append(str(t)) with open(bax_file_name + str(sequence_counter), 'a') as writeFile: writer = csv.writer(writeFile) writer.writerow(bax_row) writeFile.close() if (watch_rows[1]==1): with open(watch_file_name + str(sequence_counter), 'a') as writeFile: writer = csv.writer(writeFile) writer.writerows(watch_rows[0]) writeFile.close() watch_rows[1]=0 # s to stop # r to remove the last file # n to start new sequence # c TWICE to shutdown the node shutdown = False if (command == 's') : watch_sub.unregister() rospy.loginfo("FINISH RECORDING SEQUENCE " + str(sequence_counter)) rospy.loginfo("NODE STOPPED!") while True : rospy.Rate(2).sleep() if (command == 'r') : os.remove(bax_file_name + str(sequence_counter)) os.remove(watch_file_name + str(sequence_counter)) sequence_counter = sequence_counter - 1 rospy.loginfo("FILE REMOVED!") command = '' if (command == 'n') : rospy.loginfo("RESET NODE!") sequence_counter = sequence_counter + 1 resetNode() watch_sub = rospy.Subscriber('/imu_giver', ImuPackage, watchCallback) rospy.loginfo("START RECORDING SEQUENCE " + str(sequence_counter)) break if (command == 'c') : rospy.loginfo("Enter 'c' to shutdown... ") shutdown = True break if (shutdown) : rospy.signal_shutdown("reason...") getkey_thread.join()
class Baxter(object): def __init__(self, calibrate_grippers=True): self._baxter_state = RobotEnable() self._left = Limb(LEFT) self._right = Limb(RIGHT) self._limbs = {LEFT: self._left, RIGHT: self._right} self._head = Head() self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT) if calibrate_grippers: self.calibrate() self._left_ikservice = IKService(LEFT) self._right_ikservice = IKService(RIGHT) def calibrate(self): self._left_gripper.calibrate() self._left_gripper_max = self._left_gripper.position() self._right_gripper.calibrate() self._right_gripper_max = self._right_gripper.position() @property def left_gripper_max(self): return self._left_gripper_max @property def right_gripper_max(self): return self._right_gripper_max @property def left_gripper(self): return self._left_gripper.position() @left_gripper.setter def left_gripper(self, position): self._left_gripper.command_position(position) @property def right_gripper(self): return self._right_gripper.position() @right_gripper.setter def right_gripper(self, position): self._right_gripper.command_position(position) def set_left_joints(self, angles): joints = self._left.joint_angles() for joint, angle in angles.iteritems(): if angle: joints[joint] = angle self.enable_check() self._left.set_joint_positions(joints) def set_right_joints(self, angles): joints = self._right.joint_angles() for joint, angle in angles.iteritems(): if angle: joints[joint] = angle self.enable_check() self._right.set_joint_positions(joints) def reset_limb(self, side): angles = {joint: 0.0 for joint in self._limbs[side].joint_angles()} self.enable_check() self._limbs[side].move_to_joint_positions(angles) def enable_check(self): # Sometimes robot is disabled due to another program resetting state if not self._baxter_state.state().enabled: self._baxter_state.enable() @property def joints(self): joints = { limb: joint.joint_angles() for limb, joint in self._limbs.iteritems() } return joints @property def enabled(self): return self._baxter_state.state().enabled @property def left_s0(self): return self._left.joint_angle('left_s0') @left_s0.setter def left_s0(self, angle): self.set_left_joints({'left_s0': angle}) @property def left_s1(self): return self._left.joint_angle('left_s1') @left_s1.setter def left_s1(self, angle): self.set_left_joints({'left_s1': angle}) @property def left_e0(self): return self._left.joint_angle('left_e0') @left_e0.setter def left_e0(self, angle): self.set_left_joints({'left_e0': angle}) @property def left_e1(self): return self._left.joint_angle('left_e1') @left_e1.setter def left_e1(self, angle): self.set_left_joints({'left_e1': angle}) @property def left_w0(self): return self._left.joint_angle('left_w0') @left_w0.setter def left_w0(self, angle): self.set_left_joints({'left_w0': angle}) @property def left_w1(self): return self._left.joint_angle('left_w1') @left_w1.setter def left_w1(self, angle): self.set_left_joints({'left_w1': angle}) @property def left_w2(self): return self._left.joint_angle('left_w2') @left_w2.setter def left_w2(self, angle): self.set_left_joints({'left_w2': angle}) @property def right_s0(self): return self._right.joint_angle('right_s0') @right_s0.setter def right_s0(self, angle): self.set_right_joints({'right_s0': angle}) @property def right_s1(self): return self._right.joint_angle('right_s1') @right_s1.setter def right_s1(self, angle): self.set_right_joints({'right_s1': angle}) @property def right_e0(self): return self._right.joint_angle('right_e0') @right_e0.setter def right_e0(self, angle): self.set_right_joints({'right_e0': angle}) @property def right_e1(self): return self._right.joint_angle('right_e1') @right_e1.setter def right_e1(self, angle): self.set_right_joints({'right_e1': angle}) @property def right_w0(self): return self._right.joint_angle('right_w0') @right_w0.setter def right_w0(self, angle): self.set_right_joints({'right_w0': angle}) @property def right_w1(self): return self._right.joint_angle('right_w1') @right_w1.setter def right_w1(self, angle): self.set_right_joints({'right_w1': angle}) @property def right_w2(self): return self._right.joint_angle('right_w2') @right_w2.setter def right_w2(self, angle): self.set_right_joints({'right_w2': angle}) @property def left_position(self): return self._left.endpoint_pose()['position'] @property def left_position_x(self): return self.left_position.x @left_position_x.setter def left_position_x(self, point): self.set_left_pose(position={'x': point}) @property def left_position_y(self): return self.left_position.y @left_position_y.setter def left_position_y(self, point): self.set_left_pose(position={'y': point}) @property def left_position_z(self): return self.left_position.z @left_position_z.setter def left_position_z(self, point): self.set_left_pose(position={'z': point}) @property def left_orientation(self): return self._left.endpoint_pose()['orientation'] @property def left_orientation_x(self): return self.left_orientation.x @left_orientation_x.setter def left_orientation_x(self, point): self.set_left_pose(orientation={'x': point}) @property def left_orientation_y(self): return self.left_orientation.y @left_orientation_y.setter def left_orientation_y(self, point): self.set_left_pose(orientation={'y': point}) @property def left_orientation_z(self): return self.left_orientation.z @left_orientation_z.setter def left_orientation_z(self, point): self.set_left_pose(orientation={'z': point}) @property def left_orientation_w(self): return self.left_orientation.w @left_orientation_w.setter def left_orientation_w(self, point): self.set_left_pose(orientation={'w': point}) @property def right_position(self): return self._right.endpoint_pose()['position'] @property def right_orientation(self): return self._right.endpoint_pose()['orientation'] def set_left_pose(self, position={}, orientation={}): pos = { 'x': self.left_position_x, 'y': self.left_position_y, 'z': self.left_position_z, } for key, value in position.iteritems(): pos[key] = value orient = { 'x': self.left_orientation_x, 'y': self.left_orientation_y, 'z': self.left_orientation_z, 'w': self.left_orientation_w, } for key, value in orientation.iteritems(): orient[key] = value pos = self._left_ikservice.solve_position( Pose(position=Point(**pos), orientation=Quaternion(**orient))) if pos: self.set_left_joints(pos) else: print 'nothing' #print self.joints @property def right_position(self): return self._right.endpoint_pose()['position'] @property def right_position_x(self): return self.right_position.x @right_position_x.setter def right_position_x(self, point): self.set_right_pose(position={'x': point}) @property def right_position_y(self): return self.right_position.y @right_position_y.setter def right_position_y(self, point): self.set_right_pose(position={'y': point}) @property def right_position_z(self): return self.right_position.z @right_position_z.setter def right_position_z(self, point): self.set_right_pose(position={'z': point}) @property def right_orientation(self): return self._right.endpoint_pose()['orientation'] @property def right_orientation_x(self): return self.right_orientation.x @right_orientation_x.setter def right_orientation_x(self, point): self.set_right_pose(orientation={'x': point}) @property def right_orientation_y(self): return self.right_orientation.y @right_orientation_y.setter def right_orientation_y(self, point): self.set_right_pose(orientation={'y': point}) @property def right_orientation_z(self): return self.right_orientation.z @right_orientation_z.setter def right_orientation_z(self, point): self.set_right_pose(orientation={'z': point}) @property def right_orientation_w(self): return self.right_orientation.w @right_orientation_w.setter def right_orientation_w(self, point): self.set_right_pose(orientation={'w': point}) @property def right_position(self): return self._right.endpoint_pose()['position'] @property def right_orientation(self): return self._right.endpoint_pose()['orientation'] def set_right_pose(self, position={}, orientation={}): pos = { 'x': self.right_position_x, 'y': self.right_position_y, 'z': self.right_position_z, } for key, value in position.iteritems(): pos[key] = value orient = { 'x': self.right_orientation_x, 'y': self.right_orientation_y, 'z': self.right_orientation_z, 'w': self.right_orientation_w, } for key, value in orientation.iteritems(): orient[key] = value pos = self._right_ikservice.solve_position( Pose(position=Point(**pos), orientation=Quaternion(**orient))) if pos: self.set_right_joints(pos) @property def head_position(self): return self._head.pan() @head_position.setter def head_position(self, position): self._head.set_pan(position)
def put(object, count): tf_listener = tf.TransformListener() robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() gripper_left = Gripper("left") gripper_right = Gripper("right") group_left = moveit_commander.MoveGroupCommander("left_arm") group_right = moveit_commander.MoveGroupCommander("right_arm") group_left.set_planner_id('RRTConnectkConfigDefault') group_right.set_planner_id('RRTConnectkConfigDefault') # parameters no_detection = False drop_lemon = True press_faucet = False place_cup = False gripper_position_threshold = 4.6 # count = 3 # number of lemon slices # clean the scene # scene.remove_world_object() # forward kinematic object kin = baxter_kinematics('left') while True: rospy.sleep(1) # initial pose initial_pose = Pose() # initial_pose.orientation.x = 0.9495 # initial_pose.orientation.y = 0.017127 # initial_pose.orientation.z = -0.31322 # initial_pose.orientation.w = 0.0071202 # initial_pose.orientation.x = 0.37 # initial_pose.orientation.y = 0.93 # initial_pose.orientation.z = 0.0 # initial_pose.orientation.w = 0.0 initial_pose.orientation.y = 1.0 initial_pose.position.x = 0.0 initial_pose.position.y = 0.88 initial_pose.position.z = 0.214 rospy.sleep(1) execute_valid_plan(group_left, initial_pose) if count == 0: rospy.loginfo("Finish the task") break if no_detection: rospy.loginfo("No detection") break rospy.loginfo("Time to kill the process") start_time = rospy.get_time() while (rospy.get_time() - start_time < 3.0): if rospy.is_shutdown(): no_detection = True press_faucet = False break rospy.loginfo("Looking for the detected pose") start_time = rospy.get_time() while not rospy.is_shutdown(): if(rospy.get_time() - start_time > 4.0): no_detection = True break try: tf_listener.waitForTransform('/base', '/circle', rospy.Time(0), rospy.Duration(1.0)) break except (tf.Exception, tf.ConnectivityException, tf.LookupException): continue if not no_detection: time_delay = rospy.get_time() - tf_listener.getLatestCommonTime('/circle' , '/base').to_sec() rospy.loginfo("The time delay for the detection is: %f sec" , time_delay) if(time_delay > 3.0): rospy.loginfo("Time delay of the detection exceeds 2.0 second. Stop the process") break trans, rot = tf_listener.lookupTransform('/base', '/circle', rospy.Time(0)) rospy.loginfo(trans) # move to the position that is above the lime above_pose = get_pose_with_sleep(group_left) above_pose.pose.position.x = trans[0] above_pose.pose.position.y = trans[1] #plan = plan_with_orientation_lock(group_left, above_pose, side = "left", tolerance=1000) #group_left.execute(plan) execute_valid_plan(group_left, above_pose) # set the orientation constraint during the approach pick_pose = get_pose_with_sleep(group_left) # pick_pose.pose.position.x = trans[0] # pick_pose.pose.position.y = trans[1] pick_pose.pose.position.z = -0.025 # -0.08 for the table #plan = plan_with_orientation_lock(group_left, pick_pose, side = "left", tolerance=1000) #group_left.execute(plan) execute_valid_plan(group_left, pick_pose, dx = 0.02, dy = 0.02) gripper_left.close() rospy.sleep(1) print gripper_left.position() if gripper_left.position() > gripper_position_threshold: count-=1 if drop_lemon: initial_pose = get_pose_with_sleep(group_left) lift_pose = get_pose_with_sleep(group_left) lift_pose.pose.position.z += 0.35 # plan = plan_with_orientation_lock(group_left, lift_pose, side = "left") # group_left.execute(plan) execute_valid_plan(group_left, lift_pose, dx = 0.05, dy = 0.05) pre_drop_pose = get_pose_with_sleep(group_left) pre_drop_pose.pose.position.x = 0.75 pre_drop_pose.pose.position.y = 0.5 # execute_valid_plan(group_left, pre_drop_pose) drop_pose = get_pose_with_sleep(group_left) drop_pose.pose.position.x = 0.76 drop_pose.pose.position.y = 0.00 drop_pose.pose.position.z += 0.05 # plan = plan_with_orientation_lock(group_left, drop_pose, side = "left", threshold= 1) # group_left.execute(plan) execute_valid_plan(group_left, drop_pose) gripper_left.open() rospy.sleep(1) # execute_valid_plan(group_left, pre_drop_pose) lift_pose2 = get_pose_with_sleep(group_left) lift_pose2.pose.position.x = lift_pose.pose.position.x lift_pose2.pose.position.y = lift_pose.pose.position.y lift_pose2.pose.position.z = lift_pose.pose.position.z # plan = plan_with_orientation_lock(group_left, lift_pose2, side = "left", threshold= 1) # group_left.execute(plan) execute_valid_plan(group_left, lift_pose) # fake motion else: initial_pose = get_pose_with_sleep(group_left) lift_pose = get_pose_with_sleep(group_left) lift_pose.pose.position.z += 0.1 execute_valid_plan(group_left, lift_pose, dx = 0.01, dy = 0.01) #plan = plan_with_orientation_lock(group_left, lift_pose, side = "left") #group_left.execute(plan) execute_valid_plan(group_left, initial_pose,dx = 0.01, dy = 0.01) gripper_left.open() rospy.sleep(1) else: gripper_left.open() rospy.sleep(1)