def main(list_of_class_objs, basket_coordinates, planner_left):
"""
Main Script
input:first argument: a list of classX_objs. Ex: [class1_objs, class2_objs] where class1_objs contains the same kind of fruits
second argument: a list of baskets coordinates
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner_left = PathPlanner("left_arm")
home_coordinates = [0.544, 0.306, 0.3]
home = PoseStamped()
home.header.frame_id = "base"
#home x,y,z position
home_x = home_coordinates[0]
home_y = home_coordinates[1]
home_z = home_coordinates[2]
home.pose.position.x = home_x
home.pose.position.y = home_y
home.pose.position.z = home_z
#Orientation as a quaternion
home.pose.orientation.x = 1.0
home.pose.orientation.y = 0.0
home.pose.orientation.z = 0.0
home.pose.orientation.w = 0.0
intermediate_obstacle = PoseStamped()
intermediate_obstacle.header.frame_id = "base"
intermediate_obstacle.pose.position.x = 0
intermediate_obstacle.pose.position.y = 0
intermediate_obstacle.pose.position.z = 0.764
intermediate_obstacle.pose.orientation.x = 1
intermediate_obstacle.pose.orientation.y = 0
intermediate_obstacle.pose.orientation.z = 0
intermediate_obstacle.pose.orientation.w = 0
intermediate_size = [1, 1, 0.2]
left_gripper = robot_gripper.Gripper('left')
print('Calibrating...')
left_gripper.calibrate()
while not rospy.is_shutdown():
try:
#GO HOME
plan = planner_left.plan_to_pose(home, list())
raw_input("Press <Enter> to move the left arm to home position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
for i, classi_objs in enumerate(list_of_class_objs):
#x, y,z, orientation of class(basket)
print("processing class: " + str(i))
classi_position = basket_coordinates[i]
classi = PoseStamped()
classi.header.frame_id = "base"
classi_x = classi_position[0]
classi_y = classi_position[1]
classi_z = classi_position[2]
classi.pose.position.x = classi_x
classi.pose.position.y = classi_y
classi.pose.position.z = classi_z +.1
classi.pose.orientation.x = 1.0
classi.pose.orientation.y = 0.0
classi.pose.orientation.z = 0.0
classi.pose.orientation.w = 0.0
for fruit in classi_objs:
gripper_yaw = fruit[3]
fruit_x = fruit[0]
fruit_y = fruit[1]
fruit_z = fruit[2]
gripper_orientation = euler_to_quaternion(gripper_yaw)
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
gripper_orientation_x = gripper_orientation[0]
gripper_orientation_y = gripper_orientation[1]
gripper_orientation_z = gripper_orientation[2]
gripper_orientation_w = gripper_orientation[3]
orien_const.orientation.x = gripper_orientation_x
orien_const.orientation.y = gripper_orientation_y
orien_const.orientation.z = gripper_orientation_z
orien_const.orientation.w = gripper_orientation_w
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose)
#intermidiate_to_fruit stage: move to the top of the fruit location and open the gripper
intermidiate_to_fruit = PoseStamped()
intermidiate_to_fruit.header.frame_id = "base"
#x,y,z position
intermidiate_to_fruit.pose.position.x = fruit_x
intermidiate_to_fruit.pose.position.y = fruit_y
intermidiate_to_fruit.pose.position.z = home_z - .1
print("Trying to reach intermeidiate_to_fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z))
intermidiate_to_fruit.pose.orientation.x = gripper_orientation_x
intermidiate_to_fruit.pose.orientation.y = gripper_orientation_y
intermidiate_to_fruit.pose.orientation.z = gripper_orientation_z
intermidiate_to_fruit.pose.orientation.w = gripper_orientation_w
plan = planner_left.plan_to_pose(intermidiate_to_fruit, list())
raw_input("Press <Enter> to move the left arm to intermidiate_to_fruit position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
planner_left.remove_obstacle("intermediate")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
#go down to the actual height of the fruit and close gripper
fruitobs = generate_obstacle(fruit_x, fruit_y)
fruit = PoseStamped()
fruit.header.frame_id = "base"
#x,y,z position
fruit.pose.position.x = fruit_x
fruit.pose.position.y = fruit_y
fruit.pose.position.z = fruit_z
print("Trying to reach fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z))
#Orientation as a quaternion
fruit.pose.orientation.x = gripper_orientation_x
fruit.pose.orientation.y = gripper_orientation_y
fruit.pose.orientation.z = gripper_orientation_z
fruit.pose.orientation.w = gripper_orientation_w
plan = planner_left.plan_to_pose(fruit, [orien_const])
raw_input("Press <Enter> to move the left arm to fruit position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
fruitobs()
except Exception as e:
print e
else:
break
#close the gripper
print('Closing...')
left_gripper.close()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
fruitobs = generate_obstacle(fruit_x, fruit_y)
#intermidiate_to_basket stage1: Lift up to a height higher than the height of the basket
firt_intermidiate_to_class_i = PoseStamped()
firt_intermidiate_to_class_i.header.frame_id = "base"
#x, y, and z position
firt_intermidiate_to_class_i.pose.position.x = fruit_x
firt_intermidiate_to_class_i.pose.position.y = fruit_y
firt_intermidiate_to_class_i.pose.position.z = classi_z + 0.25
print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2))
#Orientation as a quaternion
firt_intermidiate_to_class_i.pose.orientation.x = 1.0
firt_intermidiate_to_class_i.pose.orientation.y = 0.0
firt_intermidiate_to_class_i.pose.orientation.z = 0.0
firt_intermidiate_to_class_i.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(firt_intermidiate_to_class_i, list())
raw_input("Press <Enter> to move the left arm to first_intermidiate_to_class_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
fruitobs()
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose)
#intermidiate_to_basket stage2: Move to the top of the basket
intermidiate_to_class_i = PoseStamped()
intermidiate_to_class_i.header.frame_id = "base"
#x, y, and z position
intermidiate_to_class_i.pose.position.x = classi_x
intermidiate_to_class_i.pose.position.y = classi_y
intermidiate_to_class_i.pose.position.z = classi_z + 0.2
print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2))
#Orientation as a quaternion
intermidiate_to_class_i.pose.orientation.x = 1.0
intermidiate_to_class_i.pose.orientation.y = 0.0
intermidiate_to_class_i.pose.orientation.z = 0.0
intermidiate_to_class_i.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(intermidiate_to_class_i, list())
raw_input("Press <Enter> to move the left arm to second_intermidiate_to_class_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
planner_left.remove_obstacle("intermediate")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
#basket stage: put the fruit in the basket
classi_obs = generate_obstacle(classi_x, class_y)
plan = planner_left.plan_to_pose(classi, list())
raw_input("Press <Enter> to move the left arm to sclass_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
classi_obs()
except Exception as e:
print e
else:
break
#Open the gripper
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
#intermidiate to home stage: lifting up to the home position height
intermidiate_to_home_1 = PoseStamped()
intermidiate_to_home_1.header.frame_id = "base"
#x, y, and z position
intermidiate_to_home_1.pose.position.x = classi_x
intermidiate_to_home_1.pose.position.y = classi_y
intermidiate_to_home_1.pose.position.z = home_z
#Orientation as a quaternion
intermidiate_to_home_1.pose.orientation.x = 1.0
intermidiate_to_home_1.pose.orientation.y = 0.0
intermidiate_to_home_1.pose.orientation.z = 0.0
intermidiate_to_home_1.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(intermidiate_to_home_1, list())
raw_input("Press <Enter> to move the right arm to intermidiate_to_home_1 position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
planner_left.shutdown()
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
controller = Controller(Kp, Ki, Kd, Kw, Limb('right'))
##
## Add the obstacle to the planning scene here
##
"""
X = 0.5, Y = 0.00, Z = 0.00
X = 0.00, Y = 0.00, Z = 0.00, W = 1.00
X = 0.40, Y = 1.20, Z = 0.10
"""
X = 1.20
Y = 0.10
Z = 0.50
box_size = np.array([X, Y, Z])
box_name = "box"
box_pose = PoseStamped()
box_pose.header.frame_id = "base"
#x, y, and z position
box_pose.pose.position.x = 0.9
box_pose.pose.position.y = 0.0
box_pose.pose.position.z = Z / 2
#Orientation as a quaternion
box_pose.pose.orientation.x = 0.0
box_pose.pose.orientation.y = 0.0
box_pose.pose.orientation.z = 0.0
box_pose.pose.orientation.w = 1.0
#planner.add_box_obstacle(box_size, box_name, box_pose)
planner.remove_obstacle(box_name)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
orientation_constraints = list() #[orien_const]
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.4
goal_1.pose.position.y = -0.3
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_1, orientation_constraints)
raw_input(
"Press <Enter> to move the right arm to goal pose 1: ")
if not controller.execute_path(
plan): #planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_2, orientation_constraints)
raw_input(
"Press <Enter> to move the right arm to goal pose 2: ")
if not controller.execute_path(
plan): #planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_3, orientation_constraints)
raw_input(
"Press <Enter> to move the right arm to goal pose 3: ")
if not controller.execute_path(
plan): #planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def action(hand, m, n):
'''
Left hand or right hand
'''
left_gripper = robot_gripper.Gripper('left')
left_gripper.calibrate()
left_gripper.command_position(0)
p0x = 0.768
p0y = 0.220
p0z = -0.121
p1x = 0.378
p1y = -0.200
p1z = -0.121
arm = hand + "_arm"
planner = PathPlanner(arm)
if hand == "left":
tool = "marker"
else:
tool = "eraser"
init_px = p0x #0.795#
init_py = p0y
x_interval = (p1x - p0x) / 9 #-0.045
y_interval = (p1y - p0y) / 9 #-0.046
if hand == "left":
px = init_px + x_interval * m
py = init_py + y_interval * n
pz = -0.200
else:
px = init_px + (x_interval) * m - 0.02
py = init_py + (y_interval + 0.001) * n - 0.017
pz = -0.224
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
planner.remove_obstacle("low_table")
planner.remove_obstacle("high_table")
planner.remove_obstacle("front")
planner.remove_obstacle("leftmost")
planner.remove_obstacle("rightmost")
planner.remove_obstacle("eye")
def setObstacle(x, y, z, frame):
ob = PoseStamped()
ob.header.frame_id = frame
ob.pose.position.x = x
ob.pose.position.y = y
ob.pose.position.z = z
ob.pose.orientation.x = 0.0
ob.pose.orientation.y = 0.0
ob.pose.orientation.z = 0.0
ob.pose.orientation.w = 1.0
return ob
ob_low_table = setObstacle(px, py, pz - 0.018, "base")
planner.add_box_obstacle((1.50, 1.20, 0.01), "low_table", ob_low_table)
ob_leftmost = setObstacle(0.0, 1.1, 0.0, "base")
planner.add_box_obstacle((1.50, 0.01, 1.50), "leftmost", ob_leftmost)
ob_rightmost = setObstacle(0.0, -1.1, 0.0, "base")
planner.add_box_obstacle((1.50, 0.01, 1.50), "rightmost", ob_rightmost)
ob_front = setObstacle(1.2, 0.0, 0.0, "base")
planner.add_box_obstacle((0.01, 3.00, 1.50), "front", ob_front)
ob_back = setObstacle(-0.23, 0.0, 0.0, "base")
planner.add_box_obstacle((0.01, 3.00, 1.00), "back", ob_back)
def plan_move(x, y, z, px, py, pz, pw):
while not rospy.is_shutdown():
try:
goal = PoseStamped()
goal.header.frame_id = "base"
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
goal.pose.orientation.x = px
goal.pose.orientation.y = py
goal.pose.orientation.z = pz
goal.pose.orientation.w = pw
plan = planner.plan_to_pose(goal, list())
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
'''
Move to target
'''
print("move to target:{},{},{}".format(px, py, pz))
plan_move(px, py, pz, 0.0, -1.0, 0.0, 0.0)
'''
Rotate the hand with two circles and moves inward more and more
'''
shou = baxter_interface.Limb(hand)
joints = shou.joint_names()
joints_speed = dict()
for name in joints:
joints_speed[name] = 0
save_joint = hand + "_s1"
which_joint = hand + "_w2"
joints_speed[save_joint] = -0.018
if hand == "left":
round_num = 3
else:
round_num = 1
for i in range(round_num):
left_gripper.command_position(60 - 20 * i)
joints_speed[which_joint] = 4.6
start = time.time()
while time.time() - start < 1.8:
shou.set_joint_velocities(joints_speed)
print("{}: tick {}".format(tool, i))
joints_speed[which_joint] = -4.6
start = time.time()
while time.time() - start < 1.8:
shou.set_joint_velocities(joints_speed)
print("{}: tock {}".format(tool, i))
'''
Reset arms back to origin position
'''
print("Move to origin")
if hand == "left":
plan_move(0.300, 0.854, 0.116, 0.0, -1.0, 0.0, 0.0)
else:
plan_move(0.300, -0.854, 0.116, 0.0, -1.0, 0.0, 0.0)
def main():
planner = PathPlanner("left_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# #Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
filename = "test_cup.jpg"
table_height = 0
# get positions of cups
cups = find_cups(filename)
start_cup = cups[0]
goal_cup = cups[1]
cup_pos = [0.756, 0.300, -0.162] # position of cup
goal_pos = [0.756, 0.600, -0.162] # position of goal cup
init_pos = [cup_pos[0] - .2, cup_pos[1],
cup_pos[2] + .05] # initial position
start_pos = [init_pos[0] + .2, init_pos[1],
init_pos[2]] # start position, pick up cup
up_pos = [goal_pos[0], goal_pos[1] - .05,
goal_pos[2] + .1] # up position, ready to tilt
end_pos = start_pos # end position, put down cup
final_pos = init_pos # final position, away from cup
# add table obstacle
table_size = np.array([.4, .8, .1])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
#x, y, and z position
table_pose.pose.position.x = 0.5
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = 0.0
planner.add_box_obstacle(table_size, 'table', table_pose)
# add goal cup obstacle
goal_cup_size = np.array([.1, .1, .2])
goal_cup_pose = PoseStamped()
goal_cup_pose.header.frame_id = "base"
#x, y, and z position
goal_cup_pose.pose.position.x = goal_pos[0]
goal_cup_pose.pose.position.y = goal_pos[1]
goal_cup_pose.pose.position.z = goal_pos[2]
planner.add_box_obstacle(goal_cup_size, 'goal cup', goal_cup_pose)
# add first cup obstacle
cup_size = np.array([.1, .1, .2])
cup_pose = PoseStamped()
cup_pose.header.frame_id = "base"
#x, y, and z position
cup_pose.pose.position.x = cup_pos[0]
cup_pose.pose.position.y = cup_pos[1]
cup_pose.pose.position.z = cup_pos[2]
planner.add_box_obstacle(cup_size, 'cup', cup_pose)
while not rospy.is_shutdown():
left_gripper = robot_gripper.Gripper('left')
print('Calibrating...')
left_gripper.calibrate()
pose = PoseStamped()
pose.header.frame_id = "base"
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = -1.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = init_pos[0]
pose.pose.position.y = init_pos[1]
pose.pose.position.z = init_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to initial pose: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# grab cup
# need to make sure it doesn't knock it over
planner.remove_obstacle('cup')
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = start_pos[0]
pose.pose.position.y = start_pos[1]
pose.pose.position.z = start_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to grab the cup: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
print('Closing...')
left_gripper.close()
rospy.sleep(2)
except Exception as e:
print e
else:
break
# position to pour
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = up_pos[0]
pose.pose.position.y = up_pos[1]
pose.pose.position.z = up_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to above the goal cup: "
)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# pouring
raw_input("Press <Enter> to move the left arm to begin pouring: ")
for degree in range(180):
while not rospy.is_shutdown():
try:
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = -1.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
plan = planner.plan_to_pose(pose, [])
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# move cup away from goal on table
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = end_pos[0]
pose.pose.position.y = end_pos[1]
pose.pose.position.z = end_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to away from the goal cup: "
)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# let go of cup on table
# need to make sure to not to hit cup
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = final_pos[0]
pose.pose.position.y = final_pos[1]
pose.pose.position.z = final_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to let go of the cup: "
)
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# get new cup position
new_cup_pos = []
# readd the cup obstacle
cup_pose.pose.position.x = new_cup_pos[0]
cup_pose.pose.position.y = new_cup_pos[1]
cup_pose.pose.position.z = new_cup_pos[2]
planner.add_box_obstacle(cup_size, 'cup', cup_pose)
message = rospy.wait_for_message("/colors_and_position", ColorAndPositionPairs)
cubes = message.pairs
# cubes = [cubes[0]]
# cubes[0].x = 630
# cubes[0].y = 442
cubes = process_cubes(cubes, tfBuffer, listener)
first_cube = cubes[0]
cube_size = np.array([0.02, 0.02, 0.02])
cube_pose = PoseStamped()
cube_pose.header.frame_id = "base"
cube_pose.pose.position.x = first_cube[0]
cube_pose.pose.position.z = -0.22 # for baxter
cube_pose.pose.orientation.w = 1.0
if(cnt > 0):
planner.remove_obstacle("cube")
planner.add_box_obstacle(cube_size, "cube", cube_pose)
cnt += 1
#print("found cubes", cubes)
table_height = -0.24
# cubes[0] = (0.41, -0.4, 100)
# print("first cube", cubes[0])
# cubes = [cubes[0]]
cube_path = get_cube_path_hue(cubes, table, table_height)
print("cube_path", cube_path)
manipulator_path = get_manipulator_path(cube_path, default_coords)
positions = [x.pose.position for x in manipulator_path]
