def free_spots_on_table(table, obj, orientation, blocking_objs, res=0.1):
object_pose = Pose()
object_pose.orientation = orientation
#these are the corners of the object in the object's frame
object_frame_corners = gt.bounding_box_corners(obj.shapes[0])
#rospy.loginfo('Corners are')
#for c in object_frame_corners: rospy.loginfo(str(c))
#these are the corners after we apply the rotation
header_frame_corners = [gt.transform_list(c, object_pose) for c in object_frame_corners]
#these are the corners relative to the table's lower left corner
table_frame_corners = [gt.inverse_transform_list(c, table.poses[0]) for c in header_frame_corners]
#the footprint of the object above the table
oxmax = max([c[0] for c in table_frame_corners])
oxmin = min([c[0] for c in table_frame_corners])
oymax = max([c[1] for c in table_frame_corners])
oymin = min([c[1] for c in table_frame_corners])
object_dims = (oxmax - oxmin, oymax - oymin)
rospy.loginfo('Object dimensions are '+str(object_dims))
obj_poses = free_spots_from_dimensions(table, object_dims, blocking_objs, res=res)
#return these as proper object poses with the object sitting on the table
#the height of the object in the world in its place orientation
obj_height = -1.0*min([c[2] for c in header_frame_corners])
above_table = gt.transform_list([0, 0, obj_height], table.poses[0])
place_height = above_table[2]
for pose in obj_poses:
pose.pose.position.z = place_height
pose.pose.orientation = copy.copy(orientation)
return obj_poses
def get_base(self, rotation):
bb = gt.bounding_box_corners(self.shape) # in frame of obj
transformed_bb = []
# transform bounding box to frame of the robot
for pt in bb:
transformed_pt = gt.transform_list(pt, rotation)
transformed_bb.append(transformed_pt)
# find base in this frame
transformed_bb.sort()
(minxminyminz, minxminymaxz, minxmaxyminz, minxmaxymaxz, \
maxxminyminz, maxxminymaxz, maxxmaxyminz, maxxmaxymaxz) = transformed_bb
return [minxminyminz, minxmaxyminz, maxxminyminz, maxxmaxyminz]
def block(self, obs, obj, release_pose):
buf = 0.03 #0.1#.2#0.05
# x = goal.pose.position.x
# y = goal.pose.position.y
# z = goal.pose.position.z
x = release_pose.pose.position.x + .05
y = release_pose.pose.position.y
z = release_pose.pose.position.z - 0.1
if obj.shape.type == Shape.MESH:
bb = gt.bounding_box_corners(obj.shape)
dims = [(bb[7][0] - bb[0][0]), (bb[7][1] - bb[0][1]),
(bb[7][2] - bb[0][2])]
x_displacement = dims[0] / 2 + obs.dimensions[0] / 2 + buf
y_displacement = dims[1] / 2 + obs.dimensions[1] / 2 + buf
z_displacement = dims[2] / 2 + obs.dimensions[2] / 2 + buf
else:
x_displacement = obj.shape.dimensions[0] / 2 + obs.dimensions[
0] / 2 + buf
y_displacement = obj.shape.dimensions[1] / 2 + obs.dimensions[
1] / 2 + buf
z_displacement = obj.shape.dimensions[2] / 2 + obs.dimensions[
2] / 2 + buf
mode = Mode(obj, self.goal)
dof = mode.get_degree_of_freedom(release_pose)
ordered_dof = dof.index_order()
block_poses = []
print "ORDERED DOF=", dof.index_order()
for i in dof.index_order():
block_pose = PoseStamped()
block_pose.header.frame_id = "/torso_lift_link"
if i == 0:
print "blocking x"
block_pose.pose.position.x = x - x_displacement + 0.07 #+ buf
block_pose.pose.position.y = y - 0.1
block_pose.pose.position.z = z + 0.2 #+ 0.12
block_pose.pose.orientation.w = 0.707
block_pose.pose.orientation.x = 0.0 #0.707#0.0
block_pose.pose.orientation.y = 0.707 #0.0#0.707
block_pose.pose.orientation.z = 0.0
elif i == 1:
print "blocking y"
block_pose.pose.position.x = x
block_pose.pose.position.y = y - y_displacement
block_pose.pose.position.z = z
block_pose.pose.orientation.w = 1.0
block_pose.pose.orientation.x = 0.0
block_pose.pose.orientation.y = 0.0
block_pose.pose.orientation.z = 0.0
elif i == 2:
block_pose.pose.position.x = x
block_pose.pose.position.y = y
block_pose.pose.position.z = z - z_displacement
block_pose.pose.orientation.w = 0.707
block_pose.pose.orientation.x = 0.0
block_pose.pose.orientation.y = 0.0
block_pose.pose.orientation.z = 0.707
block_poses.append(block_pose)
name = "block" + str(i)
# tools.make_marker(block_pose, namespace=name, mtype=Marker.ARROW, \
# scale=(0.1,0.1,0.05), color=(0.0,1.0,0.0,0.8))
return block_poses
def block(self, obs, i, dist, shape, goal):
'''
for a given degree of freedom, will find the best placement for the obstacle to block
that direction. moves the hand to that pose.
@type obs: arm_navigation_msgs.msg.collision_object
@param obs: obstacle (box representation of the other gripper)
@type i: int
@param i: index of the of the degree of freedom (x_trans,y_trans,z_trans,x_rot,y_rot,z_rot)
in the frame of the object
@type dist: float
@param dist: dist in the specified degree of freedom the obj will travel
@type shape: arm_navigation_msgs.msg.shape
@param shape: shape of obj
@type goal: PoseStamped
@param goal: goal pose of the object
@type boolean
@returns the success of the block move
'''
buf = 0.02 #0.1#.2#0.05
x = goal.pose.position.x
y = goal.pose.position.y
z = goal.pose.position.z
if shape.type == Shape.MESH:
bb = gt.bounding_box_corners(shape)
dims = [(bb[7][0] - bb[0][0]), (bb[7][1] - bb[0][1]),
(bb[7][2] - bb[0][2])]
x_displacement = dims[0] / 2 + obs.dimensions[0] / 2 + buf
y_displacement = dims[1] / 2 + obs.dimensions[1] / 2 + buf
z_displacement = dims[2] / 2 + obs.dimensions[2] / 2 + buf
else:
x_displacement = shape.dimensions[0] / 2 + obs.dimensions[
0] / 2 + buf
y_displacement = shape.dimensions[1] / 2 + obs.dimensions[
1] / 2 + buf
z_displacement = shape.dimensions[2] / 2 + obs.dimensions[
2] / 2 + buf
#print "shape dims="+str(shape.dimensions[1]/2)+" obs dims="+str(obs.dimensions[1]/2)
pose = PoseStamped()
pose.header.frame_id = "/torso_lift_link"
pose.pose.orientation.w = 0.707
pose.pose.orientation.x = 0.707
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0 #-0.707
if i == 0 or i == 3: # x axis
if dist == 2.0: # stick
pose.pose.orientation.w = 0.707
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.707
pose.pose.orientation.z = 0.0
x = x - x_displacement + .02
y = y + 0.09
z = z + 0.18
elif dist > 0: # tip
x = x + x_displacement
else:
x = x - x_displacement
if i == 1 or i == 4: # y axis
if dist > 0:
y = y + y_displacement
else:
y = y - y_displacement
if i == 2 or i == 5: # z axis
if dist > 0:
z = z + z_displacement
else:
z = z - z_displacement
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z + 0.02
self.make_marker(pose,namespace="block", mtype=Marker.ARROW, scale=(0.1,0.1,0.05), \
color=(0.0,1.0,0.0,0.8))
success = self.move("left_arm", pose, None, False)
rospy.loginfo("MOVE SUCCESS=%s", str(success))
return success
def free_spots_from_dimensions(table, object_dims, blocking_objs, res=0.1):
'''
Currently works only with a single shape
Assumes table is in xy plane
'''
#find the lower left corner of the table in the header frame
#we want everything relative to this point
table_corners = gt.bounding_box_corners(table.shapes[0])
lower_left = Pose()
lower_left.position = gt.list_to_point(table_corners[0])
lower_left.orientation.w = 1.0
#rospy.loginfo('Table corners are:')
#for c in table_corners: rospy.loginfo('\n'+str(c))
#rospy.loginfo('Lower left =\n'+str(lower_left))
#rospy.loginfo('Table header =\n'+str(table.header))
#rospy.loginfo('Table pose =\n'+str(table.poses[0]))
#this is the position of the minimum x, minimum y point in the table's header frame
table_box_origin = gt.transform_pose(lower_left, table.poses[0])
tr = gt.inverse_transform_list(gt.transform_list(table_corners[-1], table.poses[0]), table_box_origin)
table_dims = (tr[0], tr[1])
tbos = PoseStamped()
tbos.header = table.header
tbos.pose = table_box_origin
marray.markers.append(vt.marker_at(tbos, ns='table_origin', mtype=Marker.CUBE, r=1.0))
max_box = PointStamped()
max_box.header = table.header
max_box.point = gt.list_to_point(gt.transform_list([table_dims[0], table_dims[1], 0], table_box_origin))
marray.markers.append(vt.marker_at_point(max_box, ns='table_max', mtype=Marker.CUBE, r=1.0))
#rospy.loginfo('Table box origin is '+str(table_box_origin)+' dimensions are '+str(table_dims))
locs_on_table = _get_feasible_locs(table_dims, object_dims, res)
for i, l in enumerate(locs_on_table):
pt = Point()
pt.x = l[0]
pt.y = l[1]
mpt = PointStamped()
mpt.header = table.header
mpt.point = gt.transform_point(pt, table_box_origin)
marray.markers.append(vt.marker_at_point(mpt, mid=i, ns='locations', r=1.0, g=1.0, b=0.0))
feasible_locs = []
#check that these points really are on the table
for i, l in enumerate(locs_on_table):
pt = Point()
pt.x = l[0]
pt.y = l[1]
#this point is now defined relative to the origin of the table (rather than its minimum x, minimum y point)
table_pt = gt.inverse_transform_point(gt.transform_point(pt, table_box_origin), table.poses[0])
if point_on_table(table_pt, table.shapes[0]):
feasible_locs.append(l)
marray.markers[i+2].color.r = 0.0
marray.markers[i+2].color.b = 1.0
rospy.loginfo('Testing '+str(len(feasible_locs))+' locations')
if not feasible_locs:
return feasible_locs
forbidden=[]
for i, o in enumerate(blocking_objs):
ofcs = gt.bounding_box_corners(o.shapes[0])
objpose = tl.transform_pose(table.header.frame_id, o.header.frame_id, o.poses[0])
hfcs = [gt.transform_list(c, objpose) for c in ofcs]
tfcs = [gt.inverse_transform_list(c, table_box_origin) for c in hfcs]
oxmax = max([c[0] for c in tfcs])
oxmin = min([c[0] for c in tfcs])
oymax = max([c[1] for c in tfcs])
oymin = min([c[1] for c in tfcs])
forbidden.append(((oxmin, oymin), (oxmax - oxmin, oymax - oymin)))
#rospy.loginfo('\n'+str(forbidden[-1]))
ps = PoseStamped()
ps.header = table.header
ps.pose = objpose
ps = PoseStamped()
ps.header = table.header
ps.pose.position = gt.list_to_point(gt.transform_list([oxmin, oymin, 0], table_box_origin))
ps.pose.orientation.w = 1.0
marray.markers.append(vt.marker_at(ps, ns='forbidden', mid=i, r=1.0, b=0.0))
# Remove forbidden rectangles
for (bottom_left, dims) in forbidden:
_remove_feasible_locs(feasible_locs, object_dims,
bottom_left,
_add(bottom_left, dims),
res)
rospy.loginfo('There are '+str(len(feasible_locs))+' possible locations')
obj_poses = []
for i, fl in enumerate(feasible_locs):
table_frame_pose = Pose()
table_frame_pose.position.x = fl[0] + object_dims[0]/2.0
table_frame_pose.position.y = fl[1] + object_dims[1]/2.0
table_frame_pose.orientation.w = 1.0
pose = PoseStamped()
pose.header = copy.deepcopy(table.header)
pose.pose = gt.transform_pose(table_frame_pose, table_box_origin)
obj_poses.append(pose)
pt = PointStamped()
pt.header = table.header
pt.point = pose.pose.position
marray.markers.append(vt.marker_at_point(pt, mid=i, ns='final_locations', g=1.0, b=0.0))
#rospy.loginfo('Object poses are:')
#for op in obj_poses: rospy.loginfo(str(op))
for i in range(10):
vpub.publish(marray)
rospy.sleep(0.1)
return obj_poses
