# Path: lfd/rapprentice/math_utils.py
# def interp2d(x,xp,yp):
# def interp_mat(x, xp):
# def normalize(x):
# def normr(x):
# def normc(x):
# def norms(x,ax):
# def intround(x):
# def deriv(x):
# def linspace2d(start,end,n):
# def remove_duplicate_rows(mat):
# def invertHmat(hmat):
#     T = len(x)
#     R = hmat[:3,:3]


import bulletsimpy
import numpy as np
import trajoptpy
from lfd.rapprentice import math_utils, retiming
        from openravepy import matrixFromAxisAngle

def transform(hmat, p):
    return hmat[:3,:3].dot(p) + hmat[:3,3]

def in_grasp_region(robot, lr, pt):
    tol = .00

    manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
    manip = robot.GetManipulator(manip_name)
    l_finger = robot.GetLink("%s_gripper_l_finger_tip_link"%lr)
    r_finger = robot.GetLink("%s_gripper_r_finger_tip_link"%lr)

    def on_inner_side(pt, finger_lr):
        finger = l_finger
        closing_dir = np.cross(manip.GetLocalToolDirection(), [-1, 0, 0])
        local_inner_pt = np.array([0.234402, -0.299, 0])/20.
        if finger_lr == "r":
            finger = r_finger
            closing_dir *= -1
            local_inner_pt[1] *= -1
        inner_pt = transform(finger.GetTransform(), local_inner_pt)

# Path: lfd/rapprentice/math_utils.py
# def interp2d(x,xp,yp):
# def interp_mat(x, xp):
# def normalize(x):
# def normr(x):
# def normc(x):
# def norms(x,ax):
# def intround(x):
# def deriv(x):
# def linspace2d(start,end,n):
# def remove_duplicate_rows(mat):
# def invertHmat(hmat):
#     T = len(x)
#     R = hmat[:3,:3]


# Path: lfd/rapprentice/ropesim.py
import bulletsimpy
import numpy as np
import trajoptpy
from lfd.rapprentice import math_utils, retiming
        from openravepy import matrixFromAxisAngle


def transform(hmat, p):
    return hmat[:3,:3].dot(p) + hmat[:3,3]

def in_grasp_region(robot, lr, pt):
    tol = .00

    manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
    manip = robot.GetManipulator(manip_name)
    l_finger = robot.GetLink("%s_gripper_l_finger_tip_link"%lr)
    r_finger = robot.GetLink("%s_gripper_r_finger_tip_link"%lr)

    def on_inner_side(pt, finger_lr):
        finger = l_finger
        closing_dir = np.cross(manip.GetLocalToolDirection(), [-1, 0, 0])
        local_inner_pt = np.array([0.234402, -0.299, 0])/20.
        if finger_lr == "r":
            finger = r_finger
            closing_dir *= -1
            local_inner_pt[1] *= -1
        inner_pt = transform(finger.GetTransform(), local_inner_pt)

        return manip.GetTransform()[:3,:3].dot(closing_dir).dot(pt - inner_pt) > 0