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)
