# 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
            jtp = tm.JointTrajectoryPoint()
            jtp.time_from_start = rospy.Duration(t)
            jtp.positions = [a]
            jt.points.append(jtp)
        self.diag_pub.publish(jt)

        self.pr2.start_thread(GripperTrajectoryThread(self, times_up, angs_up))
        #self.pr2.start_thread(GripperTrajectoryThread(self, times, angs))
    def get_angle(self):
        return self.pr2.joint_listener.last_msg.position[self.ros_joint_inds[0]]
    def get_velocity(self):
        return self.pr2.joint_listener.last_msg.velocity[self.ros_joint_inds[0]]
    def get_effort(self):
        return self.pr2.joint_listener.last_msg.effort[self.ros_joint_inds[0]]
    def get_joint_positions(self):
        return [self.get_angle()]

class Base(object):

    def __init__(self, pr2):
        self.pr2 = pr2
        self.action_client = actionlib.SimpleActionClient('move_base',mbm.MoveBaseAction)
        self.command_pub = rospy.Publisher('base_controller/command', gm.Twist)        
        self.traj_pub = rospy.Publisher("base_traj_controller/command", tm.JointTrajectory)
        self.vel_limits = [.2,.2,.3]
        self.acc_limits = [2,2,2] # note: I didn't think these thru
        self.n_joints = 3

    def goto_pose(self, xya, frame_id):
        trans,rot = conv.xya_to_trans_rot(xya)

# 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

            jtp = tm.JointTrajectoryPoint()
            jtp.time_from_start = rospy.Duration(t)
            jtp.positions = [a]
            jt.points.append(jtp)
        self.diag_pub.publish(jt)

        self.pr2.start_thread(GripperTrajectoryThread(self, times_up, angs_up))
        #self.pr2.start_thread(GripperTrajectoryThread(self, times, angs))
    def get_angle(self):
        return self.pr2.joint_listener.last_msg.position[self.ros_joint_inds[0]]
    def get_velocity(self):
        return self.pr2.joint_listener.last_msg.velocity[self.ros_joint_inds[0]]
    def get_effort(self):
        return self.pr2.joint_listener.last_msg.effort[self.ros_joint_inds[0]]
    def get_joint_positions(self):
        return [self.get_angle()]

class Base(object):

    def __init__(self, pr2):
        self.pr2 = pr2
        self.action_client = actionlib.SimpleActionClient('move_base',mbm.MoveBaseAction)
        self.command_pub = rospy.Publisher('base_controller/command', gm.Twist)        
        self.traj_pub = rospy.Publisher("base_traj_controller/command", tm.JointTrajectory)
        self.vel_limits = [.2,.2,.3]
        self.acc_limits = [2,2,2] # note: I didn't think these thru
        self.n_joints = 3

    def goto_pose(self, xya, frame_id):
        trans,rot = conv.xya_to_trans_rot(xya)

        pose = conv.trans_rot_to_pose(trans, rot)