示例#1
0
def generateBodies(cloud, id):
    # BEGIN addtoenv
    names = []
    if args.geom_type == "mesh":
        mesh = generate_mesh(cloud)
        name = "simple_mesh_%i" % id
        mesh_body = mk.create_trimesh(env,
                                      get_xyz_world_frame(mesh.getCloud()),
                                      np.array(mesh.getFaces()),
                                      name=name)
        mesh_body.SetUserData(
            "bt_use_trimesh", True
        )  # Tell collision checker to use the trimesh rather than the convex hull of it
        names.append(name)
    elif args.geom_type == "cd":
        big_mesh = generate_mesh(cloud)
        convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, 30)
        for (i, mesh) in enumerate(convex_meshes):
            name = "mesh_%i_%i" % (id, i)
            verts = get_xyz_world_frame(mesh.getCloud())
            mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
            randcolor = np.random.rand(3)

            print 'vertices'
            print mesh.getVertices()

            if not isValidMesh(mesh):
                print('Mesh invalid. Removing kinbody')
                env.RemoveKinBody(env.GetKinBody(name))
                continue

            env.GetKinBody(name).GetLinks()[0].GetGeometries(
            )[0].SetAmbientColor(randcolor)
            env.GetKinBody(name).GetLinks()[0].GetGeometries(
            )[0].SetDiffuseColor(randcolor)
            names.append(name)
    # END  addtoenv
    elif args.geom_type == "spheres":
        raise Exception("don't use spheres--there's a performance issue")
        cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
        name = "spheres_%i" % id
        mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02, name=name)
        names.append(name)
    elif args.geom_type == "boxes":
        cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
        name = "boxes_%i" % id
        mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02, name=name)
        names.append(name)
    return names
示例#2
0
def generateBodies(cloud, id):
    # BEGIN addtoenv
    names = []
    if args.geom_type == "mesh":
        mesh = generate_mesh(cloud)
        name = "simple_mesh_%i"%id
        mesh_body = mk.create_trimesh(env, get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name)
        mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
        names.append(name)
    elif args.geom_type == "cd":
        big_mesh = generate_mesh(cloud)
        convex_meshes = cloudprocpy.convexDecompHACD(big_mesh,30)
        for (i,mesh) in enumerate(convex_meshes):
            name = "mesh_%i_%i" % (id,i)
            verts = get_xyz_world_frame(mesh.getCloud())
            mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
            randcolor = np.random.rand(3)

	    print 'vertices'
	    print mesh.getVertices()

	    if not isValidMesh(mesh):
	        print('Mesh invalid. Removing kinbody')
	        env.RemoveKinBody(env.GetKinBody(name))
	        continue
	

            env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
            env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
            names.append(name)
    # END  addtoenv        
    elif args.geom_type == "spheres":
        raise Exception("don't use spheres--there's a performance issue")
        cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
        name = "spheres_%i"%id
        mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02, name=name)
        names.append(name)
    elif args.geom_type == "boxes":
        cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
        name = "boxes_%i"%id
        mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02, name=name)
        names.append(name)
    return names
 def generateBodies(self, cloud, id):
     # BEGIN addtoenv
     names = []
     if self.geom_type == "mesh":
         mesh = self.generate_mesh(cloud)
         name = "simple_mesh_%i"%id
         mesh_body = mk.create_trimesh(self.env, self.get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name)
         mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
         names.append(name)
         
     elif self.geom_type == "cd":
         big_mesh = self.generate_mesh(cloud)
         #name = 'big_mesh'
         #verts = self.get_xyz_world_frame(big_mesh.getCloud())
         #mk.create_trimesh(self.env, verts, big_mesh.getTriangles(), name=name)
             
         convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, self.num_cd_components)
         for (i,mesh) in enumerate(convex_meshes):
             name = "mesh_%i_%i" % (id,i)
             verts = self.get_xyz_world_frame(mesh.getCloud())
             mk.create_trimesh(self.env, verts, mesh.getTriangles(), name=name)
             randcolor = np.random.rand(3)
             self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
             self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
             names.append(name)
     # END  addtoenv   
          
     elif self.geom_type == "spheres":
         raise Exception("don't use spheres--there's a performance issue")
         cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
         name = "spheres_%i"%id
         mk.create_spheres(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
         names.append(name)
         
     elif self.geom_type == "boxes":
         cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
         name = "boxes_%i"%id
         mk.create_boxes(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
         names.append(name)
     return names
示例#4
0
 def generateBodies(self, cloud, id):
     # BEGIN addtoenv
     names = []
     if self.geom_type == "mesh":
         mesh = self.generate_mesh(cloud)
         name = "simple_mesh_%i"%id
         mesh_body = mk.create_trimesh(self.env, self.get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name)
         mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
         names.append(name)
         
     elif self.geom_type == "cd":
         big_mesh = self.generate_mesh(cloud)
         convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, self.num_cd_components)
         for (i,mesh) in enumerate(convex_meshes):
             name = "mesh_%i_%i" % (id,i)
             verts = self.get_xyz_world_frame(mesh.getCloud())
             mk.create_trimesh(self.env, verts, mesh.getTriangles(), name=name)
             randcolor = np.random.rand(3)
             self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
             self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
             names.append(name)
     # END  addtoenv   
          
     elif self.geom_type == "spheres":
         raise Exception("don't use spheres--there's a performance issue")
         cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
         name = "spheres_%i"%id
         mk.create_spheres(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
         names.append(name)
         
     elif self.geom_type == "boxes":
         cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
         name = "boxes_%i"%id
         mk.create_boxes(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
         names.append(name)
     return names
elif args.geom_type == "cd":
    big_mesh = generate_mesh(cloud_orig)
    convex_meshes = cloudprocpy.convexDecompHACD(big_mesh,30)
    for (i,mesh) in enumerate(convex_meshes):
        name = "mesh%i"%i
        verts = get_xyz_world_frame(mesh.getCloud())
        mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
        randcolor = np.random.rand(3)
        env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
        env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
# END  addtoenv        
elif args.geom_type == "spheres":
    raise Exception("don't use spheres--there's a performance issue")
    cloud_nofloor = remove_floor(cloud_orig)
    cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04)
    mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02)
elif args.geom_type == "boxes":
    cloud_nofloor = remove_floor(cloud_orig)
    cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04)
    mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02)
    

def full_body_drive_and_reach(robot, link_name, xyz_targ, quat_targ):
        
    request = {        
        "basic_info" : {
            "n_steps" : 10,
            "manip" : "active",
            "start_fixed" : True
        },
        "costs" : [
示例#6
0
    convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, 30)
    for (i, mesh) in enumerate(convex_meshes):
        name = "mesh%i" % i
        verts = get_xyz_world_frame(mesh.getCloud())
        mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
        randcolor = np.random.rand(3)
        env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(
            randcolor)
        env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(
            randcolor)
# END  addtoenv
elif args.geom_type == "spheres":
    raise Exception("don't use spheres--there's a performance issue")
    cloud_nofloor = remove_floor(cloud_orig)
    cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04)
    mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02)
elif args.geom_type == "boxes":
    cloud_nofloor = remove_floor(cloud_orig)
    cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04)
    mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02)


def full_body_drive_and_reach(link_name, xyz_targ, quat_targ):

    request = {
        "basic_info": {
            "n_steps": 10,
            "manip": "active",
            "start_fixed": True
        },
        "costs": [{
示例#7
0
def main():

    demofile = h5py.File(args.h5file, 'r')
    
    trajoptpy.SetInteractive(args.interactive)
    rospy.init_node("exec_task",disable_signals=True)
    
    global filename
    if args.logging:
        name = raw_input ('Enter identifier of this experiment (without spaces): ')
        name = name + '.yaml'
        import os.path as osp
        filename = osp.join('/home/sibi/sandbox/rapprentice/experiments/', name)
        if not osp.exists(filename):
            with open(filename,'w') as fh:
                fh.write("experiment: %s\ninfo: \n"%name)
    
    #thc.start()
    
    if args.execution:
        Globals.pr2 = PR2.PR2()
        Globals.env = Globals.pr2.env
        Globals.robot = Globals.pr2.robot
        
    else:
        Globals.env = openravepy.Environment()
        Globals.env.StopSimulation()
        Globals.env.Load("robots/pr2-beta-static.zae")
        Globals.robot = Globals.env.GetRobots()[0]
        
    import trajoptpy.make_kinbodies as mk
    #Globals.env.Load("/home/sibi/sandbox/rapprentice/objects/table.xml")
    addBoxToRave(Globals.env, "table")
    Globals.sponge = addBoxToRave(Globals.env, "sponge") # Not sure about this
    Globals.needle_tip = mk.create_spheres(Globals.env, [(0,0,0)], radii=0.02, name="needle_tip")
    Globals.demo_env = Globals.env.CloneSelf(1)
    Globals.demo_env.StopSimulation()
    Globals.demo_robot = Globals.demo_env.GetRobot("pr2")
    Globals.demo_needle_tip = Globals.demo_env.GetKinBody("needle_tip")
    
    if not args.fake_data_segment:
        grabber = cloudprocpy.CloudGrabber()
        grabber.startRGBD()
        thc.grabber = grabber

    #Globals.viewer = trajoptpy.GetViewer(Globals.env)
    #####################
    
    #Globals.env.SetViewer('qtcoin')
    #threadClass().start()

    while True:
        redprint("Acquire point cloud")
         
        ################################
        redprint("Finding closest demonstration")
        if args.fake_data_segment:
            seg_name = args.fake_data_segment
        else:
            seg_name = find_closest(demofile)
        
        seg_info = demofile[seg_name]
        # redprint("using demo %s, description: %s"%(seg_name, seg_info["description"]))
    
        ################################
        
        redprint("Generating end-effector trajectory")    

        handles = []
        
        if seg_info.get('ar_marker_poses') is None:
            use_markers = False
        else:
            use_markers = True

        if use_markers:
            old_marker_poses_str = seg_info['ar_marker_poses']
            old_marker_poses = {}
            for i in old_marker_poses_str:
                old_marker_poses[int(i)] = old_marker_poses_str[i]
        
        
        # TODO: Have a check for only transformations -> rigid transformations
        if args.fake_data_segment:
            new_keypoints = demofile[args.fake_data_segment]["key_points"]
            if use_markers:
                new_marker_poses_str = demofile[args.fake_data_segment]["ar_marker_poses"]
                new_marker_poses = {}
                for i in new_marker_poses_str:
                    new_marker_poses[int(i)] = new_marker_poses_str[i]
            if seg_info['key_points'].keys().sort() != new_keypoints.keys().sort():
                print "Keypoints don't match."
                exit(1)
        else:
            if args.execution: Globals.pr2.update_rave()
            T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
            new_keypoints, new_marker_poses = fk.get_keypoints_execution(grabber, seg_info['key_points'].keys(), T_w_k, use_markers)


        print "Warping the points for the new situation."

        if "l_grab" in seg_info['extra_information'] or "r_grab" in seg_info['extra_information']:
            use_ntt_kp = False
        else:
            use_ntt_kp = True

        # Points from keypoints
        old_xyz, rigid, kp_mapping = fk.key_points_to_points(seg_info['key_points'], use_ntt_kp)
        new_xyz, _, _ = fk.key_points_to_points(new_keypoints, use_ntt_kp)

        # Points from markers
        if use_markers:
            old_common_poses, new_common_poses = find_common_marker_poses(old_marker_poses, new_marker_poses, new_keypoints.keys())
            old_m_xyz, rigid_m, _ = fk.key_points_to_points(old_common_poses, False)
            new_m_xyz, _, _ = fk.key_points_to_points(new_common_poses, False)

            if len(old_m_xyz) > 0 and rigid: rigid = False
            elif rigid_m and not old_xyz.any(): rigid = True

            # concatenate points
            if old_xyz.any() and old_m_xyz.any():
                old_xyz = np.r_[old_xyz, old_m_xyz]
            elif old_m_xyz.any():
                old_xyz = old_m_xyz
        
            if new_xyz.any() and new_m_xyz.any():
                new_xyz = np.r_[new_xyz, new_m_xyz]
            elif new_m_xyz.any():
                new_xyz = new_m_xyz
                
        if new_xyz.any() and new_xyz.shape != (4,4):
            #handles.append(Globals.env.plot3(old_xyz,5, (1,0,0,1)))
            #handles.append(Globals.env.plot3(old_xyz,5, np.array([(1,0,0) for _ in xrange(old_xyz.shape[0])])))
            handles.append(Globals.env.plot3(new_xyz,5, np.array([(0,0,1) for _ in xrange(old_xyz.shape[0])])))

        print 'Old points:', old_xyz
        print 'New points:', new_xyz
        
        #if not yes_or_no.yes_or_no("Use identity?"):
        if rigid:
            f = registration.ThinPlateSpline()
            rel_tfm = new_xyz.dot(np.linalg.inv(old_xyz))
            f.init_rigid_tfm(rel_tfm)
            bend_c = 0
            rot_c = 0
            scale_c = 0
            max_error = None
        elif len(new_xyz) > 0:
            #f.fit(demopoints_m3, newpoints_m3, 10,10)
            # TODO - check if this regularization on bending is correct
            #if "right_hole_normal" in new_keypoints or "left_hole_normal" in new_keypoints:
            #    bend_c = 0.1
            #    rot_c = [1e-5,1e-5,0.1]
            #wt = 5
            #wt_n = np.ones(len(old_xyz))
            #if kp_mapping.get("right_hole_normal"):
            #    wt_n[kp_mapping["right_hole_normal"][0]] = wt
            #if kp_mapping.get("left_hole_normal"):
            #    wt_n[kp_mapping["left_hole_normal"][0]] = wt
            #else:
            #    bend_c = 0.1
            #    rot_c = 1e-5#[0,0,1e-5]
            #    wt_n = None
            # f = registration.fit_ThinPlateSpline(old_xyz, new_xyz, bend_coef = bend_c,rot_coef = rot_c)
            bend_c = 0.05
            rot_c = [1e-3, 1e-3, 1e-3]
            scale_c = 0.1
            f = registration.fit_ThinPlateSpline_RotReg(old_xyz, new_xyz, bend_c, rot_c, scale_c)
            np.set_printoptions(precision=3)
            max_error = np.max(np.abs(f.transform_points(old_xyz) - new_xyz))
            print "nonlinear part", f.w_ng
            print "affine part", f.lin_ag
            print "translation part", f.trans_g
            print "residual", f.transform_points(old_xyz) - new_xyz
            print "max error ", max_error
        else:
            f = registration.ThinPlateSpline()
            bend_c = 0
            rot_c = 0
            scale_c = 0
            max_error = None
                        
#         else:
#             f = registration.ThinPlateSpline()            
#             bend_c = 0
#             rot_c = 0
#         
        if old_xyz.any() and old_xyz.shape != (4,4):
            tfm_xyz = f.transform_points(old_xyz)
            handles.append(Globals.env.plot3(tfm_xyz,5, np.array([(0,1,0) for _ in xrange(tfm_xyz.shape[0])])))
        
        #f = registration.ThinPlateSpline() XXX XXX
        
        if new_xyz.any() and new_xyz.shape != (4,4):
            handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0), old_xyz.max(axis=0), xres = .1, yres = .1, zres = .04))

        if args.ask:
#             if new_xyz.any() and new_xyz.shape != (4,4):
#                 import visualize
#                 visualize.plot_mesh_points(f.transform_points, old_xyz, new_xyz)
                #lines = plotting_openrave.gen_grid(f.transform_points, np.array([0,-1,0]), np.array([1,1,1]))
                #plotting_openrave.plot_lines(lines)
            if not yes_or_no.yes_or_no("Continue?"):
                continue
        
        miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
        success = True
        print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends

        # Assuming only lgrab or rgrab
        use_needle = None
        for lr in 'lr':      
            if "%s_grab"%lr in seg_info['extra_information']:
                if "needle_tip_transform" in seg_info['key_points']:
                    demo_ntfm = np.array(seg_info['key_points']['needle_tip_transform'])
                    ntfm = np.array(new_keypoints['needle_tip_transform'])
                    use_needle = lr
                elif Globals.rel_ntfm is not None:
                    T_w_g = Globals.robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform()
                    T_g_n = Globals.rel_ntfm
                    ntfm = T_w_g.dot(T_g_n)
                    T_w_g_demo = Globals.demo_robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform()
                    T_g_n_demo = Globals.demo_rel_ntfm
                    demo_ntfm = T_w_g_demo.dot(T_g_n_demo)
                    use_needle = lr
 
        if use_needle is not None:
            setup_needle_grabs(use_needle, seg_info["joint_states"]["name"], seg_info["joint_states"]["look_position"], demo_ntfm, ntfm)
        

        if args.logging:
            with open(filename,'a') as fh:
                fh.write("- seg_name: %s\n"%seg_name)
                fh.write("  tps_info: \n")
                if max_error is not None:
                    res_cost, bend_cost, tot_cost = f.fitting_cost(new_xyz, bend_c)
                    fh.write("    res_cost: %f\n    bend_cost: %f\n    tot_cost: %f\n"%(res_cost, bend_cost, tot_cost))
                    fh.write("    max_tps_error: %f\n"%max_error)
                else:
                    fh.write("    res_cost: -1\n    bend_cost: -1\n    tot_cost: -1\n    max_tps_error: -1\n")
                
                fh.write("  trajopt_info: \n")


        for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
            
            if args.execution: Globals.pr2.update_rave()

            # Changing the trajectory according to the end-effector
            full_traj = np.c_[seg_info["leftarm"][i_start:i_end+1], seg_info["rightarm"][i_start:i_end+1]]
            full_traj = mu.remove_duplicate_rows(full_traj)
            _, ds_traj =  resampling.adaptive_resample(full_traj, tol=.01, max_change=.1) # about 2.5 degrees, 10 degrees

            Globals.robot.SetActiveDOFs(np.r_[Globals.robot.GetManipulator("leftarm").GetArmIndices(), Globals.robot.GetManipulator("rightarm").GetArmIndices()])
            Globals.demo_robot.SetActiveDOFs(np.r_[Globals.robot.GetManipulator("leftarm").GetArmIndices(), Globals.robot.GetManipulator("rightarm").GetArmIndices()])        
            Globals.demo_robot.SetDOFValues(Globals.robot.GetDOFValues())
        

            ################################    
            redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg))

            bodypart2traj = {}
            arms_used = ""
            
            if args.logging:
                with open(filename, 'a') as fh:
                    fh.write("    mini_seg%i: \n"%i_miniseg)
        
            for lr in 'lr':
                
#                 if "%s_grab"%lr in seg_info['extra_information']:
#                     if "needle_tip_transform" in seg_info['key_points']:
#                         demo_ntfm = np.array(seg_info['key_points']['needle_tip_transform'])
#                         ntfm = np.array(new_keypoints['needle_tip_transform'])
#                         use_needle = True
#                     elif Globals.rel_ntfm is not None:
#                         T_w_g = Globals.robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform()
#                         T_g_n = Globals.rel_ntfm
#                         ntfm = T_w_g.dot(T_g_n)
#                         T_w_g_demo = Globals.demo_robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform()
#                         T_g_n_demo = Globals.demo_rel_ntfm
#                         demo_ntfm = T_w_g_demo.dot(T_g_n_demo)
#                         use_needle = True
#                     else:
#                         use_needle = False
#                 else:
#                     use_needle = False
                
                if use_needle == lr:
                    Globals.sponge.Enable(False)
                    arm_traj = {'l':ds_traj[:,:7], 'r':ds_traj[:,7:]}[lr]
                    old_ee_traj = setup_needle_traj (lr, arm_traj)
                    link = Globals.needle_tip.GetLinks()[0]  
                    redprint("Using needle for trajectory execution...")

                else:
                    arm = {"l":"leftarm", "r":"rightarm"}[lr]
                    Globals.demo_robot.SetActiveDOFs(Globals.robot.GetManipulator(arm).GetArmIndices())
                    if seg_name == "8_knot_tie_next0_seg00":
                        Globals.sponge.Enable(False)
                    else:
                        Globals.sponge.Enable(True)
                    ee_link_name = "%s_gripper_tool_frame"%lr
                    link = Globals.robot.GetLink(ee_link_name)
                    #old_ee_traj = np.asarray(seg_info[ee_link_name]["hmat"])
                    demo_link = Globals.demo_robot.GetLink(ee_link_name)
                    old_ee_traj = []
                    arm_traj = {'l':ds_traj[:,:7], 'r':ds_traj[:,7:]}[lr]
                    for row in arm_traj:
                        Globals.demo_robot.SetActiveDOFValues(row)
                        old_ee_traj.append(demo_link.GetTransform())
                        

############ CAN YOU PLOT THIS OLD_EE_TRAJ?        
                old_ee_traj = np.asarray(old_ee_traj)
                old_joint_traj = {'l':ds_traj[:,:7], 'r':ds_traj[:,7:]}[lr]

                if lr == 'l':#arm_moved(old_joint_traj):
                    
                    manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
                    new_ee_traj = f.transform_hmats(old_ee_traj)
                    #new_ee_traj = old_ee_traj
                    
#################### CAN YOU PLOT THIS NEW_EE_TRAJ?
                    
                    handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1)))
                    handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1)))
                    #old_poses = [conv.hmat_to_pose(hmat) for hmat in old_ee_traj]
                    #print new_ee_traj
                    #new_poses = [conv.hmat_to_pose(hmat) for hmat in new_ee_traj]
                    update_markers(new_ee_traj, old_ee_traj)
                    #thc.run()
                    

                    if args.execution: Globals.pr2.update_rave()
                    new_joint_traj, costs, cnts, pos_err = plan_follow_traj(Globals.robot, manip_name,
                                                      link, new_ee_traj, old_joint_traj, True)
                    new_joint_traj = unwrap_joint_traj (lr, new_joint_traj)
                    # (robot, manip_name, ee_link, new_hmats, old_traj):
                    part_name = {"l":"larm", "r":"rarm"}[lr]
                    bodypart2traj[part_name] = new_joint_traj
                    
                    arms_used += lr
                    
                    if args.logging:
                        with open(filename, 'a') as fh:
                            fh.write("    - %s: \n"%lr)
                            fh.write("        costs: \n")
                            tot_cost = 0
                            for c_name, c_val in costs:
                                fh.write("        - %s: %f\n"%(c_name, c_val))
                                tot_cost += c_val
                            fh.write("        - tot_cost: %f\n"%tot_cost)
                            fh.write("        constraints: \n")
                            for cnt in cnts:
                                fh.write("        - %s\n"%str(cnt))
                            fh.write("        max_pos_error: %f\n"%pos_err)
                        

            ################################    
            redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, arms_used))

            for lr in 'lr':
                set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start]))
                #trajoptpy.GetViewer(Globals.env).Idle()

            if len(bodypart2traj) > 0:
                exec_traj_maybesim(bodypart2traj, speed_factor=1)

            # TODO measure failure condtions
            if not success:
                break
            

        Globals.robot.ReleaseAllGrabbed()
        Globals.demo_robot.ReleaseAllGrabbed()
    
        redprint("Segment %s result: %s"%(seg_name, success))
    
        if args.fake_data_segment: break