def get_mask(rgb, depth, T_w_k):
    hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
    h = hsv[:,:,0]
    s = hsv[:,:,1]
    v = hsv[:,:,2]
    
    red_mask = ((h<10) | (h>150)) & (s > 100) & (v > 100)
    
    valid_mask = depth > 0
    
    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
    
    z = xyz_w[:,:,2]   
    z0 = xyz_k[:,:,2]
    # if DEBUG_PLOTS:
    #     cv2.imshow("z0",z0/z0.max())
    #     cv2.imshow("z",z/z.max())
    #     cv2.imshow("rgb", rgb)
    #     cv2.waitKey()
    
    height_mask = xyz_w[:,:,2] > .7 # TODO pass in parameter
    
    
    good_mask = red_mask & height_mask & valid_mask
    return good_mask
Exemplo n.º 2
0
 def get_PC (self):
     r, d = self.grabber.getRGBD()
     x = clouds.depth_to_xyz(d, berkeley_pr2.f)
     if Globals.pr2 is not None:
         Globals.pr2.update_rave()
         tfm = berkeley_pr2.get_kinect_transform(Globals.robot)
         x = x.dot(tfm[:3,:3].T) + tfm[:3,3][None,None,:]
     return ru.xyzrgb2pc(x, r, 'base_footprint')
def label_single_demo_points(seg_group, name):
    if 'old_cloud_xyz' not in seg_group:
        seg_group['old_cloud_xyz'] = seg_group['cloud_xyz'][:]
    print name
    rgb = seg_group['rgb'][:]
    old_cloud_xyz = seg_group['old_cloud_xyz'][:]
    depth = seg_group['depth'][:]
    T_w_k = seg_group['T_w_k'][:]
    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3, :3].T) + T_w_k[:3, 3][None, None, :]
    old_cloud_kd = KDTree(old_cloud_xyz)

    image = np.asarray(rgb)
    windowName = name
    ctr = 0

    finished = False
    labeled_points = []
    cv2.namedWindow(windowName)
    cv2.setMouseCallback(windowName, mark2, (image, labeled_points))
    while(1):
        cv2.imshow(windowName,image)
        k = cv2.waitKey(20) & 0xFF
        if k == ord('r'):
            return "retry"
        elif k == ord(' '):
            break
        elif k == ord('p'):
            return "restart"
        elif k == 27:
            return "quit"
    labeled_points = np.asarray(labeled_points)
    if 'labeled_points' in seg_group:
        del seg_group['labeled_points']
    seg_group.create_dataset("labeled_points",shape=(len(labeled_points),3),data=labeled_points)
    # resample the line
    labeled_xyz = []
    for i in range(labeled_points.shape[0]):
        pt_i = labeled_points[i, :2]
        x, y, z = xyz_k[pt_i[0], pt_i[1], :].dot(T_w_k[:3, :3].T) + T_w_k[:3, 3]
        if depth[pt_i[0], pt_i[1]] > 0 and z > .7:
            labeled_xyz.append([x, y, z])
    labeled_xyz = np.asarray(labeled_xyz)
    cloud_xyz = np.zeros((resample_size, 3))
    rope_len = line_length(labeled_xyz)
    for i, t in enumerate(np.linspace(0, rope_len, resample_size)):
        cloud_xyz[i, :] = point_at(labeled_xyz, t)
    plt.scatter(old_cloud_xyz[:, 0], old_cloud_xyz[:, 1], color='r')
    plt.scatter(cloud_xyz[:, 0], cloud_xyz[:, 1], color='b')
    plt.show(block=False)
    k = cv2.waitKey(0) & 0xFF
    plt.close()
    if k == ord('r'):
        return "retry"
    if 'cloud_xyz' in seg_group:
        del seg_group['cloud_xyz']
    seg_group['cloud_xyz'] = cloud_xyz
Exemplo n.º 4
0
def find_keypoint_transform_execution (kp, grabber, tfm, normal=False):
    global server, app, found, done
    found = False
    done = False
    app = None
    
    if rospy.get_name() == '/unnamed':
        rospy.init_node("find_keypoint_%s"%kp, anonymous=True, disable_signals=True)
        
    pcpub = rospy.Publisher('keypoint_pcd', PointCloud2)
    server = InteractiveMarkerServer("find_keypoint_%s"%kp)

    menu_handler.insert("Done?", callback=doneCallback)
    menu_handler.insert("Re-try?", callback=redoCallback)

    make6DofMarker( False, normal )
    #make6DofMarker( True )

    server.applyChanges()
    
    print "Open an RViz Window!"
    #if yes_or_no("Do you want to open your own rviz window? Might be a better idea."):
    outer_RVIZ = True
#     else:
#         outer_RVIZ = False
#         app = myviz.QApplication( [""] )
#         viz = myviz.MyViz()
#         viz.resize( 1000, 1000 )
#         viz.show()

    p = rospy.Rate(10)
    while not rospy.is_shutdown() and not done:
        rgb, dep = grabber.getRGBD()
        xyz = clouds.depth_to_xyz(dep, berkeley_pr2.f)
        xyz = xyz.dot(tfm[:3,:3].T) + tfm[:3,3][None,None,:]
        pcmsg = ru.xyzrgb2pc(xyz, rgb, 'base_footprint')
        pcpub.publish(pcmsg)
        
        if not outer_RVIZ:
            app.processEvents()
        p.sleep()
    
    # Unhappy with data
    if not found:
        return None
    
    marker = server.get("pose_marker")
    pose = marker.pose
    
    print "Pose of the marker for %s"%kp
    print pose
    
    server.erase("pose_marker")
    #rospy.signal_shutdown('Finished finding transform')
    
    return conversions.pose_to_hmat(pose)
Exemplo n.º 5
0
def grabcut(rgb, depth, T_w_k):
    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3, :3].T) + T_w_k[:3, 3][None, None, :]

    valid_mask = depth > 0

    import interactive_roi as ir
    xys = ir.get_polyline(rgb, "rgb")
    xy_corner1 = np.clip(np.array(xys).min(axis=0), [0, 0], [639, 479])
    xy_corner2 = np.clip(np.array(xys).max(axis=0), [0, 0], [639, 479])
    polymask = ir.mask_from_poly(xys)
    #cv2.imshow("mask",mask)

    xy_tl = np.array([xy_corner1, xy_corner2]).min(axis=0)
    xy_br = np.array([xy_corner1, xy_corner2]).max(axis=0)

    xl, yl = xy_tl
    w, h = xy_br - xy_tl
    mask = np.zeros((h, w), dtype='uint8')
    mask[polymask[yl:yl + h, xl:xl + w] > 0] = cv2.GC_PR_FGD
    print mask.shape
    #mask[h//4:3*h//4, w//4:3*w//4] = cv2.GC_PR_FGD

    tmp1 = np.zeros((1, 13 * 5))
    tmp2 = np.zeros((1, 13 * 5))
    cv2.grabCut(rgb[yl:yl + h, xl:xl + w, :],
                mask, (0, 0, 0, 0),
                tmp1,
                tmp2,
                10,
                mode=cv2.GC_INIT_WITH_MASK)

    mask = mask % 2
    #mask = ndi.binary_erosion(mask, utils_images.disk(args.erode)).astype('uint8')
    contours = cv2.findContours(mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)[0]
    cv2.drawContours(rgb[yl:yl + h, xl:xl + w, :],
                     contours,
                     -1, (0, 255, 0),
                     thickness=2)

    cv2.imshow('rgb', rgb)
    print "press enter to continue"
    cv2.waitKey()

    zsel = xyz_w[yl:yl + h, xl:xl + w, 2]
    mask = (mask % 2 == 1) & np.isfinite(zsel)  # & (zsel - table_height > -1)
    mask &= valid_mask[yl:yl + h, xl:xl + w]

    xyz_sel = xyz_w[yl:yl + h, xl:xl + w, :][mask.astype('bool')]
    return clouds.downsample(xyz_sel, .01)
Exemplo n.º 6
0
def extract_red(rgb, depth, T_w_k):
    """
    extract red points and downsample
    """
        
    hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
    h = hsv[:,:,0]
    s = hsv[:,:,1]
    v = hsv[:,:,2]
    
    red_mask = ((h<10) | (h>150)) & (s > 100) & (v > 100)
    
    valid_mask = depth > 0
    
    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
    
    z = xyz_w[:,:,2]   
    z0 = xyz_k[:,:,2]
    if DEBUG_PLOTS:
        cv2.imshow("z0",z0/z0.max())
        cv2.imshow("z",z/z.max())
        cv2.imshow("rgb", rgb)
        cv2.waitKey()
    
    height_mask = xyz_w[:,:,2] > .7 # XXXX pass in parameter
    
    
    good_mask = red_mask & height_mask & valid_mask
    if DEBUG_PLOTS:
        cv2.imshow("red",red_mask.astype('uint8')*255)
        cv2.imshow("above_table", height_mask.astype('uint8')*255)
        cv2.imshow("red and above table", good_mask.astype('uint8')*255)
        print "press enter to continue"
        cv2.waitKey()
        
        
    

    good_xyz = xyz_w[good_mask]
    
    cloud = cloudprocpy.CloudXYZ()    
    good_xyz1 = np.zeros((len(good_xyz), 4), 'float32')
    good_xyz1[:,:3] = good_xyz
    cloud.from2dArray(good_xyz1)
    
    cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)
    
    return cloud_ds.to2dArray()[:,:3]
Exemplo n.º 7
0
def extract_red(rgb, depth, T_w_k):
    """
    extract red points and downsample
    """

    hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
    h = hsv[:, :, 0]
    s = hsv[:, :, 1]
    v = hsv[:, :, 2]

    red_mask = ((h < 10) | (h > 150)) & (s > 100) & (v > 100)

    valid_mask = depth > 0

    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3, :3].T) + T_w_k[:3, 3][None, None, :]

    z = xyz_w[:, :, 2]
    z0 = xyz_k[:, :, 2]
    if DEBUG_PLOTS:
        cv2.imshow("z0", z0 / z0.max())
        cv2.imshow("z", z / z.max())
        cv2.imshow("rgb", rgb)
        cv2.waitKey()

    height_mask = xyz_w[:, :, 2] > .7  # XXXX pass in parameter

    good_mask = red_mask & height_mask & valid_mask
    if DEBUG_PLOTS:
        cv2.imshow("red", red_mask.astype('uint8') * 255)
        cv2.imshow("above_table", height_mask.astype('uint8') * 255)
        cv2.imshow("red and above table", good_mask.astype('uint8') * 255)
        print "press enter to continue"
        cv2.waitKey()

    good_xyz = xyz_w[good_mask]

    cloud = cloudprocpy.CloudXYZ()
    good_xyz1 = np.zeros((len(good_xyz), 4), 'float32')
    good_xyz1[:, :3] = good_xyz
    cloud.from2dArray(good_xyz1)

    cloud_ds = cloudprocpy.downsampleCloud(cloud, .01)

    return cloud_ds.to2dArray()[:, :3]
Exemplo n.º 8
0
def extract_red(rgb, depth, T_w_k):
    """
    extract red points and downsample
    """
        
    hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
    h = hsv[:,:,0]
    s = hsv[:,:,1]
    v = hsv[:,:,2]
    
    h_mask = (h<15) | (h>145)
    s_mask = (s > 30 )
    v_mask = (v > 100)
    red_mask = h_mask & s_mask & v_mask
    
    valid_mask = depth > 0
    
    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
    
    z = xyz_w[:,:,2]   
    z0 = xyz_k[:,:,2]

    height_mask = xyz_w[:,:,2] > .7 # TODO pass in parameter
    
    good_mask = red_mask & height_mask & valid_mask
    good_mask =   skim.remove_small_objects(good_mask,min_size=64)

    if DEBUG_PLOTS:
        cv2.imshow("z0",z0/z0.max())
        cv2.imshow("z",z/z.max())
        cv2.imshow("hue", h_mask.astype('uint8')*255)
        cv2.imshow("sat", s_mask.astype('uint8')*255)
        cv2.imshow("val", v_mask.astype('uint8')*255)
        cv2.imshow("final",good_mask.astype('uint8')*255)
        cv2.imshow("rgb", rgb)
        cv2.waitKey()
            
        
    

    good_xyz = xyz_w[good_mask]
    

    return clouds.downsample(good_xyz, .025)
Exemplo n.º 9
0
def extract_red(rgb, depth, T_w_k):
    """
    extract red points and downsample
    """
        
    hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
    h = hsv[:,:,0]
    s = hsv[:,:,1]
    v = hsv[:,:,2]
    
    red_mask = ((h<10) | (h>150)) & (s > 100) & (v > 100)
    
    valid_mask = depth > 0
    
    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
    
    z = xyz_w[:,:,2]   
    z0 = xyz_k[:,:,2]
    if DEBUG_PLOTS:
        cv2.imshow("z0",z0/z0.max())
        cv2.imshow("z",z/z.max())
        cv2.imshow("rgb", rgb)
        cv2.waitKey()
    
    height_mask = xyz_w[:,:,2] > .7 # TODO pass in parameter
    
    
    good_mask = red_mask & height_mask & valid_mask
    if DEBUG_PLOTS:
        cv2.imshow("red",red_mask.astype('uint8')*255)
        cv2.imshow("above_table", height_mask.astype('uint8')*255)
        cv2.imshow("red and above table", good_mask.astype('uint8')*255)
        print "press enter to continue"
        cv2.waitKey()
        
        
    

    good_xyz = xyz_w[good_mask]
    

    return clouds.downsample(good_xyz, .025)
Exemplo n.º 10
0
def grabcut(rgb, depth, T_w_k):
    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]

    valid_mask = depth > 0

    import interactive_roi as ir
    xys = ir.get_polyline(rgb, "rgb")
    xy_corner1 = np.clip(np.array(xys).min(axis=0), [0,0], [639,479])
    xy_corner2 = np.clip(np.array(xys).max(axis=0), [0,0], [639,479])
    polymask = ir.mask_from_poly(xys)
    #cv2.imshow("mask",mask)
        
    xy_tl = np.array([xy_corner1, xy_corner2]).min(axis=0)
    xy_br = np.array([xy_corner1, xy_corner2]).max(axis=0)

    xl, yl = xy_tl
    w, h = xy_br - xy_tl
    mask = np.zeros((h,w),dtype='uint8')    
    mask[polymask[yl:yl+h, xl:xl+w] > 0] = cv2.GC_PR_FGD
    print mask.shape
    #mask[h//4:3*h//4, w//4:3*w//4] = cv2.GC_PR_FGD

    tmp1 = np.zeros((1, 13 * 5))
    tmp2 = np.zeros((1, 13 * 5))    
    cv2.grabCut(rgb[yl:yl+h, xl:xl+w, :],mask,(0,0,0,0),tmp1, tmp2,10,mode=cv2.GC_INIT_WITH_MASK)

    mask = mask % 2
    #mask = ndi.binary_erosion(mask, utils_images.disk(args.erode)).astype('uint8')
    contours = cv2.findContours(mask,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE)[0]
    cv2.drawContours(rgb[yl:yl+h, xl:xl+w, :],contours,-1,(0,255,0),thickness=2)
    
    cv2.imshow('rgb', rgb)
    print "press enter to continue"
    cv2.waitKey()

    zsel = xyz_w[yl:yl+h, xl:xl+w, 2]
    mask = (mask%2==1) & np.isfinite(zsel)# & (zsel - table_height > -1)
    mask &= valid_mask[yl:yl+h, xl:xl+w]
    
    xyz_sel = xyz_w[yl:yl+h, xl:xl+w,:][mask.astype('bool')]
    return clouds.downsample(xyz_sel, .01)
Exemplo n.º 11
0
def extract_red(rgb, depth, T_w_k):
    """
    extract red points and downsample
    """

    hsv = cv2.cvtColor(rgb, cv2.COLOR_BGR2HSV)
    h = hsv[:, :, 0]
    s = hsv[:, :, 1]
    v = hsv[:, :, 2]

    red_mask = ((h < 10) | (h > 150)) & (s > 100) & (v > 100)

    valid_mask = depth > 0

    xyz_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
    xyz_w = xyz_k.dot(T_w_k[:3, :3].T) + T_w_k[:3, 3][None, None, :]

    z = xyz_w[:, :, 2]
    z0 = xyz_k[:, :, 2]
    if DEBUG_PLOTS:
        cv2.imshow("z0", z0 / z0.max())
        cv2.imshow("z", z / z.max())
        cv2.imshow("rgb", rgb)
        cv2.waitKey()

    height_mask = xyz_w[:, :, 2] > .7  # TODO pass in parameter

    good_mask = red_mask & height_mask & valid_mask
    if DEBUG_PLOTS:
        cv2.imshow("red", red_mask.astype('uint8') * 255)
        cv2.imshow("above_table", height_mask.astype('uint8') * 255)
        cv2.imshow("red and above table", good_mask.astype('uint8') * 255)
        print "press enter to continue"
        cv2.waitKey()

    good_xyz = xyz_w[good_mask]

    return clouds.downsample(good_xyz, .025)
Exemplo n.º 12
0
    rave_traj = [
        r2r.convert(row)
        for row in asarray(seg_info["joint_states"]["position"])
    ]
    robot.SetActiveDOFs(r2r.rave_inds)
    robot.SetActiveDOFValues(rave_traj[0])

    handles = []
    T_w_k = berkeley_pr2.get_kinect_transform(robot)
    o = T_w_k[:3, 3]
    x = T_w_k[:3, 0]
    y = T_w_k[:3, 1]
    z = T_w_k[:3, 2]

    handles.append(env.drawarrow(o, o + .3 * x, .005, (1, 0, 0, 1)))
    handles.append(env.drawarrow(o, o + .3 * y, .005, (0, 1, 0, 1)))
    handles.append(env.drawarrow(o, o + .3 * z, .005, (0, 0, 1, 1)))

    XYZ_k = clouds.depth_to_xyz(np.asarray(seg_info["depth"]), berkeley_pr2.f)
    Twk = asarray(seg_info["T_w_k"])
    XYZ_w = XYZ_k.dot(Twk[:3, :3].T) + Twk[:3, 3][None, None, :]
    RGB = np.asarray(seg_info["rgb"])
    handles.append(
        env.plot3(XYZ_w.reshape(-1, 3), 2,
                  RGB.reshape(-1, 3)[:, ::-1] / 255.))

    animate_traj.animate_traj(rave_traj, robot, pause=not args.nopause)

    print "DONE"
    trajoptpy.GetViewer(env).Idle()
Exemplo n.º 13
0
    robot.SetActiveDOFs(r2r.rave_inds)
    robot.SetActiveDOFValues(rave_traj[0])
    
    handles = []
    T_w_k = berkeley_pr2.get_kinect_transform(robot)
    o = T_w_k[:3,3]
    x = T_w_k[:3,0]
    y = T_w_k[:3,1]
    z = T_w_k[:3,2]

    handles.append(env.drawarrow(o, o+.3*x, .005,(1,0,0,1)))
    handles.append(env.drawarrow(o, o+.3*y, .005,(0,1,0,1)))
    handles.append(env.drawarrow(o, o+.3*z, .005,(0,0,1,1)))
    

    XYZ_k = clouds.depth_to_xyz(np.asarray(seg_info["depth"]), berkeley_pr2.f)
    Twk = asarray(seg_info["T_w_k"])
    XYZ_w = XYZ_k.dot(Twk[:3,:3].T) + Twk[:3,3][None,None,:]
    RGB = np.asarray(seg_info["rgb"])
    handles.append(env.plot3(XYZ_w.reshape(-1,3), 2,  RGB.reshape(-1,3)[:,::-1]/255.))


    
    animate_traj.animate_traj(rave_traj, robot, pause = not args.nopause)
    
    print "DONE"
    trajoptpy.GetViewer(env).Idle()
    
    
    
    
env = openravepy.Environment()
viewer = trajoptpy.GetViewer(env)





if args.ropefile is None:
    demo_rope_handle = env.plot3(demo_rope, 10, (0,1,1,1))
    perturbed_rope_handle = env.drawlinestrip(perturbed_rope, 10, (1,0,1,1))
else:
    perturbed_rope_handle = env.plot3(perturbed_rope, 16, (0,0,0,1))
    

try:
    grabber = cloudprocpy.CloudGrabber()
    grabber.startRGBD()
    while True:
        rgb, depth = grabber.getRGBD()
        XYZ_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
        Twk = berkeley_pr2.get_kinect_transform(pr2.robot)
        XYZ_w = XYZ_k.dot(Twk[:3,:3].T) + Twk[:3,3][None,None,:]
        cloud_handle = env.plot3(XYZ_w.reshape(-1,3), 2,  rgb.reshape(-1,3)[:,::-1]/255.)
        viewer.Step()
except:
    import traceback
    traceback.print_exc()
finally:
    grabber.stop()
Exemplo n.º 15
0
def getPC ():
    global pc
    r, d = grabber.getRGBD()
    x = clouds.depth_to_xyz(d, berkeley_pr2.f)
    pc = ru.xyzrgb2pc(x, r, 'camera_link_p')
Exemplo n.º 16
0
import numpy as np
import itertools
import cv2
from rapprentice import ros2rave
import sensorsimpy, trajoptpy, openravepy
from rapprentice import berkeley_pr2
import scipy.optimize as opt
from rapprentice import clouds

DEBUG_PLOT = True


npzfile = np.load(args.fname)

depth_images = npzfile["depth_images"]
valid_masks = [(d > 0) & ((clouds.depth_to_xyz(d, 525)**2).sum(axis=2) < 1.5**2) for d in depth_images]
depth_images = depth_images/1000.
joint_positions = npzfile["joint_positions"]
joint_names = npzfile["names"] if "names" in npzfile else npzfile["joint_names"]

env = openravepy.Environment()
env.StopSimulation()
env.Load("robots/pr2-beta-static.zae")
robot = env.GetRobots()[0]

viewer = trajoptpy.GetViewer(env)
viewer.Step()
sensor = sensorsimpy.CreateFakeKinect(env)

r2r = ros2rave.RosToRave(robot, joint_names)
Exemplo n.º 17
0
import numpy as np
import itertools
import cv2
from rapprentice import ros2rave
import sensorsimpy, trajoptpy, openravepy
from rapprentice import berkeley_pr2
import scipy.optimize as opt
from rapprentice import clouds

DEBUG_PLOT = True

npzfile = np.load(args.fname)

depth_images = npzfile["depth_images"]
valid_masks = [
    (d > 0) & ((clouds.depth_to_xyz(d, 525)**2).sum(axis=2) < 1.5**2)
    for d in depth_images
]
depth_images = depth_images / 1000.
joint_positions = npzfile["joint_positions"]
joint_names = npzfile["names"] if "names" in npzfile else npzfile["joint_names"]

env = openravepy.Environment()
env.StopSimulation()
env.Load("robots/pr2-beta-static.zae")
robot = env.GetRobots()[0]

viewer = trajoptpy.GetViewer(env)
viewer.Step()
sensor = sensorsimpy.CreateFakeKinect(env)