def _randomlyRotateCamera(self, min_r=0, max_r=70, dist=DIST_FROM_BOARD):
"""
Randomly rotates camera about x-axis and zooms out from the center of the
tabletop
:param: min_r: the minimum random rotation in degrees
:param: max_r: the maximum random rotation in degrees
:param: dist: the distance to zoom out from center of the table
:return: the angle of rotation sampled
"""
min_r = math.radians(min_r)
max_r = math.radians(max_r)
table_center = self.chessEngine.getTableCenter()
table_R = so3.from_axis_angle(([1, 0, 0], -math.pi))
table_t = table_center
rot_deg = random.uniform(min_r, max_r)
zoom_out_R = so3.from_axis_angle(([1, 0, 0], rot_deg))
zoom_out_t = vectorops.mul(
[0, math.sin(rot_deg), -math.cos(rot_deg)], dist)
xform = se3.mul((table_R, table_t), (zoom_out_R, zoom_out_t))
sensing.set_sensor_xform(self.sensor, xform)
return rot_deg
def get_desired_end_effector_rotation(self, leg_number, yaw_rads):
if leg_number == 1 or leg_number == 3:
r = [0, 0, -1, 0, -1, 0, -1, 0, 0]
else:
r = [0, 0, -1, 0, 1, 0, 1, 0, 0]
yaw_rotation_aa = ([0, 0, 1], yaw_rads)
yaw_rotation_R = so3.from_axis_angle(yaw_rotation_aa)
return so3.mul(yaw_rotation_R, r)
#two finger configurations
c_hand = 0.28
o_hand = 0.6
power_hand = 0
precision_hand = 0.75
power_close_hand = [c_hand, c_hand, c_hand, power_hand]
power_open_hand = [o_hand - 0.02, o_hand - 0.02, o_hand - 0.02, power_hand]
precision_close_hand = [c_hand, c_hand, c_hand, precision_hand]
precision_open_hand = [o_hand, o_hand, o_hand, precision_hand]
#hand orientation
move_speed = 0.5
face_down = [0, -1, 0, 1, 0, 0, 0, 0, 1]
face_down_rot = [-1, 0, 0, 0, -1, 0, 0, 0, 1]
# face_toward_shelf=[1,0,0,0,0,1,0,-1,0]
face_toward_shelf = so3.mul(
so3.from_axis_angle(([1, 0, 0], math.pi / 180 * 70)), face_down)
idle_pos = (face_down, [0, 0.5, 0.77])
start_pos_face_down = (face_down, [0, 0.4, 0.77])
start_pos_face_down_rot = (face_down_rot, [0, 0.4, 0.77])
start_pos_face_toward_shelf = (face_toward_shelf, [0, 0.4, 0.77])
drop_pos_face_down = (face_down, [0, -0.1, 0.77])
drop_pos_face_down_rot = (face_down_rot, [0, -0.1, 0.77])
drop_pos_face_toward_shelf = (face_toward_shelf, [0, -0.2, 0.77])
sweep_pos = so3.mul(so3.from_axis_angle(([1, 0, 0], math.pi / 180 * 70)),
face_down_rot)
start_pos_sweep = (sweep_pos, [0, 0.55, 0.77])
drop_pos_sweep = (sweep_pos, [0, -0.1, 0.77])
# sweep_pos_1=(face_down,[-0.03,0.85,0.7])
# sweep_pos_2=(face_down,[-0.03,0.85,0.59])
def get_torso_R_from_yaw(self, yaw_rad):
axis_angle = ([0, 0, 1], yaw_rad)
desired_r = so3.from_axis_angle(axis_angle)
return desired_r
def loop_through_sensors(
world=world,
sensor=sensor,
world_camera_viewpoints=world_camera_viewpoints,
index=index,
counters=counters):
viewno = counters[0]
variation = counters[1]
total_count = counters[2]
print("Generating data for sensor view", viewno, "variation",
variation)
sensor_xform = world_camera_viewpoints[viewno]
#TODO: problem 1.B: perturb camera and change colors
# Generate random axis, random angle, rotate about angle for orientation perturbation
# Generate random axis, random dist, translate distance over axis for position perturbation
rand_axis = np.random.rand(3)
rand_axis = vectorops.unit(rand_axis)
multiplier = np.random.choice([True, False], 1)
rand_angle = (np.random.rand(1)[0] * 10 +
10) * (-1 if multiplier else 1)
# print(rand_angle)
R = so3.from_axis_angle((rand_axis, np.radians(rand_angle)))
rand_axis = vectorops.unit(np.random.random(3))
rand_dist = np.random.random(1)[0] * .10 + .10
# print(rand_dist)
t = vectorops.mul(rand_axis, rand_dist)
sensor_xform = se3.mul((R, t), sensor_xform)
sensing.set_sensor_xform(sensor, sensor_xform)
rgb_filename = "image_dataset/color_%04d_var%04d.png" % (
total_count, variation)
depth_filename = "image_dataset/depth_%04d_var%04d.png" % (
total_count, variation)
grasp_filename = "image_dataset/grasp_%04d_var%04d.png" % (
total_count, variation)
#Generate sensor images
sensor.kinematicReset()
sensor.kinematicSimulate(world, 0.01)
print("Done with kinematic simulate")
rgb, depth = sensing.camera_to_images(sensor)
print("Saving to", rgb_filename, depth_filename)
Image.fromarray(rgb).save(rgb_filename)
depth_scale = 8000
depth_quantized = (depth * depth_scale).astype(np.uint32)
Image.fromarray(depth_quantized).save(depth_filename)
with open("image_dataset/metadata.csv", 'a') as f:
line = "{},{},{},{},{},{},{}\n".format(
index, viewno, loader.write(sensor_xform,
'RigidTransform'),
os.path.basename(rgb_filename),
os.path.basename(depth_filename),
os.path.basename(grasp_filename), variation)
f.write(line)
#calculate grasp map and save it
grasp_map = make_grasp_map(world, total_count)
grasp_map_quantized = (grasp_map * 255).astype(np.uint8)
channels = ['score', 'opening', 'axis_heading', 'axis_elevation']
for i, c in enumerate(channels):
base, ext = os.path.splitext(grasp_filename)
fn = base + '_' + c + ext
Image.fromarray(grasp_map_quantized[:, :, i]).save(fn)
#loop through variations and views
counters[1] += 1
if counters[1] >= max_variations:
counters[1] = 0
counters[0] += 1
if counters[0] >= len(world_camera_viewpoints):
vis.show(False)
counters[2] += 1
ladder = world.makeTerrain("TriangleMesh")
mesh = Geometry3D()
mesh.loadFile("../../data/terrains/drc_ladder.off")
mesh.transform([.033, 0, 0, 0, .033, 0, 0, 0, .033], [0.05, 0.5, 0])
ladder.geometry().set(mesh)
ladder.appearance().setColor(random.uniform(0., 1.), random.uniform(0., 1.),
random.uniform(0., 1.), 1.)
#example Group
ladders = world.makeTerrain("Group")
ladders_geom = Geometry3D()
ladders_geom.setGroup()
for d in range(4):
mesh = Geometry3D()
mesh.loadFile("../../data/terrains/drc_ladder.off")
mesh.transform(so3.from_axis_angle(([0, 0, 1], math.pi / 2 * d)),
[0, 0, 0])
mesh.transform([.033, 0, 0, 0, .033, 0, 0, 0, .033], [0.05, 0.8, 0])
ladders_geom.setElement(d, mesh)
ladders.geometry().set(ladders_geom)
ladders.appearance().setColor(random.uniform(0., 1.), random.uniform(0., 1.),
random.uniform(0., 1.), 1.)
#example VolumeGrid
sdf = world.makeTerrain("SDF")
grid = VolumeGrid()
grid.setBounds([0., 1., 0.], [.1, 1.1, .1])
grid.resize(32, 32, 32)
for idx in range(grid.dims[0]):
for idy in range(grid.dims[1]):
for idz in range(grid.dims[2]):