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]):