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)