# 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/conversions.py
# def pose_to_trans_rot(pose):
# def hmat_to_pose(hmat):
# def pose_to_hmat(pose):
# def hmat_to_trans_rot(hmat): 
# def hmats_to_transs_rots(hmats):
# def trans_rot_to_hmat(trans, rot): 
# def xya_to_trans_rot(xya):
# def trans_rot_to_xya(trans, rot):
# def quat_to_yaw(q):
# def yaw_to_quat(yaw):
# def quat2mat(quat):
# def mat2quat(mat33):
# def mats2quats(mats):
# def quats2mats(quats):
# def xyzs_quats_to_poses(xyzs, quats):
# def rod2mat(rod):
# def point_stamed_to_pose_stamped(pts,orientation=(0,0,0,1)):
# def array_to_pose_array(xyz_arr, frame_id, quat_arr=None):
# def trans_rot_to_pose(trans, rot):
#     H = transformations.quaternion_matrix(rot) 
#     H[0:3, 3] = trans 
# 
# Path: lfd/rapprentice/resampling.py
# def lerp(a, b, fracs):
# def adaptive_resample(x, t = None, max_err = np.inf, max_dx = np.inf, max_dt = np.inf, normfunc = None):
# def get_velocities(positions, times, tol):
# def smooth_positions(positions, tol):
# def unif_resample(x,n,weights,tol=.001,deg=3):    
# def test_resample():
# def test_resample_big():
# def interp_quats(newtimes, oldtimes, oldquats):
# def interp_hmats(newtimes, oldtimes, oldhmats):


import numpy as np, os.path as osp
import openravepy as rave
import roslib
import pr2_controllers_msgs.msg as pcm
import trajectory_msgs.msg as tm
import sensor_msgs.msg as sm
import actionlib
import rospy
import geometry_msgs.msg as gm           
import move_base_msgs.msg as mbm   
from numpy import inf, zeros, dot, r_, pi
from numpy.linalg import norm, inv
from threading import Thread
from lfd.rapprentice import retiming, math_utils as mu,conversions as conv, func_utils, resampling
        ros_values = self.get_last_joint_message().position        
        rave_values = [ros_values[i_ros] for i_ros in self.good_ros_inds]        
        self.robot.SetJointValues(rave_values[:20],self.rave_inds[:20])
        self.robot.SetJointValues(rave_values[20:],self.rave_inds[20:])   

        # if use_map:
        #     (trans,rot) = self.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0))            
        #     self.robot.SetTransform(conv.trans_rot_to_hmat(trans, rot))            

    def update_rave_without_ros(self, joint_vals):
        self.robot.SetJointValues(joint_vals)

    def join_all(self):
        for thread in self.pending_threads:
            thread.join()
        self.pending_threads = []
    def is_moving(self):
        return any(thread.is_alive() for thread in self.pending_threads)

    def stop_all(self):
        self.larm.stop()
        self.rarm.stop()
        self.head.stop()
        self.torso.stop()
        for thread in self.pending_threads:
            thread.wants_exit = True


class TrajectoryControllerWrapper(object):


# 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/conversions.py
# def pose_to_trans_rot(pose):
# def hmat_to_pose(hmat):
# def pose_to_hmat(pose):
# def hmat_to_trans_rot(hmat): 
# def hmats_to_transs_rots(hmats):
# def trans_rot_to_hmat(trans, rot): 
# def xya_to_trans_rot(xya):
# def trans_rot_to_xya(trans, rot):
# def quat_to_yaw(q):
# def yaw_to_quat(yaw):
# def quat2mat(quat):
# def mat2quat(mat33):
# def mats2quats(mats):
# def quats2mats(quats):
# def xyzs_quats_to_poses(xyzs, quats):
# def rod2mat(rod):
# def point_stamed_to_pose_stamped(pts,orientation=(0,0,0,1)):
# def array_to_pose_array(xyz_arr, frame_id, quat_arr=None):
# def trans_rot_to_pose(trans, rot):
#     H = transformations.quaternion_matrix(rot) 
#     H[0:3, 3] = trans 
# 
# Path: lfd/rapprentice/resampling.py
# def lerp(a, b, fracs):
# def adaptive_resample(x, t = None, max_err = np.inf, max_dx = np.inf, max_dt = np.inf, normfunc = None):
# def get_velocities(positions, times, tol):
# def smooth_positions(positions, tol):
# def unif_resample(x,n,weights,tol=.001,deg=3):    
# def test_resample():
# def test_resample_big():
# def interp_quats(newtimes, oldtimes, oldquats):
# def interp_hmats(newtimes, oldtimes, oldhmats):


# Path: lfd/rapprentice/PR2.py
import numpy as np, os.path as osp
import openravepy as rave
import roslib
import pr2_controllers_msgs.msg as pcm
import trajectory_msgs.msg as tm
import sensor_msgs.msg as sm
import actionlib
import rospy
import geometry_msgs.msg as gm           
import move_base_msgs.msg as mbm   
from numpy import inf, zeros, dot, r_, pi
from numpy.linalg import norm, inv
from threading import Thread
from lfd.rapprentice import retiming, math_utils as mu,conversions as conv, func_utils, resampling

        ros_values = self.get_last_joint_message().position        
        rave_values = [ros_values[i_ros] for i_ros in self.good_ros_inds]        
        self.robot.SetJointValues(rave_values[:20],self.rave_inds[:20])
        self.robot.SetJointValues(rave_values[20:],self.rave_inds[20:])   

        # if use_map:
        #     (trans,rot) = self.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0))            
        #     self.robot.SetTransform(conv.trans_rot_to_hmat(trans, rot))            

    def update_rave_without_ros(self, joint_vals):
        self.robot.SetJointValues(joint_vals)

    def join_all(self):
        for thread in self.pending_threads:
            thread.join()
        self.pending_threads = []
    def is_moving(self):
        return any(thread.is_alive() for thread in self.pending_threads)

    def stop_all(self):
        self.larm.stop()
        self.rarm.stop()
        self.head.stop()
        self.torso.stop()
        for thread in self.pending_threads:
            thread.wants_exit = True


class TrajectoryControllerWrapper(object):


    def __init__(self, pr2, controller_name):