def cb_model_states(states_msg):
global base_tf, camera_base_tf, robot_name
for n, p in zip(states_msg.name, states_msg.pose):
if n == robot_name:
base_tf = tf_to_np(
pb.Transform(
pb.Quaternion(p.orientation.x, p.orientation.y,
p.orientation.z, p.orientation.w),
pb.Vector3(p.position.x, p.position.y, p.position.z)))
try:
trans, rot = tf_listener.lookupTransform(
'/base_link', '/head_camera_rgb_optical_frame', rospy.Time(0))
#print('{} {} {}\n{} {} {} {}'.format(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3]))
camera_base_tf = tf_to_np(
pb.Transform(pb.Quaternion(*rot), pb.Vector3(*trans)))
# fake_observation()
except (ValueError, tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
def cb_link_states(states_msg):
global base_tf, camera_base_tf, robot_name
for n, p in zip(states_msg.name, states_msg.pose):
if n in phy_objects:
#print('Updating pose of {}'.format(n))
phy_objects[n].transform = pb.Transform(
pb.Quaternion(p.orientation.x, p.orientation.y,
p.orientation.z, p.orientation.w),
pb.Vector3(p.position.x, p.position.y, p.position.z))
world.update_aabbs()
visualizer.begin_draw_cycle()
visualizer.draw_world('objects', world)
visualizer.render('objects')
def post_physics_update(self, simulator, deltaT):
"""Implements post physics step behavior.
:type simulator: BasicSimulator
:type deltaT: float
"""
if not self._enabled:
return
self._last_update += deltaT
if self._last_update >= self._update_wait:
self._last_update = 0
cf_tuple = self.multibody.get_link_state(self.camera_link).worldFrame
camera_frame = gm.frame3_quaternion(cf_tuple.position.x, cf_tuple.position.y, cf_tuple.position.z, *cf_tuple.quaternion)
cov_proj = gm.rot_of(camera_frame)[:3, :3]
inv_cov_proj = cov_proj.T
out = PSAMsg()
if self.visualizer is not None:
self.visualizer.begin_draw_cycle()
poses = []
for name, body in simulator.bodies.items():
if body == self.multibody:
continue
if isinstance(body, MultiBody):
poses_to_process = [('{}/{}'.format(name, l), body.get_link_state(l).worldFrame) for l in body.links]
else:
poses_to_process = [(name, body.pose())]
for pname, pose in poses_to_process:
if not pname in self.message_templates:
msg = PoseStampedMsg()
msg.header.frame_id = pname
self.message_templates[pname] = msg
else:
msg = self.message_templates[pname]
obj_pos = gm.point3(*pose.position)
c2o = obj_pos - gm.pos_of(camera_frame)
dist = gm.norm(c2o)
if dist < self.far and dist > self.near and gm.dot_product(c2o, gm.x_of(camera_frame)) > gm.cos(self.fov * 0.5) * dist:
noise = 2 ** (self.noise_exp * dist) - 1
(n_quat, ) = np_random_quat_normal(1, 0, noise)
(n_trans, ) = np_random_normal_offset(1, 0, noise)
n_pose = pb.Transform(pb.Quaternion(*pose.quaternion), pb.Vector3(*pose.position)) *\
pb.Transform(pb.Quaternion(*n_quat), pb.Vector3(*n_trans[:3]))
if self.visualizer is not None:
poses.append(transform_to_matrix(n_pose))
msg.pose.position.x = n_pose.origin.x
msg.pose.position.y = n_pose.origin.y
msg.pose.position.z = n_pose.origin.z
msg.pose.orientation.x = n_pose.rotation.x
msg.pose.orientation.y = n_pose.rotation.y
msg.pose.orientation.z = n_pose.rotation.z
msg.pose.orientation.w = n_pose.rotation.w
out.poses.append(msg)
self.publisher.publish(out)
if self.visualizer is not None:
self.visualizer.draw_poses('debug', gm.se.eye(4), 0.1, 0.02, poses)
self.visualizer.render()
def fake_observation():
if base_tf is None:
print('Can not generate observation, base_tf is None')
return []
if camera_base_tf is None:
print('Can not generate observation, camera_base_tf is None')
return []
visualizer.begin_draw_cycle('frustum', 'aabbs')
camera_tf = base_tf.dot(camera_base_tf)
frustum = camera_tf.dot(frustum_vertices)
frust_min = frustum.min(axis=1)
frust_max = frustum.max(axis=1)
frust_aabb_center = (frust_min + frust_max) * 0.5
visualizer.draw_lines('frustum', pb.Transform.identity(), 0.05,
camera_tf.dot(frustum_lines).T.tolist())
visualizer.draw_mesh(
'frustum',
pb.Transform(frust_aabb_center[0], frust_aabb_center[1],
frust_aabb_center[2]), (frust_max - frust_min),
'package://gebsyas/meshes/bounding_box.dae')
objects = world.overlap_aabb(pb.Vector3(*frust_min[:3]),
pb.Vector3(*frust_max[:3]))
aabbs = [o.aabb for o in objects]
centers = [a * 0.5 + b * 0.5 for a, b in aabbs]
ray_starts = [pb.Vector3(*camera_tf[:3, 3])] * len(centers)
ray_results = world.closest_ray_test_batch(ray_starts, centers)
visible_set = {r.collision_object
for r in ray_results}.intersection(set(objects))
#print(sorted([inv_phy_obj[o] for o in visible_set]))
i_camera_tf = np.eye(4)
i_camera_tf[:3, :3] = camera_tf[:3, :3].T
i_camera_tf[:3, 3] = -i_camera_tf[:3, :3].dot(camera_tf[:3, 3])
bounding_boxes = []
for o in visible_set:
aabb_min, aabb_max = o.aabb
aabb_min = np.array([aabb_min[x] for x in range(3)] + [1])
aabb_max = np.array([aabb_max[x] for x in range(3)] + [1])
aabb_dim = aabb_max - aabb_min
aabb_corners = (aabb_vertex_template * aabb_dim + aabb_min +
0.5 * aabb_dim).T
aabb_center = aabb_min + 0.5 * aabb_dim
visualizer.draw_points('aabbs', pb.Transform.identity(), 0.05,
aabb_corners.T.tolist())
visualizer.draw_cube('aabbs',
pb.Transform(*aabb_center[:3]),
pb.Vector3(*aabb_dim[:3]),
g=1,
a=0.5)
corners_in_camera = i_camera_tf.dot(aabb_corners)
projected_corners = projection_matrix.dot(corners_in_camera)
projected_corners[2, :] = np.abs(projected_corners[2, :]).clip(
1e-12, 1e12)
projected_corners = np.divide(projected_corners,
projected_corners[2, :])
screen_bb_min = projected_corners.min(axis=1)
screen_bb_max = projected_corners.max(axis=1)
if screen_bb_max.min() <= -1 or screen_bb_min.max() > 1:
print(
'Bounding box for object {} is off screen.\n Min: {}\n Max: {}'
.format(inv_phy_obj[o], screen_bb_min, screen_bb_max))
else:
pixel_bb_min = screen_translation.dot(screen_bb_min.clip(-1, 1))
pixel_bb_max = screen_translation.dot(screen_bb_max.clip(-1, 1))
print('Bounding box for object {}.\n Min: {}\n Max: {}'.format(
inv_phy_obj[o], pixel_bb_min, pixel_bb_max))
bounding_boxes.append(
(i_camera_tf.dot(aabb_min + 0.5 * aabb_dim)[2],
phy_to_label[o], pixel_bb_min, pixel_bb_max, aabb_center))
min_box_width = 5
visible_boxes = []
for _, label, bmin, bmax, location in reversed(sorted(bounding_boxes)):
new_visible_boxes = [(label, bmin, bmax, location)]
for vlabel, vmin, vmax, vlocation in visible_boxes:
rmin = vmin - bmin
rmax = vmax - bmax
new_visible_boxes.append((vlabel, vmin, vmax, vlocation))
continue # AFTER THIS BOX CUTTING
if rmin.min() > 0: # Min corner is outside v-box
if rmax.max() < 0: # Max corner is outside of v-box
print('Box for {} is completely occluded.'.format(vlabel))
elif rmax.min() > 0: # Max corner is inside v-box -> L-shape
top_bar = (vlabel, np.array([vmin[0], bmax[1]]), vmax)
right_bar = (vlabel, np.array([bmax[0], vmin[1]]),
np.array([vmax[0], bmax[1]]))
new_visible_boxes.extend([top_bar, right_bar])
else: # I/bar shape
if rmax[0] >= 0: # bar shape
top_bar = (vlabel, np.array([vmin[0], bmax[1]]), vmax)
new_visible_boxes.append(top_bar)
else: # I shape
right_bar = (vlabel, np.array([bmax[0],
vmin[1]]), vmax)
new_visible_boxes.append(right_bar)
elif rmin.max() < 0: # Min corner is inside v-box
left_bar = (vlabel, vmin, np.array([bmin[0], vmax[1]]))
bot_bar = (vlabel, np.array([bmin[0], vmin[1]]),
np.array([vmax[0], bmin[1]]))
new_visible_boxes.extend([left_bar,
bot_bar]) # These always go in
if rmax.max() < 0: # Max corner is outside v-box -> L-shape
pass
elif rmax.min() > 0: # Max corner is inside v-box -> O-shape
right_bar = (vlabel, np.array([bmax[0], bmin[1]]),
np.array([vmax[0], bmax[1]]))
top_bar = (vlabel, np.array([bmin[0], bmax[1]]), vmax)
new_visible_boxes.extend([right_bar, top_bar])
else: # C/U shape
if rmax[0] > 0: # U shape
right_bar = (vlabel, np.array([bmax[0],
bmin[1]]), vmax)
new_visible_boxes.append(right_bar)
else: # C shape
top_bar = (vlabel, np.array([bmin[0], bmax[1]]), vmax)
new_visible_boxes.append(top_bar)
else: # Min corner is partially overlapping v-box
left_bar = (vlabel, vmin, np.array([bmin[0], vmax[1]]))
bot_bar = (vlabel, vmin, np.array([vmax[0], bmin[1]]))
if rmin[0] < 0: # I/II/angle/cap
new_visible_boxes.append(left_bar)
if rmax.max(
) < 0: # Max corner is outside shape -> I shape
pass
elif rmax.min() > 0: # Max corner is inside shape -> Cap
right_bar = (vlabel, np.array([bmax[0],
vmin[1]]), vmax)
top_bar = (vlabel, np.array([bmin[0], bmax[1]]),
np.array([bmax[0], vmax[1]]))
new_visible_boxes.extend([right_bar, top_bar])
else: # Max corner is overlapping -> II/Angle
if rmax[0] > 0: # II shape
right_bar = (vlabel, np.array([bmax[0],
vmin[1]]), vmax)
new_visible_boxes.append(right_bar)
else: # Angle shape
top_bar = (vlabel, np.array([bmin[0],
bmax[1]]), vmax)
new_visible_boxes.append(top_bar)
else: # bar/=/angle
new_visible_boxes.append(bot_bar)
if rmax.max(
) < 0: # Max corner is outside shape -> bar shape
pass
elif rmax.min(
) > 0: # Max corner is inside shape -> reversed C
right_bar = (vlabel, np.array([bmax[0],
bmin[1]]), vmax)
top_bar = (vlabel, np.array([vmin[0], bmax[1]]),
np.array([bmax[0], vmax[1]]))
new_visible_boxes.extend([right_bar, top_bar])
else: # Max corner is overlapping -> =/Angle
if rmax[0] > 0: # Angle shape
right_bar = (vlabel, np.array([bmax[0],
bmin[1]]), vmax)
new_visible_boxes.append(right_bar)
else: # = shape
top_bar = (vlabel, np.array([vmin[0],
bmax[1]]), vmax)
new_visible_boxes.append(top_bar)
visible_boxes = [(vlabel, vmin, vmax, vlocation)
for vlabel, vmin, vmax, vlocation in new_visible_boxes
if (vmax - vmin).min() >= min_box_width]
bboxes = {}
for label, bmin, bmax, location in visible_boxes:
if label not in bboxes:
bboxes[label] = ObjectInfoMsg(label=label)
msg = bboxes[label]
msg.bbox_xmin.append(bmin[0])
msg.bbox_ymin.append(bmin[1])
msg.bbox_xmax.append(bmax[0])
msg.bbox_ymax.append(bmax[1])
msg.location.append(
PointMsg(x=location[0], y=location[1], z=location[2]))
msg.score.append(1.0)
visualizer.render('frustum', 'aabbs')
return bboxes.values()
self.on_a_y = Position((contact_name + ('onA', 'y')).to_symbol())
self.on_a_z = Position((contact_name + ('onA', 'z')).to_symbol())
self.on_b_x = Position((contact_name + ('onB', 'x')).to_symbol())
self.on_b_y = Position((contact_name + ('onB', 'y')).to_symbol())
self.on_b_z = Position((contact_name + ('onB', 'z')).to_symbol())
self.normal_x = Position((contact_name + ('normal', 'x')).to_symbol())
self.normal_y = Position((contact_name + ('normal', 'y')).to_symbol())
self.normal_z = Position((contact_name + ('normal', 'z')).to_symbol())
def __eq__(self, other):
if isinstance(other, ContactSymbolContainer):
return self.on_a_x == other.on_a_x and self.on_a_y == other.on_a_y and self.on_a_z == other.on_a_z and self.on_b_x == other.on_b_x and self.on_b_y == other.on_b_y and self.on_b_z == other.on_b_z and self.normal_x == other.normal_x and self.normal_y == other.normal_y and self.normal_z == other.normal_z
return False
pb_zero_vector = pb.Vector3(0, 0, 0)
pb_far_away_vector = pb.Vector3(1e8, 1e8, -1e8)
pb_default_normal = pb.Vector3(0, 0, 1)
class ContactHandler(object):
def __init__(self, object_path):
self.obj_path = object_path
self.state = {}
self.var_map = {}
self._num_anon_contacts = 0
@property
def num_anon_contacts(self):
return self._num_anon_contacts