def main():
rospy.init_node('part1_obstacles')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('floor')
planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2)
table_height = 0.767
table_width = 0.7
table_x = 0.95
planning_scene.addBox('table', table_width, 2, table_height, table_x, 0,
table_height / 2)
planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2)
microwave_height = 0.28
microwave_width = 0.48
# microwave_depth = 0.33
microwave_depth = 0.27
microwave_x = 0.97
microwave_z = 0.06
microwave_y = 0.18
planning_scene.addBox('microwave', microwave_depth, microwave_width,
microwave_height, microwave_x, microwave_y,
table_height + microwave_z + microwave_height / 2)
rospy.sleep(2)
rospy.spin()
def main():
rospy.init_node('a5_obstacles')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('top_shelf')
planning_scene.removeCollisionObject('bottom_shelf')
planning_scene.addBox('top_shelf', 0.32, 1, 0.4, 0.99, 0, 1.64)
planning_scene.addBox('bottom_shelf', 0.5, 1, 1.09, 0.9, 0, 0.545)
rospy.sleep(2)
def main():
rospy.init_node('a5_obstacles')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('floor')
planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2)
planning_scene.addBox('table', 0.5, 1, 0.72, 1, 0, 0.72 / 2)
rospy.sleep(2)
def main():
rospy.init_node('part2_obstacles')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('floor')
planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01/2)
table_height = 0.767
table_width = 0.7
table_x = 0.95
planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2)
planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2)
rospy.sleep(2)
rospy.spin()
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)
def main():
rospy.init_node('pour_scene')
wait_for_time()
target_name = 'cup1'
x_pose = 0.329202820349
y_pose = -0.01
z_pose = 0.060
x_ori, y_ori, z_ori, w_ori = cvt_e2q(0, 0, 0)
x, y, z, _ = getState('cup2')
head = fetch_api.Head()
arm = fetch_api.Arm()
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
head.look_at('base_link', x, y, z)
sess = tensorflow.Session()
model = load_model(sess)
x, y, z, _ = getState(target_name)
# observation test
get_observation()
move_arm_to(arm, x_pose, y + y_pose, z + z_pose, x_ori, y_ori, z_ori,
w_ori)
x, y, z, _ = getState('table1')
base = fetch_api.Base()
if x > 1:
base.go_forward(0.6, speed=0.2)
# collision in motion planning
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
length_table = 1
width_table = 1
height_table = 0.04
x_table, y_table, z_table, _ = getState('table1')
z_table = z + 0.7
planning_scene.addBox('table', length_table, width_table, height_table,
x_table, y_table, z_table)
length_box = 0.05
width_box = 0.05
height_box = 0.2
x, y, z, _ = getState('cup1')
x_box = x
y_box = y
z_box = z
planning_scene.addBox('mug_1', length_box, width_box, height_box, x_box,
y_box, z_box)
length_box = 0.03
width_box = 0.05
height_box = 0.2
x, y, z, _ = getState('cup2')
x_box = x
y_box = y
z_box = z
planning_scene.addBox('mug_2', length_box, width_box, height_box, x_box,
y_box, z_box)
# the initial position of gripper is (-0.5, 0, 0), and
# the final position of gripper is (-0.5+x_pose, y_pose, z_pose).
x, y, z, _ = getState(target_name)
move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose, x_ori, y_ori,
z_ori, w_ori)
planning_scene.removeCollisionObject('mug_1')
# planning_scene.removeCollisionObject('mug_2')
# planning_scene.removeCollisionObject('mug')
# planning_scene.removeCollisionObject('table')
gripper = fetch_api.Gripper()
effort = gripper.MAX_EFFORT
gripper.close(effort)
x, y, z, _ = getState(target_name)
move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose + 0.06, x_ori,
y_ori, z_ori, w_ori)
x, y, z, _ = getState('cup2')
head.look_at('base_link', x, y, z)
for _ in xrange(50):
obs = get_observation()
act = model.choose_action(obs)
print('obs: {}, action: {}'.format(obs, act))
execute_action(act, arm)
time.sleep(0.5)
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()
def main():
rospy.init_node('arm_obstacle_demo')
wait_for_time()
argv = rospy.myargv()
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
planning_scene = PlanningSceneInterface('base_link')
# Create table obstacle
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
# Create divider obstacle
planning_scene.removeCollisionObject('divider')
size_x = 0.3
size_y = 0.01
size_z = 0.4
x = table_x - (table_size_x / 2) + (size_x / 2)
y = 0
z = table_z + (table_size_z / 2) + (size_z / 2)
planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
planning_scene.removeAttachedObject('tray')
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
kwargs = {
'allowed_planning_time': 20,
'execution_timeout': 40,
'num_planning_attempts': 50,
'replan': False,
}
error = arm.move_to_pose(pose1, **kwargs)
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to, frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
rospy.sleep(1)
#kwargs['orientation_constraint'] = oc
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
# At the end of your program
planning_scene.removeAttachedObject('tray')
planning_scene.clear()
def main():
# Create table obstacle
rospy.init_node('arm_obstacle_demo')
planning_scene = PlanningSceneInterface('base_link')
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z)
# Create divider obstacle
# planning_scene.removeCollisionObject('divider')
# size_x = 0.3
# size_y = 0.01
# size_z = 0.4
# x = table_x - (table_size_x / 2) + (size_x / 2)
# y = 0
# z = table_z + (table_size_z / 2) + (size_z / 2)
# planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
#Orientation constraint
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
# move to pose args
kwargs = {
'allowed_planning_time': 20,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': True,
'orientation_constraint': oc
}
# Before moving to the first pose
planning_scene.removeAttachedObject('tray')
error = arm.move_to_pose(pose1, **kwargs)
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to, frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
rospy.sleep(1)
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
# At the end of your program
planning_scene.removeAttachedObject('tray')
# Was working without this but now its needed
planning_scene.clear()
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 GraspingClient(object):
def __init__(self, sim=False):
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
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:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.17, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.larm_const_stow = [-2.28, -2.17, 2.56, 0.09, -0.15, 2.08]
self.rarm_const_stow = [2.28, 2.17, -2.56, -0.09, 0.15, 1.06]
self.tableDist = 0.7
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
]
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
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._listener = tf.TransformListener()
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()
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)
beer, grasps = self.getGraspableBeer(False)
pringles, grasps = self.getGraspablePringles(False)
if (None == beer) or (None == pringles):
return None
center_objects = (beer.primitive_poses[0].position.y +
pringles.primitive_poses[0].position.y) / 2
surface = self.getSupportSurface(beer.support_surface)
tmp1 = surface.primitive_poses[
0].position.x - surface.primitives[0].dimensions[0] / 2
surface = self.getSupportSurface(pringles.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 updateScene(self, gripper=0, plan=True):
# find objects
rospy.loginfo("Updating scene...")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = plan
goal.gripper = gripper
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, True)
# insert objects to scene
idx = -1
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 getGraspableBeer(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.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 "beer: ", obj.object.primitive_poses[0]
return obj.object, obj.grasps
# nothing detected
return None, None
def getGraspablePringles(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)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.constrained_stow, 0.05)
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)
gripper_goal.command.max_effort = 10.0
scene = PlanningSceneInterface('base_link')
p = Pose()
p.position.x = 0.4 + 0.15
p.position.y = -0.4
p.position.z = 0.7 + 0.15
p.orientation = Quaternion(*quaternion_from_euler(0, 1, 1))
arm.set_pose_target(p)
while True:
if arm.go(True):
break
clear_octomap()
scene.clear()
look_at_bin()
while not rospy.is_shutdown():
rate.sleep()
try:
t = tf_listener.getLatestCommonTime('/base_link', item_frame)
if (rospy.Time.now() - t).to_sec() > 0.2:
rospy.sleep(0.1)
continue
item_translation, item_orientation = tf_listener.lookupTransform(
'/base_link', item_frame, t)
except (tf.Exception, tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
def main():
rospy.init_node('arm_obstacle_demo')
wait_for_time()
argv = rospy.myargv()
# Create the arm and safety measures
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
return
rospy.on_shutdown(shutdown)
planning_scene = PlanningSceneInterface(frame='base_link')
planning_scene.clear()
# add table to the scene
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.4
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
# Create divider obstacle
planning_scene.removeCollisionObject('divider')
size_x = 0.3
size_y = 0.01
size_z = 0.4
x = table_x - (table_size_x / 2) + (size_x / 2)
y = 0
z = table_z + (table_size_z / 2) + (size_z / 2)
planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
kwargs1 = {
'allowed_planning_time': 15,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': False,
}
kwargs2 = {
'allowed_planning_time': 15,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': False,
'orientation_constraint': oc
}
gripper = fetch_api.Gripper()
planning_scene.removeAttachedObject('tray')
while True:
# Before moving to the first pose
error = arm.move_to_pose(pose1, **kwargs1)
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link',
'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to,
frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
rospy.sleep(1)
error = arm.move_to_pose(pose2, **kwargs2)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
planning_scene.removeAttachedObject('tray')
def main():
rospy.init_node('part1_obstacles')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('floor')
planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2)
table_height = 0.767
table_width = 0.7
table_x = 0.95
planning_scene.addBox('table', table_width, 2, table_height, table_x, 0,
table_height / 2)
planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2)
microwave_height = 0.28
microwave_width = 0.48
microwave_depth = 0.27
microwave_x = 0.97
microwave_y = 0.18
microwave_z = 0.06
microwave_side_height = 0.2
microwave_r_width = 0.135
microwave_r_y = microwave_y - 0.175
microwave_l_width = 0.035
microwave_l_y = microwave_y + 0.222
microwave_bottom_height = 0.05
microwave_top_height = 0.04
microwave_back_depth = 0.03
microwave_back_x = table_x + (microwave_depth /
2) + (microwave_back_depth / 2)
microwave_door_width = 0.09
microwave_door_x = microwave_x - 0.33
microwave_door_y = microwave_l_y + 0.027
planning_scene.addBox(
'microwave_top', microwave_depth, microwave_width,
microwave_top_height, microwave_x, microwave_y,
table_height + microwave_z + microwave_bottom_height +
microwave_side_height + (microwave_top_height / 2))
planning_scene.addBox(
'microwave_bottom', microwave_depth, microwave_width,
microwave_bottom_height, microwave_x, microwave_y,
table_height + microwave_z + (microwave_bottom_height / 2))
planning_scene.addBox(
'microwave_side_r', microwave_depth, microwave_r_width,
microwave_side_height, microwave_x, microwave_r_y, table_height +
microwave_z + microwave_bottom_height + (microwave_side_height / 2))
planning_scene.addBox(
'microwave_side_l', microwave_depth, microwave_l_width,
microwave_side_height, microwave_x, microwave_l_y, table_height +
microwave_z + microwave_bottom_height + microwave_side_height / 2)
planning_scene.addBox('microwave_back', microwave_back_depth,
microwave_width, microwave_height, microwave_back_x,
microwave_y,
table_height + microwave_z + microwave_height / 2)
planning_scene.addBox(
'microwave_door', 0.39, microwave_door_width, microwave_height + 0.01,
microwave_door_x, microwave_door_y,
table_height + microwave_z + microwave_height / 2 + 0.005)
rospy.sleep(2)
rospy.spin()
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)
req.ik_request.timeout = rospy.Duration(3.0)
res = self.ik_service(req)
joint_state = res.solution.joint_state
joints = []
for name, position in zip(joint_state.name, joint_state.position):
if name in fetch_api.ArmJoints.names():
joints.append((position))
return joints
if __name__ == "__main__":
rospy.init_node("pick_demo")
dist = 0
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('cube')
planning_scene.removeCollisionObject('table')
group = moveit_commander.MoveGroupCommander("arm")
planning_frame = group.get_planning_frame()
pd = pick_demo()
grasp = pd.calculate_grasp()
base = fetch_api.Base()
arm = fetch_api.Arm()
torso = fetch_api.Torso()
gripper = fetch_api.Gripper()
torso.set_height(0.4)
joint_goal = group.get_current_joint_values()
joint_goal = [0,0,0,-math.pi/2,0,math.pi/2,0]
group.go(joint_goal, wait=True)
group.stop()
class GraspingClient(object):
def __init__(self, sim=False):
# Define planning groups
self.scene = PlanningSceneInterface("base_link")
self.dof = rospy.get_param('~jaco_dof')
self.move_group = MoveGroupInterface("upper_body", "base_link")
self.rmove_group = MoveGroupInterface("right_arm", "base_link")
planner_id = "RRTConnectkConfigDefault"
self.move_group.setPlannerId(planner_id)
self.rmove_group.setPlannerId(planner_id)
self.objects_heights = [0.122, 0.240]
self.objects_heights_tolerances = [0.02, 0.03]
# Define joints and positions for 6 and 7 DOF
if "6dof" == self.dof:
self._upper_body_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint",
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint", "linear_joint",
"pan_joint", "tilt_joint"
]
self._right_arm_joints = [
"right_elbow_joint", "right_shoulder_lift_joint",
"right_shoulder_pan_joint", "right_wrist_1_joint",
"right_wrist_2_joint", "right_wrist_3_joint"
]
self._left_arm_joints = [
"left_elbow_joint", "left_shoulder_lift_joint",
"left_shoulder_pan_joint", "left_wrist_1_joint",
"left_wrist_2_joint", "left_wrist_3_joint"
]
self.tucked = [
-2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571,
0.0371, 0.0, 0.0
]
self.constrained_stow = [
2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56,
0.09, -0.15, 2.06, 0.42, 0.0, 0.0
]
self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06]
#self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32]
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
]
self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0]
else:
rospy.logerr("DoF needs to be set 6 or 7, aborting demo")
return
# Define the MoveIt pickplace pipeline
self.pickplace = None
self.pickplace = PickPlaceInterface("left_side",
"left_gripper",
verbose=True)
self.pickplace.planner_id = planner_id
self.pick_result = None
self.place_result = None
self._listener = tf.TransformListener()
self._lgripper = GripperActionClient('left')
find_grasp_planning_topic = "grasp_planner/plan"
rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic)
self.find_grasp_planning_client = actionlib.SimpleActionClient(
find_grasp_planning_topic, GraspPlanningAction)
self.find_grasp_planning_client.wait_for_server()
rospy.loginfo("...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 __del__(self):
self.scene.clear()
def add_objects_to_keep(self, obj):
self._objs_to_keep.append(obj)
def clearScene(self):
self.scene.clear()
def calculateGraspForObject(self, object_to_grasp, gripper):
goal = GraspPlanningGoal()
goal.object = object_to_grasp
goal.gripper = gripper
self.find_grasp_planning_client.send_goal(goal)
self.find_grasp_planning_client.wait_for_result(rospy.Duration(5.0))
return self.find_grasp_planning_client.get_result(
).grasps #moveit_msgs/Grasp[]
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
retries=1,
scene=self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
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.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
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.place_with_retry(
block.name, places, retries=1, scene=self.scene)
self.place_result = place_result
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)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def goto_plan_grasp(self):
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(
self._upper_body_joints, self.constrained_stow, 0.05)
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,
planning_time=120.0)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def open_gripper(self):
self._lgripper.command(self._gripper_open, block=True)
def close_gripper(self):
self._lgripper.command(self._gripper_closed, block=True)
class RoboEatsServer(object):
DEFAULT_TORSO_HEIGHT = 0.4
MICROWAVE_LOCATION_NAME = "microwave_location"
DROPOFF_LOCATION_NAME = "dropoff_location"
FOOD_ALIAS = "food_id"
def __init__(self, save_file_path="food_items.pkl", nav_file_path="annotator_positions.pkl"):
self._food_items_pub = rospy.Publisher(FOOD_ITEMS_TOPIC,
FoodItems, queue_size=10, latch=True)
rospy.loginfo("Given save file path: " + save_file_path)
if os.path.isfile(save_file_path):
rospy.loginfo("File already exists, loading saved positions.")
with open(save_file_path, "rb") as save_file:
try:
self._food_items = pickle.load(save_file)
except EOFError:
# this can be caused if the file is empty.
self._food_items = {}
rospy.loginfo("File loaded...")
else:
rospy.loginfo("File doesn't exist.")
self._food_items = {}
self.__print_food_items__()
self._save_file_path = save_file_path
self.__pub_food_items__()
rospy.loginfo("initializing arm...")
self.arm = robot_api.Arm()
rospy.loginfo("initializing gripper...")
self.gripper = robot_api.Gripper()
rospy.loginfo("initializing head...")
self.head = robot_api.Head()
rospy.loginfo("initializing torso...")
self.torso = robot_api.Torso()
rospy.loginfo("initializing planning scene...")
self.planning_scene = PlanningSceneInterface('base_link')
self.curr_obstacle = None
rospy.loginfo("Starting map annotator...")
# We should expect the nav file given to contain the annotated positions:
# MICROWAVE_LOCATION_NAME - starting location in front of the microwave.
# DROPOFF_LOCATION_NAME - ending dropoff location.
# TODO: Remember to uncomment this section when we get the map working.
# self._map_annotator = Annotator(save_file_path=nav_file_path)
# if not self._map_annotator.exists(self.MICROWAVE_LOCATION_NAME):
# rospy.logwarn("Annotator is missing location '%s'" %
# (self.MICROWAVE_LOCATION_NAME))
# if not self._map_annotator.exists(self.DROPOFF_LOCATION_NAME):
# rospy.logwarn("Annotator is missing location '%s'" %
# (self.DROPOFF_LOCATION_NAME))
rospy.loginfo("Initialization finished...")
def __print_food_items__(self):
rospy.loginfo("Current food items:")
for f in self._food_items.values():
rospy.loginfo("\t" + str(f))
def __save_file__(self):
"""
Saves the pickle file containing food item information.
"""
with open(self._save_file_path, "wb") as save_file:
pickle.dump(self._food_items, save_file,
protocol=pickle.HIGHEST_PROTOCOL)
# flush() saves file immediately instead of buffering changes.
save_file.flush()
def __pub_food_items__(self):
"""
Publishes the current list of food items to the FOOD_ITEMS_TOPIC topic.
"""
food_items = FoodItems()
if len(self._food_items) > 0:
food_items.names, food_items.descriptions, food_items.ids = zip(*[(f.name, f.description, f.id) for f in self._food_items.values()])
self._food_items_pub.publish(food_items)
def __food_list_modified__(self):
"""
Helper method which does any actions that're needed after the food item dict has been modified.
"""
self.__save_file__()
self.__pub_food_items__()
def __remove_food_item__(self, id):
if id in self._food_items:
self._food_items.pop(id)
self.__food_list_modified__()
def handle_create_food_item(self, request):
"""
input: request(name, description, id)
Creates and adds the given food item.
If a food item with the same ID already exists, then it will be overridden by this food item.
"""
rospy.loginfo(request)
self._food_items[request.id] = FoodItem(request.name, request.description, request.id)
self.__food_list_modified__()
return CreateFoodItemResponse()
def handle_remove_food_item(self, request):
"""
input: request(id)
Removes the food item with the given id iff it exists.
"""
self.__remove_food_item__(request.id)
return RemoveFoodItemResponse()
def __food_id_to_ar_frame__(self, id):
return "ar_marker_" + str(id)
def __load_program_and_run__(self, program_fp, id):
"""Load a pickle file from the given file path and run it with respect to the given food id.
Arguments:
program_fp {str} -- program file path
id {int} -- food id
"""
if os.path.isfile(program_fp):
rospy.loginfo("File " + program_fp + " exists. Loading...")
with open(program_fp, "rb") as load_file:
program = Program(self.arm, self.gripper, self.head, self.torso)
program.commands = pickle.load(load_file)
rospy.loginfo("Program loaded...")
ar_marker_frame = self.__food_id_to_ar_frame__(id)
rospy.loginfo("Program before changes:")
program.print_program()
# Make the program relative to this food item
rospy.loginfo("Program after changes:")
program.replace_frame(self.FOOD_ALIAS, ar_marker_frame)
program.run()
else:
rospy.logerr("Program from given file path does not exist: " + program_fp)
raise Exception
def init_robot(self):
"""
0a. Move torso to default position
0b. reset head
0c. open gripper
0d. move arm to starting pos (start_pos.pkl)
"""
rospy.loginfo("STARTING SEGMENT 1")
rospy.loginfo("0a. Move torso to default position")
torso = robot_api.Torso()
torso.set_height(0.4)
rospy.sleep(2)
rospy.loginfo("0b. reset head")
head = robot_api.Head()
head.pan_tilt(-0.1, 0.57)
rospy.sleep(2)
rospy.loginfo("0c. open gripper")
self.gripper.open()
rospy.loginfo("0d. Move arm to starting pos")
self.__load_program_and_run__("start_pos.pkl", id)
rospy.sleep(1.5)
def clear_obstacles(self):
self.planning_scene.clear()
self.planning_scene.removeCollisionObject('table')
self.planning_scene.removeCollisionObject('floor')
self.planning_scene.removeCollisionObject('lunchbox')
self.planning_scene.removeAttachedObject('lunchbox')
def start_obstacles_1(self):
"""
This scene has the microwave in a closed position.
"""
if self.curr_obstacle == 1:
# don't change it if it's already set to this obstacle
return
self.planning_scene.clear()
self.planning_scene.removeCollisionObject('table')
self.planning_scene.removeCollisionObject('floor')
self.planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01/2)
table_height = 0.767
table_width = 0.7
table_x = 0.95
self.planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2)
self.planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2)
microwave_height = 0.28
microwave_width = 0.48
# microwave_depth = 0.33
microwave_depth = 0.27
microwave_x = 0.97
microwave_z = 0.06
microwave_y = 0.18
self.planning_scene.addBox('microwave', microwave_depth, microwave_width, microwave_height, microwave_x, microwave_y, table_height + microwave_z + microwave_height/2)
self.curr_obstacle = 1
rospy.sleep(3.5)
def start_obstacles_2(self):
"""
this scene has the microwave in an open position with the lid open.
"""
if self.curr_obstacle == 2:
# don't change it if it's already set to this obstacle
return
self.planning_scene.clear()
self.planning_scene.removeCollisionObject('table')
self.planning_scene.removeCollisionObject('floor')
self.planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01/2)
table_height = 0.767
table_width = 0.7
table_x = 0.95
self.planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2)
self.planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2)
microwave_height = 0.28
microwave_width = 0.48
microwave_depth = 0.27
microwave_x = 0.97
microwave_y = 0.18
microwave_z = 0.06
microwave_side_height = 0.2
microwave_r_width = 0.04 + 0.05
microwave_r_y = microwave_y - 0.19
microwave_l_width = 0.035
microwave_l_y = microwave_y + 0.2
microwave_bottom_height = 0.05
microwave_top_height = 0.04
microwave_back_depth = 0.03
microwave_back_x = table_x + (microwave_depth / 2) + (microwave_back_depth/2)
microwave_door_width = 0.045
microwave_door_x = microwave_x - 0.33
microwave_door_y = microwave_l_y + 0.05
self.planning_scene.addBox('microwave_top', microwave_depth, microwave_width, microwave_top_height, microwave_x, microwave_y, table_height + microwave_z + microwave_bottom_height + microwave_side_height + (microwave_top_height/2))
self.planning_scene.addBox('microwave_bottom', microwave_depth, microwave_width, microwave_bottom_height, microwave_x, microwave_y, table_height + microwave_z + (microwave_bottom_height/2) - 0.015)
self.planning_scene.addBox('microwave_side_r', microwave_depth, microwave_r_width, microwave_side_height, microwave_x, microwave_r_y, table_height + microwave_z + microwave_bottom_height + (microwave_side_height/2))
self.planning_scene.addBox('microwave_side_l', microwave_depth, microwave_l_width, microwave_side_height, microwave_x, microwave_l_y, table_height + microwave_z + microwave_bottom_height + microwave_side_height/2)
self.planning_scene.addBox('microwave_back', microwave_back_depth, microwave_width, microwave_height, microwave_back_x, microwave_y, table_height + microwave_z + microwave_height/2)
self.planning_scene.addBox('microwave_door', 0.42, microwave_door_width, microwave_height + 0.01, microwave_door_x, microwave_door_y, table_height + microwave_z + microwave_height/2 + 0.005)
self.curr_obstacle = 2
rospy.sleep(3.5)
def attach_lunchbox(self):
self.remove_lunchbox()
frame_attached_to = 'gripper_link'
frames_okay_collide_with = ['gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link', 'wrist_roll_link']
lunchbox_x_offset = 0.1
self.planning_scene.attachBox("lunchbox", 0.10, 0.10, 0.02, lunchbox_x_offset, 0, 0, frame_attached_to, frames_okay_collide_with)
def remove_lunchbox(self):
self.planning_scene.removeAttachedObject('lunchbox')
self.planning_scene.removeCollisionObject('lunchbox')
def start_segment1a(self, id):
"""
(Segment 1a)
0. Initialize robot
1. (OMITTED) Move to start pose
2. Open microwave (p2.pkl) (done)
2b. Move microwave lid (p2b.pkl) (done)
"""
# if id in self._food_items:
rospy.loginfo("0. Initialize robot")
self.init_robot()
self.start_obstacles_2()
# rospy.loginfo("1. Move to start pose")
# self._map_annotator.goto_position(self.MICROWAVE_LOCATION_NAME)
# rospy.sleep(2)
rospy.loginfo("2. Open microwave")
self.__load_program_and_run__("p2.pkl", id)
rospy.sleep(1.5)
rospy.loginfo("2b. Move microwave lid")
self.__load_program_and_run__("p2b.pkl", id)
rospy.sleep(1.5)
rospy.loginfo("FINISHED SEGMENT 1a")
# else:
# print("Food item " + str(id) + " does not exist.")
def start_segment1b(self, id):
"""
(Segment 1b)
3. Grab lunchbox (p1.pkl) (done - but redo if have enough time)
4. Put it into microwave (p3.pkl) (done-iffy)
"""
self.start_obstacles_2()
rospy.loginfo("STARTING SEGMENT 1b")
rospy.loginfo("3. Grab lunchbox")
self.__load_program_and_run__("p1.pkl", id)
rospy.sleep(1.5)
self.attach_lunchbox()
rospy.sleep(2)
rospy.loginfo("4. Put it into microwave")
self.__load_program_and_run__("p3.pkl", id)
rospy.sleep(1.5)
self.remove_lunchbox()
# else:
# print("Food item " + str(id) + " does not exist.")
def start_segment1c(self, id):
self.start_obstacles_2()
rospy.loginfo("5a. Close microwave pt. 1")
self.__load_program_and_run__("p4a.pkl", id)
rospy.sleep(1.5)
rospy.loginfo("5b. Changing obstacles...")
self.start_obstacles_1()
rospy.loginfo("5b. Close microwave pt. 2")
self.__load_program_and_run__("p4b.pkl", id)
rospy.sleep(1.5)
rospy.loginfo("FINISHED SEGMENT 1c")
def start_segment2(self, id):
"""
(Segment 2)
6. Enter time (1 min) & 7. Start microwave (p5.pkl)
8. Wait for food to finish microwaving
9. Wait for cooldown
"""
# if id in self._food_items:
rospy.loginfo("STARTING SEGMENT 2")
rospy.loginfo("6. Enter time(1 min)")
self.__load_program_and_run__("p5.pkl", id)
rospy.loginfo("8. Wait for food to finish microwaving (in seconds)")
rospy.sleep(5)
rospy.loginfo("9. Wait for cooldown (in seconds)")
# rospy.sleep(40)
rospy.loginfo("FINISHED SEGMENT 2")
# else:
# print("Food item " + str(id) + " does not exist.")
def start_segment3a(self, id):
"""
(Segment 3)
10a. Open microwave (p2.pkl) (done)
10b. Move microwave lid (p2b.pkl) (done)
"""
# if id in self._food_items:
self.start_obstacles_2()
rospy.loginfo("PRE INITIALIZING FOR SEGMENT 3")
self.__load_program_and_run__("segment3a-pre.pkl", id)
self.__load_program_and_run__("start_pos.pkl", id)
rospy.loginfo("STARTING SEGMENT 3")
rospy.loginfo("10a. Open microwave")
self.__load_program_and_run__("p2.pkl", id)
rospy.loginfo("10b. Move microwave lid ")
self.__load_program_and_run__("p2b.pkl", id)
# else:
# print("Food item " + str(id) + " does not exist.")
def start_segment3b(self, id):
"""
(Segment 3)
11. Grab lunchbox (p6a.pkl, p6b.pkl)
"""
self.start_obstacles_2()
rospy.loginfo("11. Grab lunchbox")
# print('lowering torso for better view')
# self.torso.set_height(0.35)
self.__load_program_and_run__("p6a.pkl", id)
self.attach_lunchbox()
rospy.sleep(2)
self.__load_program_and_run__("p6b.pkl", id)
def start_segment3c(self, id):
"""
(Segment 3)
12. (OMITTED) Move to dropoff pose
13. Put down lunchbox (p7.pkl)
"""
self.start_obstacles_2()
# rospy.loginfo("12. Move to dropoff pose")
# self._map_annotator.goto_position(self.DROPOFF_LOCATION_NAME)
# rospy.sleep(2)
rospy.loginfo("13. Put down lunchbox")
self.__load_program_and_run__("p7.pkl", id)
self.remove_lunchbox()
rospy.loginfo("FINISHED SEGMENT 3")
def start_segment4(self, id):
"""
(Segment 4)
14. (OMITTED) Move to start pose
15. Close microwave (p4a.pkl, p4b.pkl)
"""
# if id in self._food_items:
rospy.loginfo("STARTING SEGMENT 4")
# rospy.loginfo("14. Move to start pose")
# self._map_annotator.goto_position(self.MICROWAVE_LOCATION_NAME)
# rospy.sleep(2)
self.start_obstacles_2()
rospy.loginfo("15a. Close microwave pt. 1")
self.__load_program_and_run__("p4a.pkl", id)
rospy.sleep(1.5)
rospy.loginfo("15b. Changing obstacles...")
self.start_obstacles_1()
rospy.loginfo("15b. Close microwave pt. 2")
self.__load_program_and_run__("p4b.pkl", id)
rospy.sleep(1.5)
rospy.loginfo("FINISHED SEGMENT 4")
# else:
# print("Food item " + str(id) + " does not exist.")
def handle_start_sequence(self, request):
"""
input: request(id)
Starts the entire food sequence and removes the food item from the dictionary after it has been finished.
"""
id = request.id
# if id in self._food_items:
rospy.loginfo("Starting sequence for food item: " + str(self._food_items[id]))
self.start_segment1a(id)
self.start_segment1b(id)
self.start_segment1c(id)
self.start_segment2(id)
self.start_segment3a(id)
self.start_segment3b(id)
self.start_segment3c(id)
# we decioded that we don't need to close the door :)
self.start_segment4(id)
rospy.loginfo("Remove food item from the list.")
# self.__remove_food_item__(id)
rospy.loginfo("Finished sequence.")
# else:
# print("Food item " + str(id) + " does not exist.")
return StartSequenceResponse()
