p04798526
/
OmniGibson
mirror of https://github.com/StanfordVL/OmniGibson.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge with PC optimization. Removed subprocess.PIPE to avoid deadlock

This commit is contained in:
hzyjerry 2017-10-13 14:11:09 -07:00
parent 22f697a6e3 002961dc5c
commit 99afb69ee6
7 changed files with 233 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
dev/show_3d2_f.py
View File

@ -208,6 +208,11 @@ class PCRenderer:
self.fps = 0
self.rotation_const = np.array([[0,1,0,0],[0,0,1,0],[-1,0,0,0],[0,0,0,1]])
<<<<<<< HEAD
=======
self.old_topk = set([])
>>>>>>> 7cecc4365b6a20e1a0d236c1272d48a841f2feed
def onmouse(self, *args):
if args[0] == cv2.EVENT_LBUTTONDOWN:
self.org_pitch, self.org_yaw, self.org_x, self.org_y, self.org_z =\
@ -376,16 +381,21 @@ class PCRenderer:
)
'''
with Profiler("Render pointcloud"):
<<<<<<< HEAD
scale = 100. # 512
target_depth = np.int32(opengl_arr * scale)
show[:] = 0
show_all = np.zeros((len(imgs), 1024, 2048, 3)).astype(np.uint8)
=======
scale = 100.
>>>>>>> 7cecc4365b6a20e1a0d236c1272d48a841f2feed
poses_after = [
pose.dot(np.linalg.inv(poses[i])).astype(np.float32)
for i in range(len(imgs))]
<<<<<<< HEAD
#from IPython import embed; embed()
depth_test = np.zeros((1024 * 2048 * len(imgs),), dtype = np.float32)
@ -421,6 +431,26 @@ class PCRenderer:
render_pc(opengl_arr)
render_depth(opengl_arr)
=======
dll.render(ct.c_int(len(imgs)),
ct.c_int(imgs[0].shape[0]),
ct.c_int(imgs[0].shape[1]),
imgs.ctypes.data_as(ct.c_void_p),
depths.ctypes.data_as(ct.c_void_p),
np.asarray(poses_after, dtype = np.float32).ctypes.data_as(ct.c_void_p),
show.ctypes.data_as(ct.c_void_p),
opengl_arr.ctypes.data_as(ct.c_void_p)
)
threads = [
Process(target=render_pc, args=(opengl_arr,)),
Process(target=render_depth, args=(opengl_arr,))]
[t.start() for t in threads]
[t.join() for t in threads]
#render_pc(opengl_arr)
#render_depth(opengl_arr)
>>>>>>> 7cecc4365b6a20e1a0d236c1272d48a841f2feed
if model:
tf = transforms.ToTensor()
@ -530,6 +560,10 @@ class PCRenderer:
self.quat = new_quat
self.changed = True
<<<<<<< HEAD
=======
>>>>>>> 7cecc4365b6a20e1a0d236c1272d48a841f2feed
if self.changed:
new_quat = mat_to_quat_xyzw(world_cpose)
new_quat = z_up_to_y_up(quat_xyzw_to_wxyz(new_quat))
@ -542,8 +576,12 @@ class PCRenderer:
if PHYSICS_FIRST or hasNoCollision(new_posi, new_quat):
## Optimization
depth_buffer = np.zeros(imgs[0].shape[:2], dtype=np.float32)
<<<<<<< HEAD
#render(imgs, depths, cpose.astype(np.float32), model, poses, depth_buffer)
=======
#render(imgs, depths, cpose.astype(np.float32), model, poses, depth_buffer)
>>>>>>> 7cecc4365b6a20e1a0d236c1272d48a841f2feed
relative_poses = np.copy(target_poses)
for i in range(len(relative_poses)):
@ -556,10 +594,19 @@ class PCRenderer:
pose_after_distance = [np.linalg.norm(rt[:3,-1]) for rt in poses_after]
k = 3
topk = (np.argsort(pose_after_distance))[:k]
<<<<<<< HEAD
imgs_topk = [imgs[i] for i in topk]
depths_topk = [depths[i] for i in topk]
relative_poses_topk = [relative_poses[i] for i in topk]
=======
if set(topk) != self.old_topk:
imgs_topk = np.array([imgs[i] for i in topk])
depths_topk = np.array([depths[i] for i in topk]).flatten()
relative_poses_topk = [relative_poses[i] for i in topk]
self.old_topk = set(topk)
>>>>>>> 7cecc4365b6a20e1a0d236c1272d48a841f2feed
self.render(imgs_topk, depths_topk, cpose.astype(np.float32), model, relative_poses_topk, target_poses[0], show, target_depth, depth_buffer)
old_state = [self.x, self.y, self.z, self.roll, self.pitch, self.yaw]
@ -673,7 +720,11 @@ if __name__=='__main__':
parser.add_argument('--model' , type = str, default = '', help='path of model')
opt = parser.parse_args()
<<<<<<< HEAD
d = ViewDataSet3D(root=opt.datapath, transform = np.array, mist_transform = np.array, seqlen = 2, off_3d = False, train = False)
=======
d = ViewDataSet3D(root=opt.datapath, transform = np.array, mist_transform = np.array, seqlen = 2, off_3d = False, train = True)
>>>>>>> 7cecc4365b6a20e1a0d236c1272d48a841f2feed
scene_dict = dict(zip(d.scenes, range(len(d.scenes))))
if not opt.model_id in scene_dict.keys():

4
realenv/core/physics/physics_object.py
View File

@ -27,8 +27,8 @@ class PhysicsObject():
self.fps = float(fps)
self.action = self._createDefaultAction()
self.camera_offset = np.array([0, 0, 0.8])
#self.camera_offset = np.array([0, 0, 0])
#self.camera_offset = np.array([0, 0, 0.8])
self.camera_offset = np.array([0, 0, 0])
self.pos_init_xyz = np.array(pos)
self.quat_init_xyzw = np.array(quat)

23
realenv/core/physics/render_physics.py
View File

@ -39,22 +39,23 @@ class PhysRenderer(object):
p.setRealTimeSimulation(0)
collisionId = p.createCollisionShape(p.GEOM_MESH, fileName=obj_path, meshScale=[1, 1, 1], flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
visualId = p.createVisualShape(p.GEOM_MESH, fileName=obj_path, meshScale=[1, 1, 1], rgbaColor = [1, 0.2, 0.2, 0.3], specularColor=[0.4, 4.0])
boundaryUid = p.createMultiBody(baseCollisionShapeIndex = collisionId, baseVisualShapeIndex = visualId)
print("Exterior boundary", boundaryUid)
p.changeVisualShape(boundaryUid, -1, rgbaColor=[1, 0.2, 0.2, 0.3], specularColor=[1, 1, 1])
#p.changeVisualShape(visualId, -1, rgbaColor=[1, 0.2, 0.2, 0.3])
p.setGravity(0,0,-10)
if debug:
visualId = p.createVisualShape(p.GEOM_MESH, fileName=obj_path, meshScale=[1, 1, 1], rgbaColor = [1, 0.2, 0.2, 0.3], specularColor=[0.4, 4.0])
boundaryUid = p.createMultiBody(baseCollisionShapeIndex = collisionId, baseVisualShapeIndex = visualId)
print("Exterior boundary", boundaryUid)
p.changeVisualShape(boundaryUid, -1, rgbaColor=[1, 0.2, 0.2, 0.3], specularColor=[1, 1, 1])
#p.changeVisualShape(visualId, -1, rgbaColor=[1, 0.2, 0.2, 0.3])
else:
visualId = 0
#p.setGravity(0,0,-10)
p.setRealTimeSimulation(0)
self.framePerSec = framePerSec
file_dir = os.path.dirname(__file__)
#objectUid = p.loadURDF("models/quadrotor.urdf", globalScaling = 0.8)
self.objectUid = p.loadURDF(os.path.join(file_dir, "models/husky.urdf"), globalScaling = 0.8)
self.objectUid = p.loadURDF(os.path.join(file_dir, "models/quadrotor.urdf"), globalScaling = 0.8)
#self.objectUid = p.loadURDF(os.path.join(file_dir, "models/husky.urdf"), globalScaling = 0.8)
self.viewMatrix = p.computeViewMatrixFromYawPitchRoll([0, 0, 0], 10, 0, 90, 0, 2)
self.projMatrix = p.computeProjectionMatrix(-0.01, 0.01, -0.01, 0.01, 0.01, 128)

116
realenv/core/render/render_cuda_filter.cu
View File

@ -72,6 +72,40 @@ __global__ void int_to_char(int * img2, unsigned char * img)
}
__global__ void merge(unsigned char * img_all, unsigned char * img, int n, int stride)
{
int x = blockIdx.x * TILE_DIM + threadIdx.x;
int y = blockIdx.y * TILE_DIM + threadIdx.y;
int width = gridDim.x * TILE_DIM;
int idx = 0;
for (int j = 0; j < TILE_DIM; j+= BLOCK_ROWS) {
int nz = 0;
for (idx = 0; idx < n; idx ++)
if (img_all[stride * idx + 3*((y+j)*width + x)] + img_all[stride * idx + 3*((y+j)*width + x) + 1] + img_all[stride * idx + 3*((y+j)*width + x) + 2] > 0)
nz +=1 ;
img[3*((y+j)*width + x)] = 0;
img[3*((y+j)*width + x)+1] = 0;
img[3*((y+j)*width + x)+2] = 0;
if (nz > 0)
for (idx = 0; idx < n; idx ++) {
img[3*((y+j)*width + x)] += img_all[idx * stride + 3*((y+j)*width + x)] / nz;
img[3*((y+j)*width + x)+1] += img_all[idx * stride + 3*((y+j)*width + x) + 1] / nz;
img[3*((y+j)*width + x)+2] += img_all[idx * stride + 3*((y+j)*width + x) + 2] / nz;
}
}
}
__global__ void to3d_point(float *depth, float *points3d)
{
int x = blockIdx.x * TILE_DIM + threadIdx.x;
@ -157,7 +191,7 @@ __global__ void render_depth(float *points3d_polar, unsigned int * depth_render)
__global__ void render_final(float *points3d_polar, int * depth_render, int * img, int * render)
__global__ void render_final(float *points3d_polar, float * depth_render, int * img, int * render)
{
int x = blockIdx.x * TILE_DIM + threadIdx.x;
int y = blockIdx.y * TILE_DIM + threadIdx.y;
@ -185,16 +219,15 @@ __global__ void render_final(float *points3d_polar, int * depth_render, int * im
int this_depth = (int)(12800/128 * points3d_polar[(ih * w + iw) * 3 + 0]);
int delta = this_depth - depth_render[(ty * w + tx)];
int delta = this_depth - (int)(100 * depth_render[(ty * w + tx)]);
int txmin = min(min(txlu, txrd), min(txru, txld));
int txmax = max(max(txlu, txrd), max(txru, txld));
int tymin = min(min(tylu, tyrd), min(tyru, tyld));
int tymax = max(max(tylu, tyrd), max(tyru, tyld));
if ((y > h/8) && (y < h*7/8))
if ((delta > -10) && (delta < 10) && (this_depth < 10000)) {
if ((y > h/8) && (y < (h*7)/8))
if ((delta > -15) && (delta < 15) && (this_depth < 10000)) {
if ((txmax - txmin) * (tymax - tymin) < 50)
{
for (tx = txmin; tx < txmax; tx ++)
@ -208,9 +241,9 @@ __global__ void render_final(float *points3d_polar, int * depth_render, int * im
extern "C"{
void render(int n, int idx, int h,int w,unsigned char * img, float * depth,float * pose, unsigned char * render, int * depth_render){
void render(int n, int h,int w,unsigned char * img, float * depth,float * pose, unsigned char * render, float * depth_render){
//int ih, iw, i, ic;
printf("inside cuda code %d\n", depth);
//printf("inside cuda code %d\n", depth);
const int nx = w;
const int ny = h;
const size_t depth_mem_size = nx*ny*sizeof(float);
@ -219,50 +252,60 @@ void render(int n, int idx, int h,int w,unsigned char * img, float * depth,float
dim3 dimGrid(nx/TILE_DIM, ny/TILE_DIM, 1);
dim3 dimBlock(TILE_DIM, BLOCK_ROWS, 1);
unsigned char *d_img, *d_render;
unsigned char *d_img, *d_render, *d_render_all;
float *d_depth, *d_pose;
int *d_depth_render;
float *d_depth_render;
float *d_3dpoint, *d_3dpoint_after;
int *d_render2, *d_img2;
cudaMalloc((void **)&d_img, frame_mem_size);
cudaMalloc((void **)&d_render, frame_mem_size);
cudaMalloc((void **)&d_render_all, frame_mem_size * n);
cudaMalloc((void **)&d_depth, depth_mem_size);
cudaMalloc((void **)&d_depth_render, nx * ny * sizeof(int));
cudaMalloc((void **)&d_depth_render, nx * ny * sizeof(float));
cudaMalloc((void **)&d_3dpoint, depth_mem_size * 4);
cudaMalloc((void **)&d_3dpoint_after, depth_mem_size * 4);
cudaMalloc((void **)&d_pose, sizeof(float) * 16);
cudaMalloc((void **)&d_render2, nx * ny * sizeof(int));
cudaMalloc((void **)&d_img2, nx * ny * sizeof(int));
cudaMemcpy(d_depth_render, depth_render, nx * ny * sizeof(float), cudaMemcpyHostToDevice);
cudaMemcpy(d_depth_render, depth_render, nx * ny * sizeof(int), cudaMemcpyHostToDevice);
cudaMemcpy(d_pose, &(pose[idx * 16]), sizeof(float) * 16, cudaMemcpyHostToDevice);
cudaMemcpy(d_img, &(img[idx * nx * ny * 3]), frame_mem_size, cudaMemcpyHostToDevice);
cudaMemcpy(d_depth, &(depth[idx * nx * ny]), depth_mem_size, cudaMemcpyHostToDevice);
cudaMemset(d_render_all, 0, frame_mem_size * n);
int i;
for (i = 0; i < 100; i++) {
printf("%f ", depth[i + idx * nx * ny]);
int idx;
for (idx = 0; idx < n; idx ++) {
cudaMemcpy(d_pose, &(pose[idx * 16]), sizeof(float) * 16, cudaMemcpyHostToDevice);
cudaMemcpy(d_img, &(img[idx * nx * ny * 3]), frame_mem_size, cudaMemcpyHostToDevice);
cudaMemcpy(d_depth, &(depth[idx * nx * ny]), depth_mem_size, cudaMemcpyHostToDevice);
//int i;
//for (i = 0; i < 100; i++) {
// printf("%f ", depth[i + idx * nx * ny]);
//}
//printf("\n");
cudaMemset(d_render, 0, frame_mem_size);
cudaMemset(d_render2, 0, nx * ny * sizeof(int));
cudaMemset(d_img2, 0, nx * ny * sizeof(int));
cudaMemset(d_3dpoint, 0, depth_mem_size * 4);
cudaMemset(d_3dpoint_after, 0, depth_mem_size * 4);
to3d_point<<< dimGrid, dimBlock >>>(d_depth, d_3dpoint);
transform<<< dimGrid, dimBlock >>>(d_3dpoint_after, d_3dpoint, d_pose);
transform2d<<<dimGrid, dimBlock>>>(d_3dpoint_after);
char_to_int <<< dimGrid, dimBlock >>> (d_img2, d_img);
render_final <<< dimGrid, dimBlock >>> (d_3dpoint_after, d_depth_render, d_img2, d_render2);
int_to_char <<< dimGrid, dimBlock >>> (d_render2, d_render);
int_to_char <<< dimGrid, dimBlock >>> (d_render2, &(d_render_all[idx * nx * ny * 3]));
}
printf("\n");
cudaMemset(d_render, 0, frame_mem_size);
cudaMemset(d_render2, 0, nx * ny * sizeof(int));
cudaMemset(d_img2, 0, nx * ny * sizeof(int));
cudaMemset(d_3dpoint, 0, depth_mem_size * 4);
cudaMemset(d_3dpoint_after, 0, depth_mem_size * 4);
to3d_point<<< dimGrid, dimBlock >>>(d_depth, d_3dpoint);
transform<<< dimGrid, dimBlock >>>(d_3dpoint_after, d_3dpoint, d_pose);
transform2d<<<dimGrid, dimBlock>>>(d_3dpoint_after);
char_to_int <<< dimGrid, dimBlock >>> (d_img2, d_img);
render_final <<< dimGrid, dimBlock >>> (d_3dpoint_after, d_depth_render, d_img2, d_render2);
int_to_char <<< dimGrid, dimBlock >>> (d_render2, d_render);
cudaMemcpy(&(render[idx * nx * ny * 3]), d_render, frame_mem_size, cudaMemcpyDeviceToHost);
merge <<< dimGrid, dimBlock >>> (d_render_all, d_render, n, nx * ny * 3);
cudaMemcpy(render, d_render, frame_mem_size, cudaMemcpyDeviceToHost);
cudaFree(d_img);
cudaFree(d_depth);
@ -273,7 +316,8 @@ void render(int n, int idx, int h,int w,unsigned char * img, float * depth,float
cudaFree(d_3dpoint);
cudaFree(d_3dpoint_after);
cudaFree(d_pose);
cudaFree(d_render_all);
}
}//extern "C"
}//extern "C"

68
realenv/core/render/show_3d2.py
View File

@ -51,6 +51,7 @@ class InImg(object):
return (indx + 1, remx, remy)
class PCRenderer:
ROTATION_CONST = np.array([[0,1,0,0],[0,0,1,0],[-1,0,0,0],[0,0,0,1]])
def __init__(self, port, imgs, depths, target, target_poses):
self.roll, self.pitch, self.yaw = 0, 0, 0
self.quat = [1, 0, 0, 0]
@ -62,7 +63,6 @@ class PCRenderer:
self.clickstart = (0,0)
self.mousedown = False
self.fps = 0
self.rotation_const = np.array([[0,1,0,0],[0,0,1,0],[-1,0,0,0],[0,0,0,1]])
self.overlay = False
self.show_depth = False
self._context_phys = zmq.Context()
@ -73,6 +73,8 @@ class PCRenderer:
self.depths = depths
self.target = target
self.model = None
self.old_topk = set([])
self.k = 5
def _onmouse(self, *args):
if args[0] == cv2.EVENT_LBUTTONDOWN:
@ -169,21 +171,19 @@ class PCRenderer:
v_cam2world = target_pose
v_cam2cam = self._getViewerRelativePose()
p = v_cam2world.dot(np.linalg.inv(v_cam2cam))
p = p.dot(np.linalg.inv(self.rotation_const))
p = p.dot(np.linalg.inv(PCRenderer.ROTATION_CONST))
pos = utils.mat_to_posi_xyz(p)
quat_wxyz = utils.quat_xyzw_to_wxyz(utils.mat_to_quat_xyzw(p))
return pos, quat_wxyz
def render(self, imgs, depths, pose, model, poses, target_pose, show, target_depth, opengl_arr):
t0 = time.time()
v_cam2world = target_pose
p = (v_cam2world).dot(np.linalg.inv(pose))
p = p.dot(np.linalg.inv(self.rotation_const))
p = p.dot(np.linalg.inv(PCRenderer.ROTATION_CONST))
s = utils.mat_to_str(p)
#with Profiler("Depth request round-trip"):
#with Profiler("Depth request round-trip"):
socket_mist.send(s)
message = socket_mist.recv()
@ -202,22 +202,20 @@ class PCRenderer:
def _render_pc(opengl_arr):
#with Profiler("Render pointcloud"):
scale = 100. # 512
target_depth = np.int32(opengl_arr * scale)
show[:] = 0
poses_after = [
pose.dot(np.linalg.inv(poses[i])).astype(np.float32)
for i in range(len(imgs))]
for i in range(len(imgs)):
cuda_pc.render(ct.c_int(imgs[i].shape[0]),
ct.c_int(imgs[i].shape[1]),
imgs[i].ctypes.data_as(ct.c_void_p),
depths[i].ctypes.data_as(ct.c_void_p),
poses_after[i].ctypes.data_as(ct.c_void_p),
show.ctypes.data_as(ct.c_void_p),
target_depth.ctypes.data_as(ct.c_void_p)
)
cuda_pc.render(ct.c_int(len(imgs)),
ct.c_int(imgs[0].shape[0]),
ct.c_int(imgs[0].shape[1]),
imgs.ctypes.data_as(ct.c_void_p),
depths.ctypes.data_as(ct.c_void_p),
np.asarray(poses_after, dtype = np.float32).ctypes.data_as(ct.c_void_p),
show.ctypes.data_as(ct.c_void_p),
opengl_arr.ctypes.data_as(ct.c_void_p)
)
threads = [
Process(target=_render_pc, args=(opengl_arr,)),
Process(target=_render_depth, args=(opengl_arr,))]
@ -240,9 +238,6 @@ class PCRenderer:
show[:] = (show2[:] * 255).astype(np.uint8)
print('Transfer to CPU time:', time.time() - before)
t1 =time.time()
t = t1-t0
self.fps = 1/t
def renderOffScreenInitialPose(self):
## TODO (hzyjerry): error handling
@ -264,7 +259,7 @@ class PCRenderer:
v_cam2world = self.target_poses[0]
v_cam2cam = self._getViewerRelativePose()
cpose = np.linalg.inv(np.linalg.inv(v_cam2world).dot(v_cam2cam).dot(self.rotation_const))
cpose = np.linalg.inv(np.linalg.inv(v_cam2world).dot(v_cam2cam).dot(PCRenderer.ROTATION_CONST))
## Entry point for change of view
## Optimization
@ -277,12 +272,16 @@ class PCRenderer:
poses_after = [cpose.dot(np.linalg.inv(relative_poses[i])).astype(np.float32) for i in range(len(self.imgs))]
pose_after_distance = [np.linalg.norm(rt[:3,-1]) for rt in poses_after]
top5 = (np.argsort(pose_after_distance))[:5]
imgs_top5 = [self.imgs[i] for i in top5]
depths_top5 = [self.depths[i] for i in top5]
relative_poses_top5 = [relative_poses[i] for i in top5]
topk = (np.argsort(pose_after_distance))[:self.k]
if set(topk) != self.old_topk:
self.imgs_topk = np.array([self.imgs[i] for i in topk])
self.depths_topk = np.array([self.depths[i] for i in topk]).flatten()
self.relative_poses_topk = [relative_poses[i] for i in topk]
self.old_topk = set(topk)
self.render(self.imgs_topk, self.depths_topk, cpose.astype(np.float32), self.model, self.relative_poses_topk, self.target_poses[0], show, target_depth, depth_buffer)
self.render(imgs_top5, depths_top5, cpose.astype(np.float32), self.model, relative_poses_top5, self.target_poses[0], show, target_depth, depth_buffer)
if self.overlay:
show_out = (show/2 + self.target/2).astype(np.uint8)
@ -291,6 +290,7 @@ class PCRenderer:
else:
show_out = show
show_rgb = cv2.cvtColor(show_out, cv2.COLOR_BGR2RGB)
return show_rgb
@ -302,20 +302,24 @@ class PCRenderer:
cv2.setMouseCallback('show3d',self._onmouse)
def renderToScreen(self, pose):
t0 = time.time()
showsz = self.target.shape[0]
show = np.zeros((showsz,showsz * 2,3),dtype='uint8')
target_depth = np.zeros((showsz,showsz * 2)).astype(np.int32)
imgv = Variable(torch.zeros(1,3, showsz, showsz*2), volatile=True).cuda()
maskv = Variable(torch.zeros(1,1, showsz, showsz*2), volatile=True).cuda()
#imgv = Variable(torch.zeros(1,3, showsz, showsz*2), volatile=True).cuda()
#maskv = Variable(torch.zeros(1,1, showsz, showsz*2), volatile=True).cuda()
show_rgb = self.renderOffScreen(pose)
t1 =time.time()
t = t1-t0
self.fps = 1/t
cv2.putText(show_rgb,'pitch %.3f yaw %.2f roll %.3f x %.2f y %.2f z %.2f'%(self.pitch, self.yaw, self.roll, self.x, self.y, self.z),(15,showsz-15),0,0.5,(255,255,255))
cv2.putText(show_rgb,'fps %.1f'%(self.fps),(15,15),0,0.5,(255,255,255))
cv2.imshow('show3d',show_rgb)
## TODO (hzyjerry): does this introduce extra time delay?
cv2.waitKey(5)
cv2.waitKey(1)
return show_rgb
@ -346,7 +350,7 @@ if __name__=='__main__':
parser.add_argument('--model' , type = str, default = '', help='path of model')
opt = parser.parse_args()
d = ViewDataSet3D(root=opt.datapath, transform = np.array, mist_transform = np.array, seqlen = 2, off_3d = False, train = False)
d = ViewDataSet3D(root=opt.datapath, transform = np.array, mist_transform = np.array, seqlen = 2, off_3d = False, train = True)
scene_dict = dict(zip(d.scenes, range(len(d.scenes))))
if not opt.model_id in scene_dict.keys():

85
realenv/envs/simple_env.py
View File

@ -26,27 +26,28 @@ class SimpleEnv(gym.Env):
self.datapath = "data"
self.model_id = "11HB6XZSh1Q"
self.p_channel = subprocess.Popen(cmd_channel.split(), stdout=subprocess.PIPE)
#self.p_physics = subprocess.Popen()
#self.p_render = subprocess.Popen()
self.p_channel = subprocess.Popen(cmd_channel.split())#, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
self.state_old = None
try:
self.r_visuals = self._setupVisuals()
pose_init = self.r_visuals.renderOffScreenInitialPose()
pose_init = self.r_visuals.renderOffScreenInitialPose()
self.r_physics = self._setupPhysics(human)
self.r_physics.initialize(pose_init)
if self.debug_mode:
self.r_visuals.renderToScreenSetup()
self.r_displayer = RewardDisplayer() #MPRewardDisplayer()
self._setupRewardFunc()
except Exception as e:
print(e)
print("ending")
self._end()
self.r_physics = self._setupPhysics(human)
self.r_physics.initialize(pose_init)
if self.debug_mode:
self.r_visuals.renderToScreenSetup()
self.r_displayer = RewardDisplayer() #MPRewardDisplayer()
self._setupRewardFunc()
self.state_old = None
def _setupRewardFunc(self):
def _getReward(state_old, state_new):
print(state_old)
if not state_old:
return 0
else:
@ -62,7 +63,6 @@ class SimpleEnv(gym.Env):
else:
scene_id = scene_dict[self.model_id]
uuids, rts = d.get_scene_info(scene_id)
#print(uuids, rts)
targets = []
sources = []
source_depths = []
@ -73,7 +73,6 @@ class SimpleEnv(gym.Env):
' (', progressbar.ETA(), ') ',
])
for k,v in pbar(uuids):
#print(k,v)
data = d[v]
source = data[0][0]
target = data[1]
@ -84,10 +83,6 @@ class SimpleEnv(gym.Env):
poses.append(pose)
sources.append(target)
source_depths.append(target_depth)
#print('target', poses, poses[0])
#print('no.1 pose', poses, poses[1])
# print(source_depth)
print(sources[0].shape, source_depths[0].shape)
context_mist = zmq.Context()
socket_mist = context_mist.socket(zmq.REQ)
socket_mist.connect("tcp://localhost:5555")
@ -100,34 +95,38 @@ class SimpleEnv(gym.Env):
def _setupPhysics(self, human):
framePerSec = 13
renderer = PhysRenderer(self.datapath, self.model_id, framePerSec, debug = self.debug_mode, human = human)
#renderer.renderToScreen()
print('finish setup physics')
return renderer
def testShow3D(self):
return
def _step(self, action):
#renderer.renderToScreen(sources, source_depths, poses, model, target, target_depth, rts)
if not self.debug_mode:
pose, state = self.r_physics.renderOffScreen(action)
#reward = random.randrange(-8, 20)
reward = self.reward_func(self.state_old, state)
self.state_old = state
visuals = self.r_visuals.renderOffScreen(pose)
else:
with Profiler("Physics to screen"):
pose, state = self.r_physics.renderToScreen(action)
#reward = random.randrange(-8, 20)
reward = self.reward_func(self.state_old, state)
self.r_displayer.add_reward(reward)
self.state_old = state
#with Profiler("Display reward"):
with Profiler("Render to screen"):
visuals = self.r_visuals.renderToScreen(pose)
print()
return visuals, reward
try:
#renderer.renderToScreen(sources, source_depths, poses, model, target, target_depth, rts)
if not self.debug_mode:
pose, state = self.r_physics.renderOffScreen(action)
#reward = random.randrange(-8, 20)
reward = self.reward_func(self.state_old, state)
self.state_old = state
visuals = self.r_visuals.renderOffScreen(pose)
else:
with Profiler("Physics to screen"):
pose, state = self.r_physics.renderToScreen(action)
#reward = random.randrange(-8, 20)
with Profiler("Reward func"):
reward = self.reward_func(self.state_old, state)
#self.r_displayer.add_reward(reward)
self.state_old = state
#with Profiler("Display reward"):
with Profiler("Render to screen"):
visuals = self.r_visuals.renderToScreen(pose)
print()
return visuals, reward
except Exception:
print(e)
print("ending")
self._end()
def _reset(self):
return
@ -136,19 +135,25 @@ class SimpleEnv(gym.Env):
return
def _end(self):
#print("trying to kill this", self.p_channel)
## TODO (hzyjerry): this does not kill cleanly
## to reproduce bug, set human = false, debug_mode = false
self.p_channel.kill()
return
if __name__ == "__main__":
env = SimpleEnv()
t_start = time.time()
r_current = 0
try:
while True:
t0 = time.time()
img, reward = env._step({})
t1 = time.time()
t = t1-t0
print('fps', 1/t, np.mean(img))
r_current = r_current + 1
print('(Round %d) fps %.3f total time %.3f' %(r_current, 1/t, time.time() - t0))
except KeyboardInterrupt:
env._end()
print("Program finished")

12
realenv/examples/agents/random_husky.py
View File

@ -31,21 +31,23 @@ class RandomAgent(object):
if __name__ == '__main__':
action_space = generate_actions()
agent = RandomAgent(action_space)
env = SimpleEnv(human=True)
env = SimpleEnv(human=False)
ob = None
i = 0
try:
for i in range(100000):
while True:
if (i <= 14):
observation, reward = env._step({})
elif (i == 10000):
observation, reward = env.reset()
#elif (i == 10000):
# observation, reward = env.reset()
else:
action = agent.act(ob)
with Profiler("Agent step function"):
observation, reward = env._step(action)
print("Husky action", action, "reward %.3f"% reward)
i = i + 1
print("current step", i)
#time.sleep(0.2)
except KeyboardInterrupt: