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
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)
q_current = arm.get_joint_angles() q_diff = np.subtract(q_desi, q_current) diff = np.abs(np.sum(q_diff)) while(diff > .01): arm.set_joint_positions_mod(q_desi) q_current = arm.get_joint_angles() diff = np.abs(np.sum(np.subtract(q_desi, q_current))) print("Run Process?") wait = raw_input("Press Enter to continue...") gripper.command_position(100) # open gripper q_desi = np.transpose(q[0:7,0]) while((np.abs(np.sum(q_desi - q_current)))> .01): arm.set_joint_positions_mod(q_desi) q_current = arm.get_joint_angles() q_desi = np.transpose(q[0:7,1]) time.sleep(1) while((np.abs(np.sum(q_desi - q_current)))> .01): arm.set_joint_positions_mod(q_desi) q_current = arm.get_joint_angles() time.sleep(.5) gripper.command_position(0) # Close gripper #time.sleep(.5) q_desi = np.transpose(q[0:7,2])
def serve(): 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 press_faucet = True place_cup = False gripper_position_threshold = 4.6 # clean the scene scene.remove_world_object() # forward kinematic object kin = baxter_kinematics('left') rospy.loginfo("Time to kill the process") start_time = rospy.get_time() while (rospy.get_time() - start_time < 1.0): if rospy.is_shutdown(): no_detection = True press_faucet = False break joint_states = { 'observe_r': [-0.7255729118408204, -0.4893398707763672, 0.022626216595458985, 1.9788352141113283, -1.549704089190674, -1.5301458341674805, -0.016106798254394532], 'observe_l': [0.7255729118408204, -0.4893398707763672, 0.022626216595458985, 1.9788352141113283, 1.549704089190674, -1.5301458341674805, -0.016106798254394532] } if place_cup: group_left.set_joint_value_target(joint_states['observe_l']) plan_valid = False while not rospy.is_shutdown() and not plan_valid: plan = group_left.plan() plan_valid = plan_validate(plan, 100,100,100) print plan_valid group_left.execute(plan) # step 1 pose1 = Pose() pose1.position.x = 0.551 pose1.position.y = 0.615 pose1.position.z = -0.0728 pose1.orientation.x = -0.0140 pose1.orientation.y = 0.866 pose1.orientation.z = -0.0272 pose1.orientation.w = 0.499 # execute_valid_plan(group_left, pose1) # print group_left.get_current_joint_values() group_left.set_joint_value_target([0.08283496245117188, -0.26192721923217777, 0.0609757362487793, 1.7851701398620607, 0.9625729432983399, -1.0622816943969726, -0.14841264105834961]) rospy.sleep(1) plan_valid = False while not rospy.is_shutdown() and not plan_valid: plan = group_left.plan() plan_valid = plan_validate(plan, 100,100,100) print plan_valid group_left.execute(plan) # step 2 pose2 = get_pose_with_sleep(group_left) pose2.pose.position.x+= 0.15 plan = plan_with_orientation_lock(group_left, pose2) group_left.execute(plan) gripper_left.command_position(40) # step 3 rospy.sleep(1) pose2 = get_pose_with_sleep(group_left) pose2.pose.position.x-= 0.10 plan = plan_with_orientation_lock(group_left, pose2) group_left.execute(plan) # step 4 pose2 = get_pose_with_sleep(group_left) pose2.pose.position.z+= 0.3 plan = plan_with_orientation_lock(group_left, pose2) group_left.execute(plan) # step 5 pose2 = get_pose_with_sleep(group_left) pose2.pose.position.y-= 0.5 plan = plan_with_orientation_lock(group_left, pose2, threshold = 0.2) group_left.execute(plan) # step 6 pose2 = get_pose_with_sleep(group_left) pose2.pose.position.x-= 0.03 plan = plan_with_orientation_lock(group_left, pose2) group_left.execute(plan) pose2 = get_pose_with_sleep(group_left) pose2.pose.position.z-= 0.3 plan = plan_with_orientation_lock(group_left, pose2) group_left.execute(plan) # step 7 pose2 = get_pose_with_sleep(group_left) pose2.pose.position.x+= 0.08 plan = plan_with_orientation_lock(group_left, pose2, threshold = 0.01) group_left.execute(plan) pose2 = get_pose_with_sleep(group_left) pose2.pose.position.y-= 0.02 plan = plan_with_orientation_lock(group_left, pose2, threshold = 0.01) group_left.execute(plan) gripper_left.open() # step 8 rospy.sleep(1) # pose2 = get_pose_with_sleep(group_left) # pose2.pose.position.y-= 0.01 # plan = plan_with_orientation_lock(group_left, pose2, threshold = 0.01) # group_left.execute(plan) pose2 = get_pose_with_sleep(group_left) pose2.pose.position.x-= 0.08 plan = plan_with_orientation_lock(group_left, pose2, threshold = 0.01) group_left.execute(plan) pose2 = get_pose_with_sleep(group_left) pose2.pose.position.y+= 0.03 plan = plan_with_orientation_lock(group_left, pose2, threshold = 0.01) group_left.execute(plan) # step 9 pose2 = get_pose_with_sleep(group_left) pose2.pose.position.z+= 0.3 plan = plan_with_orientation_lock(group_left, pose2) group_left.execute(plan) if press_faucet: # press faucet group_left.set_joint_value_target(joint_states['observe_l']) rospy.sleep(1) plan_valid = False while not rospy.is_shutdown() and not plan_valid: plan = group_left.plan() plan_valid = plan_validate(plan, 100,100,100) print plan_valid group_left.execute(plan) pre_pose = get_pose_with_sleep(group_left) pre_pose.pose.position.z += 0.1 plan = plan_with_orientation_lock(group_left, pre_pose) group_left.execute(plan) # move to the pose above the faucet faucet_pose = Pose() faucet_pose.orientation.x = -0.489 faucet_pose.orientation.y = 0.517 faucet_pose.orientation.z = -0.517 faucet_pose.orientation.w = 0.476 faucet_pose.position.x = 0.69 faucet_pose.position.y = 0.18 faucet_pose.position.z = 0.244 execute_valid_plan(group_left, faucet_pose) # press down the faucet press_pose = get_pose_with_sleep(group_left) press_pose.pose.position.z -= 0.13 plan = plan_with_orientation_lock(group_left, press_pose) group_left.execute(plan) press_pose = get_pose_with_sleep(group_left) press_pose.pose.position.z -= 0.06 plan = plan_with_orientation_lock(group_left, press_pose) group_left.execute(plan) rospy.sleep(5) release_pose = get_pose_with_sleep(group_left) release_pose.pose.position.z += 0.18 release_pose.pose.position.y += 0.1 release_pose.pose.position.x += 0.05 plan = plan_with_orientation_lock(group_left, release_pose) group_left.execute(plan) group_left.set_joint_value_target(joint_states['observe_l']) plan_valid = False while not rospy.is_shutdown() and not plan_valid: plan = group_left.plan() plan_valid = plan_validate(plan, 100, 100, 100, threshold = 0.03) print plan_valid group_left.execute(plan)