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)
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 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 main():
rospy.init_node("arm_obstacle_demo")
wait_for_time()
argv = rospy.myargv()
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.2
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
gripper = robot_api.Gripper()
arm = robot_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
kwargs = {
'allowed_planning_time': 15,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': True,
'orientation_constraint': oc
}
planning_scene.removeAttachedObject('tray')
gripper.open()
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')
rospy.sleep(2)
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()
gripper.close()
rospy.sleep(2)
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')
gripper.open()
planning_scene.removeAttachedObject('tray')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
return
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('arm_obstacle_demo')
wait_for_time()
# argv = rospy.myargv()
planning_scene = PlanningSceneInterface(frame='base_link')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
planning_scene.removeAttachedObject('tray')
# Create table obstacle
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
# 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)
# add 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
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
# # set torso to max height
torso = robot_api.Torso()
torso.set_height(robot_api.Torso.MAX_HEIGHT)
arm = robot_api.Arm()
# register shutdown method
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
kwargs = {
'allowed_planning_time': 20,
'execution_timeout': 20,
'num_planning_attempts': 10,
'replan': True
}
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')
# attach an object if pose1 is successfully reached
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()
# close the gripper
# gripper = robot_api.Gripper()
# gripper.close()
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')
# At the end of the program
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
planning_scene.removeAttachedObject('tray')
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()
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import argparse
import rospy
from moveit_python import PlanningSceneInterface
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Attach objects to link in the MoveIt planning scene.")
parser.add_argument("--name",
nargs="?",
help="Name of object to be attached")
parser.add_argument(
"--link",
nargs="?",
help="Name of link to where the object should be attached")
args = parser.parse_args()
if args.name:
if args.link:
rospy.init_node("attache_objects")
scene = PlanningSceneInterface("/base_link")
print("Attach Object with name: %s to Link: %s" %
(args.name, args.link))
scene.attachBox(args.name, 0.1, 0.1, 0.1, 0.0, 0.0, 0.0, args.link)
else:
parser.print_help()
class GripperInterfaceClient:
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
rospy.loginfo("...connected.")
self.scene = PlanningSceneInterface("base_link")
self.attached_object_publisher = rospy.Publisher(
"attached_collision_object", AttachedCollisionObject, queue_size=10
)
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
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.debug_index = -1
self.graspables = []
def execute_trajectory(self, trajectory):
goal_position = trajectory.points[-1].positions
print "==========", goal_position
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position[0] + goal_position[1]
command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this
self.client.send_goal(command_goal)
self.client.wait_for_result()
def move_to(self, goal_position, max_effort):
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position
command_goal.command.max_effort = max_effort
self.client.send_goal(command_goal)
self.client.wait_for_result()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
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, wait=False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, wait=False)
rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition.
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(
obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False
)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0] + 0.1,
1.5, # 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=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
# if obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07 or \
# obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07 or \
# obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07:
# continue
# has to be on table
# if obj.object.primitive_poses[0].position.z < 0.5:
# continue
return obj.object, obj.grasps
# nothing detected
return None, None
def set_target_object(self, primitive_type, target_pose, dimensions):
primitive_types = {"BOX": 1, "SPHERE": 2, "CYLINDER": 3, "CONE": 4}
self.sought_object = Object()
primitive = SolidPrimitive()
primitive.type = primitive_types[primitive_type.upper()]
primitive.dimensions = dimensions
self.sought_object.primitives.append(primitive)
p = Pose()
p.position.x = target_pose[0]
p.position.y = target_pose[1]
p.position.z = target_pose[2]
quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5])
p.orientation.x = quaternion[0]
p.orientation.y = quaternion[1]
p.orientation.z = quaternion[2]
p.orientation.w = quaternion[3]
self.sought_object.primitive_poses.append(p)
def find_graspable_object(self, tolerance=0.01):
self.updateScene()
self.current_grasping_target = None
for obj in self.objects:
# Must have grasps
if len(obj.grasps) < 1:
continue
# Compare to sought object
detected_object_dimensions = np.array(obj.object.primitives[0].dimensions)
sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions)
try:
difference = detected_object_dimensions - sought_object_dimensions
print "===DEBUG: Difference: ", difference
mag_diff = np.linalg.norm(difference)
print "===DEBUG: mag_diff: ", mag_diff
if mag_diff <= tolerance:
self.current_grasping_target = obj
tolerance = mag_diff
print "===DEBUG: Tolerance: ", tolerance
else:
rospy.loginfo("Object dimensions do not match. Trying another object...")
except:
rospy.loginfo("Object geometries do not match. Trying another object...")
if self.current_grasping_target is None:
tolerance += 0.01
self.find_graspable_object(tolerance=tolerance)
# Nothing detected
def computeGraspToPickMatrix(self):
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion)
translation = [pick_translation_distance, 0, 0]
rotation_matrix[[0, 1, 2], 3] = translation
pick_to_grasp_matrix = np.mat(rotation_matrix)
grasp_to_pick_matrix = pick_to_grasp_matrix.getI()
return grasp_to_pick_matrix
def computePlaceToBaseMatrix(self, place):
place_quaternion = place[3:]
rotation_matrix = quaternion_matrix(place_quaternion)
translation = place[0:3]
rotation_matrix[[0, 1, 2], 3] = translation
place_to_base_matrix = rotation_matrix
return place_to_base_matrix
def broadcastCurrentGraspingTargetTransform(self):
pose = self.current_grasping_target.object.primitive_poses[0]
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "base_link"
t.child_frame_id = "current_grasping_target"
t.transform.translation.x = pose.position.x
t.transform.translation.y = pose.position.y
t.transform.translation.z = pose.position.z
t.transform.rotation.x = pose.orientation.x
t.transform.rotation.y = pose.orientation.y
t.transform.rotation.z = pose.orientation.z
t.transform.rotation.w = pose.orientation.w
br.sendTransform(t)
def plan_pick(self):
success, pick_result = self.pickplace.pick_with_retry(
self.current_grasping_target.object.name,
self.current_grasping_target.grasps,
support_name=self.current_grasping_target.object.support_surface,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id="PRMkConfigDefault",
)
self.pick_result = pick_result
print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects()
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return pick_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Getting new grasps for replan...")
else:
rospy.logwarn("Planning failed. Getting new grasps for replan...")
self.find_graspable_object()
return self.plan_pick()
def plan_place(self, desired_pose):
places = list()
ps = PoseStamped()
# ps.pose = self.current_grasping_target.object.primitive_poses[0]
ps.header.frame_id = self.current_grasping_target.object.header.frame_id
grasp_to_pick_matrix = self.computeGraspToPickMatrix()
place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose)
grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix
position_vector = grasp2_to_place_matrix[0:-1, 3]
quaternion = quaternion_from_matrix(grasp2_to_place_matrix)
position_array = position_vector.getA1()
l = PlaceLocation()
direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector
l.place_pose.header.frame_id = ps.header.frame_id
l.place_pose.pose.position.x = position_array[0]
l.place_pose.pose.position.y = position_array[1]
l.place_pose.pose.position.z = position_array[2]
l.place_pose.pose.orientation.x = quaternion[0]
l.place_pose.pose.orientation.y = quaternion[1]
l.place_pose.pose.orientation.z = quaternion[2]
l.place_pose.pose.orientation.w = quaternion[3]
# copy the posture, approach and retreat from the grasp used
approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach)
approach.desired_distance /= 2.0
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 90 degrees in roll direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
print "DEBUG: Places: ", places[0]
success, place_result = self.pickplace.place_with_retry(
self.current_grasping_target.object.name,
places,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id="PRMkConfigDefault",
)
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return place_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Replanning...")
else:
rospy.logwarn("Planning failed. Replanning...")
desired_pose[2] += 0.05
return self.plan_place(desired_pose)
def recognizeAttachedObject(self):
print "========= Recognizing attached object"
name = self.current_grasping_target.object.name
size_x = self.current_grasping_target.object.primitives[0].dimensions[0]
size_y = self.current_grasping_target.object.primitives[0].dimensions[1]
size_z = self.current_grasping_target.object.primitives[0].dimensions[2]
(x, y, z) = (0.03, 0.0, 0.0)
link_name = "gripper_link"
touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"]
detach_posture = None
weight = 0.0
wait = True
"""
object_x = self.current_grasping_target.object.primitive_poses[0].position.x
object_y = self.current_grasping_target.object.primitive_poses[0].position.y
object_z = self.current_grasping_target.object.primitive_poses[0].position.z
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
grasp_position = self.pick_result.grasp.grasp_pose.pose.position
pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion)
grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array)
displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]]))
print displacement
attached_location = list()
attached_location.append(object_x - displacement[0,0])
attached_location.append(object_y - displacement[1,0])
attached_location.append(object_z - displacement[2,0])
print attached_location
quaternion = Quaternion()
quaternion.x = pick_to_grasp_quaternion[0]
quaternion.y = pick_to_grasp_quaternion[1]
quaternion.z = pick_to_grasp_quaternion[2]
quaternion.w = pick_to_grasp_quaternion[3]
pose = Pose()
pose.position.x = attached_location[0]
pose.position.y = attached_location[1]
pose.position.z = attached_location[2]
pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation
collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name,
self.current_grasping_target.object.primitives[0],
pose)
attached_collision_object = self.scene.makeAttached("gripper_link",
collision_object,
touch_links, detach_posture, 0.0)
self.attached_object_publisher.publish(attached_collision_object)
"""
self.scene.attachBox(
name,
size_x,
size_y,
size_z,
x,
y,
z,
link_name,
touch_links=touch_links,
detach_posture=detach_posture,
weight=weight,
wait=wait,
)
def removeCollisionObjects(self):
collision_object_names = self.scene.getKnownCollisionObjects()
print self.current_grasping_target.object.name
print collision_object_names
raw_input("press ENTER")
x = self.scene.getKnownAttachedObjects()
print x
raw_input("press Enter")
for name in collision_object_names:
if name != self.current_grasping_target.object.name:
self.scene.removeCollisionObject(name, wait=False)
def main():
rospy.init_node('arm_obstacle_demo')
wait_for_time()
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)
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
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
# and add an orientation constraint to pose 2:
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
kwargs = {
'allowed_planning_time': 100,
'execution_timeout': 100,
'num_planning_attempts': 15,
'replan': True,
'replan_attempts': 10
}
# Before moving to the first pose
planning_scene.removeAttachedObject('tray')
error = arm.move_to_pose(pose1, **kwargs)
# If the robot reaches the first pose successfully, then "attach" an object there
# Of course, you would have to close the gripper first and ensure that you grasped the object properly
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'] = 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')
class GripperInterfaceClient():
def __init__(self):
self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"]
self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action",
GripperCommandAction)
rospy.loginfo("Waiting for gripper_controller...")
self.client.wait_for_server()
rospy.loginfo("...connected.")
self.scene = PlanningSceneInterface("base_link")
self.attached_object_publisher = rospy.Publisher('attached_collision_object',
AttachedCollisionObject,
queue_size = 10)
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True)
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.debug_index = -1
self.graspables = []
def execute_trajectory(self, trajectory):
goal_position = trajectory.points[-1].positions
print '==========', goal_position
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position[0]+goal_position[1]
command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this
self.client.send_goal(command_goal)
self.client.wait_for_result()
def move_to(self, goal_position, max_effort):
command_goal = GripperCommandGoal()
command_goal.command.position = goal_position
command_goal.command.max_effort = max_effort
self.client.send_goal(command_goal)
self.client.wait_for_result()
def updateScene(self):
# find objects
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
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, wait=False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, wait=False)
rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition.
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0] + 0.1,
1.5, # 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 = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
#if obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07 or \
# obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07 or \
# obj.object.primitives[0].dimensions[0] < 0.05 or \
# obj.object.primitives[0].dimensions[0] > 0.07:
# continue
# has to be on table
#if obj.object.primitive_poses[0].position.z < 0.5:
# continue
return obj.object, obj.grasps
# nothing detected
return None, None
def set_target_object(self, primitive_type, target_pose, dimensions):
primitive_types = {"BOX":1, "SPHERE":2, "CYLINDER":3, "CONE":4}
self.sought_object = Object()
primitive = SolidPrimitive()
primitive.type = primitive_types[primitive_type.upper()]
primitive.dimensions = dimensions
self.sought_object.primitives.append(primitive)
p = Pose()
p.position.x = target_pose[0]
p.position.y = target_pose[1]
p.position.z = target_pose[2]
quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5])
p.orientation.x = quaternion[0]
p.orientation.y = quaternion[1]
p.orientation.z = quaternion[2]
p.orientation.w = quaternion[3]
self.sought_object.primitive_poses.append(p)
def find_graspable_object(self, tolerance=0.01):
self.updateScene()
self.current_grasping_target = None
for obj in self.objects:
# Must have grasps
if len(obj.grasps) < 1:
continue
# Compare to sought object
detected_object_dimensions = np.array(obj.object.primitives[0].dimensions)
sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions)
try:
difference = detected_object_dimensions - sought_object_dimensions
print "===DEBUG: Difference: " , difference
mag_diff = np.linalg.norm(difference)
print "===DEBUG: mag_diff: ", mag_diff
if mag_diff <= tolerance:
self.current_grasping_target = obj
tolerance = mag_diff
print "===DEBUG: Tolerance: ", tolerance
else:
rospy.loginfo("Object dimensions do not match. Trying another object...")
except:
rospy.loginfo("Object geometries do not match. Trying another object...")
if self.current_grasping_target is None:
tolerance += .01
self.find_graspable_object(tolerance = tolerance)
# Nothing detected
def computeGraspToPickMatrix(self):
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion)
translation = [pick_translation_distance, 0, 0]
rotation_matrix[[0,1,2],3] = translation
pick_to_grasp_matrix = np.mat(rotation_matrix)
grasp_to_pick_matrix = pick_to_grasp_matrix.getI()
return grasp_to_pick_matrix
def computePlaceToBaseMatrix(self, place):
place_quaternion = place[3:]
rotation_matrix = quaternion_matrix(place_quaternion)
translation = place[0:3]
rotation_matrix[[0,1,2], 3] = translation
place_to_base_matrix = rotation_matrix
return place_to_base_matrix
def broadcastCurrentGraspingTargetTransform(self):
pose = self.current_grasping_target.object.primitive_poses[0]
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "base_link"
t.child_frame_id = "current_grasping_target"
t.transform.translation.x = pose.position.x
t.transform.translation.y = pose.position.y
t.transform.translation.z = pose.position.z
t.transform.rotation.x = pose.orientation.x
t.transform.rotation.y = pose.orientation.y
t.transform.rotation.z = pose.orientation.z
t.transform.rotation.w = pose.orientation.w
br.sendTransform(t)
def plan_pick(self):
success, pick_result = self.pickplace.pick_with_retry(self.current_grasping_target.object.name,
self.current_grasping_target.grasps,
support_name = self.current_grasping_target.object.support_surface,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id='PRMkConfigDefault')
self.pick_result = pick_result
print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects()
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return pick_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Getting new grasps for replan...")
else:
rospy.logwarn("Planning failed. Getting new grasps for replan...")
self.find_graspable_object()
return self.plan_pick()
def plan_place(self, desired_pose):
places = list()
ps = PoseStamped()
#ps.pose = self.current_grasping_target.object.primitive_poses[0]
ps.header.frame_id = self.current_grasping_target.object.header.frame_id
grasp_to_pick_matrix = self.computeGraspToPickMatrix()
place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose)
grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix
position_vector = grasp2_to_place_matrix[0:-1,3]
quaternion = quaternion_from_matrix(grasp2_to_place_matrix)
position_array = position_vector.getA1()
l = PlaceLocation()
direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector
l.place_pose.header.frame_id = ps.header.frame_id
l.place_pose.pose.position.x = position_array[0]
l.place_pose.pose.position.y = position_array[1]
l.place_pose.pose.position.z = position_array[2]
l.place_pose.pose.orientation.x = quaternion[0]
l.place_pose.pose.orientation.y = quaternion[1]
l.place_pose.pose.orientation.z = quaternion[2]
l.place_pose.pose.orientation.w = quaternion[3]
# copy the posture, approach and retreat from the grasp used
approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach)
approach.desired_distance /= 2.0
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 90 degrees in roll direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
print "DEBUG: Places: ", places[0]
success, place_result = self.pickplace.place_with_retry(self.current_grasping_target.object.name,
places,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id='PRMkConfigDefault')
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return place_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Replanning...")
else:
rospy.logwarn("Planning failed. Replanning...")
desired_pose[2] += 0.05
return self.plan_place(desired_pose)
def recognizeAttachedObject(self):
print "========= Recognizing attached object"
name = self.current_grasping_target.object.name
size_x = self.current_grasping_target.object.primitives[0].dimensions[0]
size_y = self.current_grasping_target.object.primitives[0].dimensions[1]
size_z = self.current_grasping_target.object.primitives[0].dimensions[2]
(x, y, z) = (0.03, 0.0, 0.0)
link_name = "gripper_link"
touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"]
detach_posture = None
weight = 0.0
wait = True
"""
object_x = self.current_grasping_target.object.primitive_poses[0].position.x
object_y = self.current_grasping_target.object.primitive_poses[0].position.y
object_z = self.current_grasping_target.object.primitive_poses[0].position.z
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
grasp_position = self.pick_result.grasp.grasp_pose.pose.position
pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion)
grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array)
displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]]))
print displacement
attached_location = list()
attached_location.append(object_x - displacement[0,0])
attached_location.append(object_y - displacement[1,0])
attached_location.append(object_z - displacement[2,0])
print attached_location
quaternion = Quaternion()
quaternion.x = pick_to_grasp_quaternion[0]
quaternion.y = pick_to_grasp_quaternion[1]
quaternion.z = pick_to_grasp_quaternion[2]
quaternion.w = pick_to_grasp_quaternion[3]
pose = Pose()
pose.position.x = attached_location[0]
pose.position.y = attached_location[1]
pose.position.z = attached_location[2]
pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation
collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name,
self.current_grasping_target.object.primitives[0],
pose)
attached_collision_object = self.scene.makeAttached("gripper_link",
collision_object,
touch_links, detach_posture, 0.0)
self.attached_object_publisher.publish(attached_collision_object)
"""
self.scene.attachBox(name, size_x, size_y, size_z, x, y, z, link_name,
touch_links=touch_links, detach_posture=detach_posture, weight=weight,
wait=wait)
def removeCollisionObjects(self):
collision_object_names = self.scene.getKnownCollisionObjects()
print self.current_grasping_target.object.name
print collision_object_names
raw_input("press ENTER")
x = self.scene.getKnownAttachedObjects()
print x
raw_input("press Enter")
for name in collision_object_names:
if name != self.current_grasping_target.object.name:
self.scene.removeCollisionObject(name, wait=False)
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import argparse
import rospy
from moveit_python import PlanningSceneInterface
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Attach objects to link in the MoveIt planning scene.")
parser.add_argument("name", help="Name of the box to be attached")
parser.add_argument("link", help="Name of link to where the box should be attached")
parser.add_argument("x", type=float, help="X coordinate of center of box")
parser.add_argument("y", type=float, help="Y coordinate of center of box")
parser.add_argument("z", type=float, help="Z coordinate of center of box")
parser.add_argument("size_x", type=float, help="Size of box in x dimension")
parser.add_argument("size_y", type=float, help="Size of box in y dimension")
parser.add_argument("size_z", type=float, help="Size of box in z dimension")
args = parser.parse_args()
if args.name:
if args.link:
rospy.init_node("attach_box")
scene = PlanningSceneInterface("/base_link")
print("Attach Object with name: %s to Link: %s" % (args.name, args.link))
scene.attachBox(args.name, args.size_x, args.size_y, args.size_z,
args.x, args.y, args.z, args.link)
else:
parser.print_help()
def main():
print('We are running the main of arm_obstable_demo')
rospy.init_node('arm_obstacle_demo')
wait_for_time()
print(
"If planning scene fails to init, make sure you have launched move_group.launch"
)
planning_scene = PlanningSceneInterface("base_link")
# Create table object
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 object
planning_scene.removeCollisionObject('divider')
size_x = 0.3
size_y = 0.01
size_z = 0.2
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)
# Before moving to the first pose
planning_scene.removeAttachedObject('tray')
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)
kwargs = {
'allowed_planning_time': 15,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': False
}
error = arm.move_to_pose(pose1, **kwargs)
# If the robot reaches the first pose successfully, then "attach" an object there
# Of course, you would have to close the gripper first and ensure that you grasped the object properly
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')
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()
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()
