Esempio n. 1
0
def get_finger_pts_traj(robot, lr, full_traj_or_ee_finger_traj):
    """
    ee_traj = sim_util.get_ee_traj(robot, lr, arm_traj)
    flr2finger_pts_traj1 = get_finger_pts_traj(robot, lr, (ee_traj, finger_traj))
    
    full_traj = sim_util.get_full_traj(robot, {lr:arm_traj}, {lr:finger_traj})
    flr2finger_pts_traj2 = get_finger_pts_traj(robot, lr, full_traj)
    """
    flr2finger_pts_traj = {}
    assert type(full_traj_or_ee_finger_traj) == tuple
    if full_traj_or_ee_finger_traj[0].ndim == 3:
        ee_traj, finger_traj = full_traj_or_ee_finger_traj
        assert len(ee_traj) == len(finger_traj)
        for finger_lr in 'lr':
            gripper_full_traj = get_full_traj(robot, {}, {lr:finger_traj})
            rel_ee_traj = get_ee_traj(robot, lr, gripper_full_traj)
            rel_finger_traj = get_ee_traj(robot, lr, gripper_full_traj, ee_link_name_fmt="%s"+"_gripper_%s_finger_tip_link"%finger_lr)
            
            flr2finger_pts_traj[finger_lr] = []
            for (world_from_ee, world_from_rel_ee, world_from_rel_finger) in zip(ee_traj, rel_ee_traj, rel_finger_traj):
                ee_from_finger = mu.invertHmat(world_from_rel_ee).dot(world_from_rel_finger)
                world_from_finger = world_from_ee.dot(ee_from_finger)
                finger_pts = world_from_finger[:3,3] + get_finger_rel_pts(finger_lr).dot(world_from_finger[:3,:3].T)
                flr2finger_pts_traj[finger_lr].append(finger_pts)
            flr2finger_pts_traj[finger_lr] = np.asarray(flr2finger_pts_traj[finger_lr])
    else:
        full_traj = full_traj_or_ee_finger_traj
        for finger_lr in 'lr':
            finger_traj = get_ee_traj(robot, lr, full_traj, ee_link_name_fmt="%s"+"_gripper_%s_finger_tip_link"%finger_lr)
            flr2finger_pts_traj[finger_lr] = []
            for world_from_finger in finger_traj:
                flr2finger_pts_traj[finger_lr].append(world_from_finger[:3,3] + get_finger_rel_pts(finger_lr).dot(world_from_finger[:3,:3].T))
            flr2finger_pts_traj[finger_lr] = np.asarray(flr2finger_pts_traj[finger_lr])
    return flr2finger_pts_traj
Esempio n. 2
0
def get_finger_pts_traj(robot, lr, full_traj_or_ee_finger_traj):
    """
    ee_traj = sim_util.get_ee_traj(robot, lr, arm_traj)
    flr2finger_pts_traj1 = get_finger_pts_traj(robot, lr, (ee_traj, finger_traj))
    
    full_traj = sim_util.get_full_traj(robot, {lr:arm_traj}, {lr:finger_traj})
    flr2finger_pts_traj2 = get_finger_pts_traj(robot, lr, full_traj)
    """
    flr2finger_pts_traj = {}
    assert type(full_traj_or_ee_finger_traj) == tuple
    if full_traj_or_ee_finger_traj[0].ndim == 3:
        ee_traj, finger_traj = full_traj_or_ee_finger_traj
        assert len(ee_traj) == len(finger_traj)
        for finger_lr in 'lr':
            gripper_full_traj = get_full_traj(robot, {}, {lr: finger_traj})
            rel_ee_traj = get_ee_traj(robot, lr, gripper_full_traj)
            rel_finger_traj = get_ee_traj(
                robot,
                lr,
                gripper_full_traj,
                ee_link_name_fmt="%s" +
                "_gripper_%s_finger_tip_link" % finger_lr)

            flr2finger_pts_traj[finger_lr] = []
            for (world_from_ee, world_from_rel_ee,
                 world_from_rel_finger) in zip(ee_traj, rel_ee_traj,
                                               rel_finger_traj):
                ee_from_finger = mu.invertHmat(world_from_rel_ee).dot(
                    world_from_rel_finger)
                world_from_finger = world_from_ee.dot(ee_from_finger)
                finger_pts = world_from_finger[:3, 3] + get_finger_rel_pts(
                    finger_lr).dot(world_from_finger[:3, :3].T)
                flr2finger_pts_traj[finger_lr].append(finger_pts)
            flr2finger_pts_traj[finger_lr] = np.asarray(
                flr2finger_pts_traj[finger_lr])
    else:
        full_traj = full_traj_or_ee_finger_traj
        for finger_lr in 'lr':
            finger_traj = get_ee_traj(
                robot,
                lr,
                full_traj,
                ee_link_name_fmt="%s" +
                "_gripper_%s_finger_tip_link" % finger_lr)
            flr2finger_pts_traj[finger_lr] = []
            for world_from_finger in finger_traj:
                flr2finger_pts_traj[finger_lr].append(
                    world_from_finger[:3, 3] + get_finger_rel_pts(
                        finger_lr).dot(world_from_finger[:3, :3].T))
            flr2finger_pts_traj[finger_lr] = np.asarray(
                flr2finger_pts_traj[finger_lr])
    return flr2finger_pts_traj
Esempio n. 3
0
def plan_follow_trajs(robot,
                      manip_name,
                      ee_link_names,
                      ee_trajs,
                      old_traj,
                      no_collision_cost_first=False,
                      use_collision_cost=True,
                      start_fixed=False,
                      joint_vel_limits=None,
                      beta_pos=settings.BETA_POS,
                      beta_rot=settings.BETA_ROT,
                      gamma=settings.GAMMA):
    orig_dof_inds = robot.GetActiveDOFIndices()
    orig_dof_vals = robot.GetDOFValues()

    n_steps = len(ee_trajs[0])
    dof_inds = sim_util.dof_inds_from_name(robot, manip_name)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == len(dof_inds)
    assert len(ee_link_names) == len(ee_trajs)

    if no_collision_cost_first:
        init_traj, _, _ = plan_follow_trajs(robot,
                                            manip_name,
                                            ee_link_names,
                                            ee_trajs,
                                            old_traj,
                                            no_collision_cost_first=False,
                                            use_collision_cost=False,
                                            start_fixed=start_fixed,
                                            joint_vel_limits=joint_vel_limits,
                                            beta_pos=beta_pos,
                                            beta_rot=beta_rot,
                                            gamma=gamma)
    else:
        init_traj = old_traj.copy()

    if start_fixed:
        init_traj = np.r_[robot.GetDOFValues(dof_inds)[None, :], init_traj[1:]]
        sim_util.unwrap_in_place(init_traj, dof_inds)
        init_traj += robot.GetDOFValues(dof_inds) - init_traj[0, :]

    request = {
        "basic_info": {
            "n_steps": n_steps,
            "manip": manip_name,
            "start_fixed": start_fixed
        },
        "costs": [
            {
                "type": "joint_vel",
                "params": {
                    "coeffs": [gamma / (n_steps - 1)]
                }
            },
        ],
        "constraints": [],
        "init_info": {
            "type": "given_traj",
            "data": [x.tolist() for x in init_traj]
        }
    }

    if use_collision_cost:
        request["costs"].append({
            "type": "collision",
            "params": {
                "continuous": True,
                "coeffs": [
                    1000
                ],  # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
                "dist_pen": [
                    0.025
                ]  # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
            }
        })

    if joint_vel_limits is not None:
        request["constraints"].append({
            "type": "joint_vel_limits",
            "params": {
                "vals": joint_vel_limits,
                "first_step": 0,
                "last_step": n_steps - 1
            }
        })

    for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
        poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj]
        for (i_step, pose) in enumerate(poses):
            if start_fixed and i_step == 0:
                continue
            request["costs"].append({
                "type": "pose",
                "params": {
                    "xyz": pose[4:7].tolist(),
                    "wxyz": pose[0:4].tolist(),
                    "link": ee_link_name,
                    "timestep": i_step,
                    "pos_coeffs": [np.sqrt(beta_pos / n_steps)] * 3,
                    "rot_coeffs": [np.sqrt(beta_rot / n_steps)] * 3
                }
            })

    s = json.dumps(request)
    with openravepy.RobotStateSaver(robot):
        orig_dof_vals
        with util.suppress_stdout():
            prob = trajoptpy.ConstructProblem(s, robot.GetEnv(
            ))  # create object that stores optimization problem
            result = trajoptpy.OptimizeProblem(prob)  # do optimization
    traj = result.GetTraj()

    pose_costs = np.sum([
        cost_val for (cost_type, cost_val) in result.GetCosts()
        if cost_type == "pose"
    ])
    pose_err = []
    with openravepy.RobotStateSaver(robot):
        for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
            ee_link = robot.GetLink(ee_link_name)
            for (i_step, hmat) in enumerate(ee_traj):
                if start_fixed and i_step == 0:
                    continue
                robot.SetDOFValues(traj[i_step], dof_inds)
                new_hmat = ee_link.GetTransform()
                pose_err.append(
                    openravepy.poseFromMatrix(
                        mu.invertHmat(hmat).dot(new_hmat)))
    pose_err = np.asarray(pose_err)
    pose_costs2 = (beta_rot / n_steps) * np.square(pose_err[:, 1:4]).sum() + (
        beta_pos / n_steps) * np.square(pose_err[:, 4:7]).sum()

    joint_vel_cost = np.sum([
        cost_val for (cost_type, cost_val) in result.GetCosts()
        if cost_type == "joint_vel"
    ])
    joint_vel_err = np.diff(traj, axis=0)
    joint_vel_cost2 = (gamma / (n_steps - 1)) * np.square(joint_vel_err).sum()
    sim_util.unwrap_in_place(traj, dof_inds)
    joint_vel_err = np.diff(traj, axis=0)

    collision_costs = [
        cost_val for (cost_type, cost_val) in result.GetCosts()
        if "collision" in cost_type
    ]
    if len(collision_costs) > 0:
        collision_err = np.asarray(collision_costs)
        collision_costs = np.sum(collision_costs)

    obj_value = np.sum(
        [cost_val for (cost_type, cost_val) in result.GetCosts()])

    print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed")
    print "{:>15} | {:>10}".format("COSTS", "-" * 23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost,
                                                  joint_vel_cost2)
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)",
                                                   collision_costs, "-")
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs,
                                                  pose_costs2)
    print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-")
    print ""

    print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max")
    print "{:>15} | {:>10}".format("ERRORS", "-" * 23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format(
        "joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()),
        np.rad2deg(np.abs(joint_vel_err).max()))
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10,.4}".format(
            "collision(s)",
            np.abs(-collision_err).min(),
            np.abs(-collision_err).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format(
        "rot pose(s)",
        np.abs(pose_err[:, 1:4]).min(),
        np.abs(pose_err[:, 1:4]).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format(
        "trans pose(s)",
        np.abs(pose_err[:, 4:7]).min(),
        np.abs(pose_err[:, 4:7]).max())
    print ""

    # make sure this function doesn't change state of the robot
    assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices())
    assert not np.any(orig_dof_vals - robot.GetDOFValues())

    return traj, obj_value, pose_costs
Esempio n. 4
0
def plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, 
                     no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None,
                     beta_pos=settings.BETA_POS, beta_rot=settings.BETA_ROT, gamma=settings.GAMMA):
    orig_dof_inds = robot.GetActiveDOFIndices()
    orig_dof_vals = robot.GetDOFValues()
    
    n_steps = len(ee_trajs[0])
    dof_inds = sim_util.dof_inds_from_name(robot, manip_name)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == len(dof_inds)
    assert len(ee_link_names) == len(ee_trajs)
        
    if no_collision_cost_first:
        init_traj, _, _ = plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, 
                                        no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits,
                                        beta_pos = beta_pos, beta_rot = beta_rot, gamma = gamma)
    else:
        init_traj = old_traj.copy()

    if start_fixed:
        init_traj = np.r_[robot.GetDOFValues(dof_inds)[None,:], init_traj[1:]]
        sim_util.unwrap_in_place(init_traj, dof_inds)
        init_traj += robot.GetDOFValues(dof_inds) - init_traj[0,:]

    request = {
        "basic_info" : {
            "n_steps" : n_steps,
            "manip" : manip_name,
            "start_fixed" : start_fixed
        },
        "costs" : [
        {
            "type" : "joint_vel",
            "params": {"coeffs" : [gamma/(n_steps-1)]}
        },            
        ],
        "constraints" : [
        ],
        "init_info" : {
            "type":"given_traj",
            "data":[x.tolist() for x in init_traj]
        }
    }
    
    if use_collision_cost:
        request["costs"].append(
            {
                "type" : "collision",
                "params" : {
                  "continuous" : True,
                  "coeffs" : [1000],  # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
                  "dist_pen" : [0.025]  # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
                }
            })
    
    if joint_vel_limits is not None:
        request["constraints"].append(
             {
                "type" : "joint_vel_limits",
                "params": {"vals" : joint_vel_limits,
                           "first_step" : 0,
                           "last_step" : n_steps-1
                           }
              })

    for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
        poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj]
        for (i_step,pose) in enumerate(poses):
            if start_fixed and i_step == 0:
                continue
            request["costs"].append(
                {"type":"pose",
                 "params":{
                    "xyz":pose[4:7].tolist(),
                    "wxyz":pose[0:4].tolist(),
                    "link":ee_link_name,
                    "timestep":i_step,
                    "pos_coeffs":[np.sqrt(beta_pos/n_steps)]*3,
                    "rot_coeffs":[np.sqrt(beta_rot/n_steps)]*3
                 }
                })

    s = json.dumps(request)
    with openravepy.RobotStateSaver(robot):
        orig_dof_vals
        with util.suppress_stdout():
            prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem
            result = trajoptpy.OptimizeProblem(prob) # do optimization
    traj = result.GetTraj()
    
    pose_costs = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "pose"])
    pose_err = []
    with openravepy.RobotStateSaver(robot):
        for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
            ee_link = robot.GetLink(ee_link_name)
            for (i_step, hmat) in enumerate(ee_traj):
                if start_fixed and i_step == 0:
                    continue
                robot.SetDOFValues(traj[i_step], dof_inds)
                new_hmat = ee_link.GetTransform()
                pose_err.append(openravepy.poseFromMatrix(mu.invertHmat(hmat).dot(new_hmat)))
    pose_err = np.asarray(pose_err)
    pose_costs2 = (beta_rot/n_steps) * np.square(pose_err[:,1:4]).sum() + (beta_pos/n_steps) * np.square(pose_err[:,4:7]).sum()

    joint_vel_cost = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel"])
    joint_vel_err = np.diff(traj, axis=0)
    joint_vel_cost2 = (gamma/(n_steps-1)) * np.square(joint_vel_err).sum()
    sim_util.unwrap_in_place(traj, dof_inds)
    joint_vel_err = np.diff(traj, axis=0)
    
    collision_costs = [cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type]
    if len(collision_costs) > 0:
        collision_err = np.asarray(collision_costs)
        collision_costs = np.sum(collision_costs)

    obj_value = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts()])
    
    print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed")
    print "{:>15} | {:>10}".format("COSTS", "-"*23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2)
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-")
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs, pose_costs2)
    print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-")
    print ""

    print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max")
    print "{:>15} | {:>10}".format("ERRORS", "-"*23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max()))
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10,.4}".format("collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("rot pose(s)", np.abs(pose_err[:,1:4]).min(), np.abs(pose_err[:,1:4]).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("trans pose(s)", np.abs(pose_err[:,4:7]).min(), np.abs(pose_err[:,4:7]).max())
    print ""

    # make sure this function doesn't change state of the robot
    assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices())
    assert not np.any(orig_dof_vals - robot.GetDOFValues())
    
    return traj, obj_value, pose_costs
Esempio n. 5
0
def plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, 
                     no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None,
                     beta_pos=settings.BETA_POS, beta_rot=settings.BETA_ROT, gamma=settings.GAMMA, pose_weights=1, grasp_cup=False, R=0.02, D=0.035, cup_xyz=None):
    orig_dof_inds = robot.GetActiveDOFIndices()
    orig_dof_vals = robot.GetDOFValues()
    
    n_steps = len(ee_trajs[0])
    dof_inds = sim_util.dof_inds_from_name(robot, manip_name)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == len(dof_inds)
    assert len(ee_link_names) == len(ee_trajs)
        
    if no_collision_cost_first:
        init_traj, _, _ = plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, 
                                        no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits,
                                        beta_pos = beta_pos, beta_rot = beta_rot, gamma = gamma)
    else:
        init_traj = old_traj.copy()

    if start_fixed:
        init_traj = np.r_[robot.GetDOFValues(dof_inds)[None,:], init_traj[1:]]
        sim_util.unwrap_in_place(init_traj, dof_inds)
        init_traj += robot.GetDOFValues(dof_inds) - init_traj[0,:]

    request = {
        "basic_info" : {
            "n_steps" : n_steps,
            "manip" : manip_name,
            "start_fixed" : start_fixed
        },
        "costs" : [
        {
            "type" : "joint_vel",
            #"params": {"coeffs" : [gamma/(n_steps-1)]}
            "params": {"coeffs" : [1]}
        },            
        ],
        "constraints" : [
        {                                                                             
            "type" : "cart_vel",                                                        
            "name" : "cart_vel",                                                        
            "params" : {                                                                
                "max_displacement" : .02,
                "first_step" : 0,                                                       
                "last_step" : n_steps-1, #inclusive                                     
                "link" : "r_gripper_tool_frame"                                         
            }
        },
        #{
        #    "type" : "collision",
        #    "params" : {
        #        "continuous" : True,
        #        "coeffs" : [100]*(n_steps-3) + [0]*3,
        #        "dist_pen" : [0.01]
        #    }
        #}
        ],
        "init_info" : {
            "type":"given_traj",
            "data":[x.tolist() for x in init_traj]
        }
    }

    if use_collision_cost:
        request["costs"].append(
            {
                "type" : "collision",
                "params" : {
                  "continuous" : True,
                  "coeffs" : [20000],  # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
                  #"coeffs" : [0] * (len(ee_trajs[0]) - 3) + [100]*3,
                  "dist_pen" : [0.025]  # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
                }
            })
    
    if joint_vel_limits is not None:
        request["constraints"].append(
             {
                "type" : "joint_vel_limits",
                "params": {"vals" : joint_vel_limits,
                           "first_step" : 0,
                           "last_step" : n_steps-1
                           }
              })

    #coeff_mult = [0.0] * (len(poses) - 1) + [100]
    #coeff_mult = np.linspace(1, 100, num=len(poses))
    #coeff_mult_trans = np.logspace(-3, 1, num=len(ee_trajs[0]))
    #coeff_mult_rot = np.logspace(-3, 1, num=len(ee_trajs[0]))
    coeff_mult_trans = np.array([0] * (len(ee_trajs[0])-1) + [100])
    coeff_mult_rot = np.array([0] * (len(ee_trajs[0])-1) + [100])
    if not grasp_cup:
        #coeff_mult_trans = np.array([0.1] * (len(ee_trajs[0])-1) + [100])
        #coeff_mult_rot = np.array([1] * len(ee_trajs[0]))
        for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
            poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj]
            for (i_step,pose) in enumerate(poses):
                if start_fixed and i_step == 0:
                    continue
                request["costs"].append(
                    {"type":"pose",
                     "params":{
                        "xyz":pose[4:7].tolist(),
                        "wxyz":pose[0:4].tolist(),
                        "link":ee_link_name,
                        "timestep":i_step,
                        #"pos_coeffs":list(pose_weights * [np.sqrt(beta_pos/n_steps)]*3),
                        "pos_coeffs":[np.sqrt(beta_pos/n_steps) * coeff_mult_trans[i_step]]*3,
                        #"rot_coeffs":list(pose_weights * [np.sqrt(beta_rot/n_steps)]*3)
                        "rot_coeffs":[np.sqrt(beta_rot/n_steps) * coeff_mult_rot[i_step]]*3
                     }
                    })


    print "Cup XYZ:", cup_xyz

    s = json.dumps(request)
    with openravepy.RobotStateSaver(robot):
        with util.suppress_stdout():
            prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem
            # for t in range(1,n_steps): 
            #     prob.AddCost(table_cost, [(t,j) for j in range(7)], "table%i"%t)






            if grasp_cup:
                tool_link = robot.GetLink("r_gripper_tool_frame")
                local_dir = np.array([0.,0.,1.])
                manip = robot.GetManipulator(manip_name)
                arm_inds = manip.GetArmIndices()
                arm_joints = [robot.GetJointFromDOFIndex(ind) for ind in arm_inds]
                f = lambda x: gripper_tilt(x, robot, arm_inds, tool_link, local_dir)
                f2 = lambda x: gripper_tilt2(x, robot, arm_inds, tool_link, local_dir)
                dfdx = lambda x: gripper_tilt_dx(x, robot, arm_inds, tool_link, local_dir, arm_joints)

                #for t in xrange(1,n_steps):
                for t in xrange(n_steps-2,n_steps):
                    prob.AddConstraint(f, dfdx, [(t,j) for j in xrange(7)], "EQ", "up%i"%t)

                #for t in xrange(1,n_steps-1):
                #    prob.AddConstraint(f2, [(t,j) for j in xrange(7)], "EQ", "up%i"%t)
                #prob.AddConstraint(f2, [(n_steps-1,j) for j in xrange(7)], "EQ", "up%i"%(n_steps-1))

                g = lambda x: gripper_around_cup(x, robot, arm_inds, tool_link, cup_xyz, R, D)
                prob.AddConstraint(g, [(n_steps-1,j) for j in xrange(7)], "EQ", "cup%i"%(n_steps-1))
            result = trajoptpy.OptimizeProblem(prob) # do optimization

    print [(_name, viol) for (_name, viol) in result.GetConstraints() if viol > 0.0001]
    #assert [viol <= 1e-4 for (_name,viol) in result.GetConstraints()]

    traj = result.GetTraj()

    pose_costs = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "pose"])
    pose_err = []
    with openravepy.RobotStateSaver(robot):
        for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
            ee_link = robot.GetLink(ee_link_name)
            for (i_step, hmat) in enumerate(ee_traj):
                if start_fixed and i_step == 0:
                    continue
                robot.SetDOFValues(traj[i_step], dof_inds)
                new_hmat = ee_link.GetTransform()
                pose_err.append(openravepy.poseFromMatrix(mu.invertHmat(hmat).dot(new_hmat)))
    pose_err = np.asarray(pose_err)
    pose_costs2 = (beta_rot/n_steps) * np.square(pose_err[:,1:4]).sum() + (beta_pos/n_steps) * np.square(pose_err[:,4:7]).sum()

    pose_costs3 = (beta_rot/n_steps) * np.dot(coeff_mult_rot[np.newaxis,1:], np.square(pose_err[:,1:4])).sum() + (beta_pos/n_steps) * np.dot(coeff_mult_trans[np.newaxis,1:], np.square(pose_err[:,4:7])).sum()

    joint_vel_cost = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel"])
    joint_vel_err = np.diff(traj, axis=0)
    joint_vel_cost2 = (gamma/(n_steps-1)) * np.square(joint_vel_err).sum()
    sim_util.unwrap_in_place(traj, dof_inds)
    joint_vel_err = np.diff(traj, axis=0)
    
    collision_costs = [cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type]
    #collision_costs = [cost_val for (cost_type, cost_val) in result.GetConstraints() if "collision" in cost_type]
    if len(collision_costs) > 0:
        collision_err = np.asarray(collision_costs)
        collision_costs = np.sum(collision_costs)

    obj_value = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts()])
    
    print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed")
    print "{:>15} | {:>10}".format("COSTS", "-"*23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2)
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-")
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs, pose_costs2)
    print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-")
    print ""

    print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max")
    print "{:>15} | {:>10}".format("ERRORS", "-"*23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max()))
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10,.4}".format("collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("rot pose(s)", np.abs(pose_err[:,1:4]).min(), np.abs(pose_err[:,1:4]).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("trans pose(s)", np.abs(pose_err[:,4:7]).min(), np.abs(pose_err[:,4:7]).max())
    print ""

    # make sure this function doesn't change state of the robot
    assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices())
    assert not np.any(orig_dof_vals - robot.GetDOFValues())

    rot_err = np.abs(pose_err[:,1:4]).sum() 
    pos_err = np.abs(pose_err[:,4:7]).sum()

    print collision_err
    
    return traj, obj_value, (pose_costs3, np.abs(collision_err[:-1]).sum())