def main():
# find objects on the table
rospy.loginfo("Waiting for find_cluster_bounding_box2 service")
find_box = rospy.ServiceProxy("/find_cluster_bounding_box", FindClusterBoundingBox)
detector = TablewareDetection(table_thickness=0.1)
det = detector.detect_objects(point_head_at=None)
table = det.table
table_dims = (table.x_max - table.x_min, table.y_max - table.y_min)
rospy.loginfo("Table dims: %s", table_dims)
objs = []
for goal in det.pickup_goals:
pointcloud = goal.target.cluster
box = find_box(pointcloud)
rospy.loginfo("----- Found label %s", goal.label)
dims = box.box_dims
pos = (box.pose.pose.position.x, box.pose.pose.position.y)
obj = ( (pos[0] - dims.x/2 - table.x_min, pos[1] - dims.y/2 - table.y_min), (dims.x, dims.y))
rospy.loginfo("Obj tuple: %s", obj)
objs.append(obj)
obj_dim = [(0.1, 0.10)]
spots = free_spots(table_dims, obj_dim, 0.01, objs)
rospy.loginfo("Spots: %s", spots)
def main():
# find objects on the table
head_point = PointStamped()
head_point.header.frame_id = '/torso_lift_link'
head_point.point.x = 0.5
head_point = None
detector = TablewareDetection(table_thickness=0.1)
det = detector.detect_objects(point_head_at=head_point)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
# pick up the first object that we found
if len(sys.argv) > 1:
arm = sys.argv[1]
else:
arm = "right_arm"
rospy.loginfo("Trying arm: %s", arm)
pickplace = PickPlace()
pg = det.pickup_goals[0]
pg.arm_name = arm
try:
pickplace.pickup(pg)
except Exception as e:
rospy.logerr("No grasping, err: %s", e)
return
gripper = Gripper(arm)
gripper.open()
def pickplace_main():
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
wi = WorldInterface()
psi = get_planning_scene_interface()
add_map_tables(wi)
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal = DARRTGoal()
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.lift_distance = 0.3
goal.pickup_goal.arm_name = 'right_arm'
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
place_pose_stamped.pose.position.x = -1.3
place_pose_stamped.pose.position.y = 0.7
if 'graspable' in goal.pickup_goal.collision_object_name:
place_pose_stamped.pose.position.z = 0.8
else:
place_pose_stamped.pose.position.z = 0.75
place_pose_stamped.pose.orientation.w = 1.0
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = 'serving_table',
collision_object_name = goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.object_sampling_fraction = 0.3
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.execute = False
goal.planning_time = 30
goal.tries = 10
goal.goal_bias = 0.2
average_time = 0.0
rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
rospy.loginfo('Restart after '+str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error '+str(result.error_code)+'. We had '+str(i)+
' successful trials with an average time of '+str(average_time/float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
def main():
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
client.wait_for_server()
goal = DARRTGoal()
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.arm_name = 'right_arm'
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = 'map'
place_pose_stamped.pose.position.x -= 1.3
place_pose_stamped.pose.position.y -= 0.3
place_pose_stamped.pose.position.z = 0.98
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = det.table_name,
collision_object_name = goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.min_grasp_distance_from_surface = 0.17
goal.object_sampling_fraction = 0.9
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.planning_time = 6000
goal.tries = 1
goal.goal_bias = 0.2
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code == result.SUCCESS:
#pickle everything!! great excitement
filename = 'pickplace_and_objects.pck'
rospy.loginfo('Pickling to '+filename)
#the last bit allows us to recreate the planning
pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb'))
marray = MarkerArray()
if result.error_code == result.SUCCESS:
for t in result.primitive_trajectories:
marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers
for (i, m) in enumerate(marray.markers):
m.id = i
(m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1)
while not rospy.is_shutdown():
pub.publish(marray)
rospy.sleep(0.1)
def main():
det = TablewareDetection()
head_point = PointStamped()
head_point.header.frame_id = '/torso_lift_link'
head_point.point.x = 0.6
res = det.detect_objects(point_head_at=head_point)
rospy.loginfo('Detected objects with the following labels and poses:')
for pg in res.pickup_goals:
rospy.loginfo('Label: '+str(pg.label)+', Pose:\n'+str(pg.object_pose_stamped))
return
def main():
# find objects on the table
rospy.loginfo("Waiting for find_cluster_bounding_box2 service")
find_box = rospy.ServiceProxy("/find_cluster_bounding_box", FindClusterBoundingBox)
detector = TablewareDetection(table_thickness=0.1)
det = detector.detect_objects(point_head_at=None)
if not det.pickup_goals:
rospy.loginfo("Nothing to pick up!")
return
for goal in det.pickup_goals:
pointcloud = goal.target.cluster
box = find_box(pointcloud)
rospy.loginfo("----- Found label %s", goal.label)
dims = box.box_dims
rospy.loginfo("Bounding box dims: %.2f, %.2f, %.2f", dims.x, dims.y, dims.z)
area = dims.x * dims.y * dims.z
squareness = min(dims.x, dims.y) / max(dims.x, dims.y)
rospy.loginfo("Area: %f", area)
rospy.loginfo("Squareness: %f", squareness)
def plate_main():
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
wi = WorldInterface()
psi = get_planning_scene_interface()
detector = TablewareDetection()
plate = wi.collision_object('plate')
good_detection = False
goal = DARRTGoal()
if plate:
rospy.loginfo('Use last detection?')
good_detection = (raw_input() == 'y')
ops = PoseStamped()
ops.header = copy.deepcopy(plate.header)
ops.pose = copy.deepcopy(plate.poses[0])
goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id),
object_pose_stamped=ops, collision_object_name='plate',
collision_support_surface_name='serving_table')
while not good_detection:
det = detector.detect_objects(add_table_to_map=False,
add_objects_as_mesh=False, table_name='serving_table')
goal.pickup_goal = get_plate(det.pickup_goals, det.table.pose, wi)
psi.reset()
if not goal.pickup_goal:
rospy.loginfo('Nothing to pick up!')
rospy.loginfo('Good detection?')
good_detection = (raw_input() == 'y')
if not goal.pickup_goal:
rospy.loginfo('Nothing to pick up!')
return
add_map_tables(wi)
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal.pickup_goal.arm_name = 'right_arm'
goal.pickup_goal.desired_grasps = plate_grasps(goal.pickup_goal.object_pose_stamped,
goal.pickup_goal.target.reference_frame_id)
goal.pickup_goal.lift.desired_distance = 0.3
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
# place_pose_stamped.pose.position.x = 1.3
# place_pose_stamped.pose.position.y = -0.3
# place_pose_stamped.pose.position.z = 1.01
place_pose_stamped.pose.position.x = 0.0
place_pose_stamped.pose.position.y = 0.0
place_pose_stamped.pose.position.z = 0.76
place_pose_stamped.pose.orientation.w = 1.0
# place_pose_stamped.pose.orientation.x = 0.1
# place_pose_stamped.pose.orientation.y = 0.1
# place_pose_stamped.pose.orientation.w = np.sqrt(0.98)
#place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
#place_pose_stamped.pose.position.x -= 0.2
#place_pose_stamped.pose.position.y -= 0.2
#place_pose_stamped.pose.position.z += 0.2
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = 'dirty_table',
collision_object_name = goal.pickup_goal.collision_object_name)
goal.place_goal.approach.direction.header.frame_id = wi.world_frame
goal.place_goal.approach.direction.vector.x = np.sqrt(0.18)
goal.place_goal.approach.direction.vector.y = -np.sqrt(0.18)
goal.place_goal.approach.direction.vector.z = -0.8
goal.place_goal.approach.desired_distance = 0.2
goal.primitives = [goal.PICK, goal.PLACE, goal.PUSH, goal.BASE_TRANSIT]
goal.min_grasp_distance_from_surface = 0.19
goal.object_sampling_fraction = 0.7
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.execute = False
goal.planning_time = 6000
goal.tries = 1
goal.goal_bias = 0.2
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
marray = MarkerArray()
if result.error_code == result.SUCCESS:
#pickle everything!! great excitement
filename = 'trajectory_and_objects.pck'
rospy.loginfo('Pickling to '+filename)
#the last bit allows us to recreate the planning
pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb'))
rospy.loginfo('Successful write')
for t in result.primitive_trajectories:
marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers
for (i, m) in enumerate(marray.markers):
m.id = i
(m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1)
while not rospy.is_shutdown():
pub.publish(marray)
rospy.sleep(0.1)
def main():
#Find an object to pick up
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
#DARRT action client
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
rospy.loginfo('Waiting for DARRT action')
client.wait_for_server()
rospy.loginfo('Found DARRT action')
#DARRTGoal
goal = DARRTGoal()
#pickup goal (from the tabletop detection)
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.arm_name = 'right_arm'
#place goal (move left 20 cm)
place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
place_pose_stamped.pose.position.y += 0.2
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name,
[place_pose_stamped],
collision_support_surface_name =
det.table_name,
collision_object_name =
goal.pickup_goal.collision_object_name)
#Send the goal to the action
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
#Check the result
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Planning failed. Error code: '+str(result.error_code))
return
rospy.loginfo('Planning succeeded!')
#at this point the planning is done.
#now we execute and visualize the plan
#visualize trajectory
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
marray = get_trajectory_markers(result.primitive_trajectories)
for i in range(10):
pub.publish(marray)
rospy.sleep(0.05)
#Executor is a Python class for executing DARRT plans
executor = Executor()
#execute trajectory
rospy.loginfo('Press enter to execute')
raw_input()
executor.execute_trajectories(result)
def main():
#Find an object to pick up
psi = get_planning_scene_interface()
psi.reset()
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
psi.reset()
#DARRT action client
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
rospy.loginfo('Waiting for DARRT action')
client.wait_for_server()
rospy.loginfo('Found DARRT action')
#DARRTGoal
goal = DARRTGoal()
#pickup goal (from the tabletop detection)
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.arm_name = 'right_arm'
#world interface to tell us whether we're using the
#map or odom_combined frame
wi = WorldInterface()
#place goal (move far away)
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
place_pose_stamped.pose.position.x = -2
place_pose_stamped.pose.position.y = 0
place_pose_stamped.pose.position.z = 0.85
place_pose_stamped.pose.orientation.w = 1.0
goal.place_goal = PlaceGoal(
goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name=det.table_name,
collision_object_name=goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.planning_time = 30
goal.tries = 3
goal.debug_level = 1
goal.object_sampling_fraction = 0.5
#Send the goal to the action
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
#Check the result
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Planning failed. Error code: ' + str(result.error_code))
return
rospy.loginfo('Planning succeeded!')
#at this point the planning is done.
#now we execute and visualize the plan
#visualize trajectory
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
marray = get_trajectory_markers(result.primitive_trajectories)
for i in range(10):
pub.publish(marray)
rospy.sleep(0.05)
#Executor is a Python class for executing DARRT plans
executor = Executor()
#execute trajectory
rospy.loginfo('Press enter to execute')
raw_input()
executor.execute_trajectories(result)
rospy.loginfo('Successfully executed!')
