def main():
    import sys
    def usage():
        print("Tests for kdl_parser:\n")
        print("kdl_parser <urdf file>")
        print("\tLoad the URDF from file.")
        print("kdl_parser")
        print("\tLoad the URDF from the parameter server.")
        sys.exit(1)

    if len(sys.argv) > 2:
        usage()
    if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
        usage()
    if (len(sys.argv) == 1):
        robot = URDF.load_from_parameter_server(verbose=False)
    else:
        robot = URDF.load_xml_file(sys.argv[1], verbose=False)
    tree = kdl_tree_from_urdf_model(robot)
    num_non_fixed_joints = 0
    for j in robot.joints:
        if robot.joints[j].joint_type != 'fixed':
            num_non_fixed_joints += 1
    print "URDF non-fixed joints: %d;" % num_non_fixed_joints,
    print "KDL joints: %d" % tree.getNrOfJoints()
    print "URDF joints: %d; KDL segments: %d" %(len(robot.joints),
                                                tree.getNrofSegments())
    import random
    base_link = robot.get_root()
    end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)]
    chain = tree.getChain(base_link, end_link)
    print "Root link: %s; Random end link: %s" % (base_link, end_link)
    for i in range(chain.getNrOfSegments()):
        print chain.getSegment(i).getName()

def main():
    global save, psm1_robot, psm1_kin, psm2_robot, psm2_kin, ecm_robot, ecm_kin
    
    rospy.init_node('psm_optimization_data_collector')
    # Get the joint angles from the hardware and move the simulation from hardware
    rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, psm1_read_cb)
    rospy.Subscriber('/dvrk/PSM2/state_joint_current', JointState, psm2_read_cb)
    rospy.Subscriber('/dvrk/ECM/state_joint_current', JointState, ecm_read_cb)
    
    if psm1_robot is None:
        psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
        psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[-1].name)
    if psm2_robot is None:
        psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
        psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[-1].name)
    if ecm_robot is None:
        ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
        ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name)

    while True:
        print("save now? ")
        print("(y) yes\n(n) no\n(q) quit")
        r = raw_input(" : ")
        
        if r == "q":
            global file
            file.close()
            return
        if r == "y":
            psm1_read_cb.save = True
            psm2_read_cb.save = True
            ecm_read_cb.save = True
            
    rospy.spin()

def main():
    global psm1_kin,psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot
    
    objective_function.mode = MODE
    
    if psm1_robot is None:
        psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
        psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[1].name, psm1_robot.links[-1].name)
    if psm2_robot is None:
        psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
        psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[1].name, psm2_robot.links[-1].name)
    if ecm_robot is None:
        ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
        ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[3].name, ecm_robot.links[-1].name)
        
    initial_guess = [ (.80,0.5,.3), (0.2,0.7,1.57)]
    res = minimize(objective_function, initial_guess, method='nelder-mead', options={'xtol':1e-12, 'disp':True, 'maxiter': 100000, 'maxfev':100000},)
    print(res)
    print(res.x)
    file.close()
    print(objective_function.mode)   
    if objective_function.mode == 'psm2_psm1':
        print('psm2 relative to world: ')
        v = find_everything_related_to_world('psm2', res.x)
 #       print("""xyz="{} {} {}" rpy="{} {} {}" """.format(v[0], v[1]) )
        print("""xyz="{0} {1} {2}" rpy="{3} {4} {5}" """.format(v[0][0],v[0][1],v[0][2],v[1][0],v[1][1],v[1][2]))
    if objective_function.mode == 'ecm_psm1':
        print('ecm relative to world: ')
        v = find_everything_related_to_world('ecm', res.x)
        print("""xyz="{0} {1} {2}" rpy="{3} {4} {5}" """.format(v[0][0],v[0][1],v[0][2],v[1][0],v[1][1],v[1][2]))

def create_kdl_kin(base_link, end_link, urdf_filename=None):
    if urdf_filename is None:
        robot = URDF.from_parameter_server()
    else:
        f = open(urdf_filename, "r")
        robot = URDF.from_xml_string(f.read())
    return KDLKinematics(robot, base_link, end_link)

    def __init__(self ,urdf=None):
        if urdf is None:
            print 'FROM PARAM SERVER'
            self._youbot = URDF.from_parameter_server(key='robot_description')
        else:
            print 'FROM STRING'
            self._youbot = URDF.from_xml_string(urdf)

        self._kdl_tree = kdl_tree_from_urdf_model(self._youbot)
        self._base_link = 'arm_link_0'
        print "ROOT : ",self._base_link
        self._tip_link = 'arm_link_5'
        print "self._tip_link : ", self._tip_link
        self._tip_frame = PyKDL.Frame()
        self._arm_chain = self._kdl_tree.getChain(self._base_link,
                                                  self._tip_link)

        # Baxter Interface Limb Instances
        #self._limb_interface = youbot_interface.Limb(limb)
        #self._joint_names = self._limb_interface.joint_names()
        self._joint_names = ['arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4', 'arm_joint_5']
        self._num_jnts = len(self._joint_names)

        # KDL Solvers
        self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
        self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
        self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
        self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
                                                   self._fk_p_kdl,
                                                   self._ik_v_kdl)
        self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
        self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
                                            PyKDL.Vector.Zero())

 def __init__(self):
     self.t = time.time()
     
     self.__AUTOCAMERA_MODE__ = self.MODE.simulation
     
     self.autocamera = Autocamera() # Instantiate the Autocamera Class
     
     self.jnt_msg = JointState()
     self.joint_angles = {'ecm':None, 'psm1':None, 'psm2':None}
     self.cam_info = {'left':CameraInfo(), 'right':CameraInfo()}
     
     self.last_ecm_jnt_pos = None
     
     self.first_run = True
     
     # For forward and inverse kinematics
     self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
     self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
     self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
     self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
     self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description')
     self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name)
     self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
     self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name)
     
     
     # For camera clutch control    
     self.camera_clutch_pressed = False        
     self.ecm_manual_control_lock_mtml_msg = None
     self.ecm_manual_control_lock_ecm_msg = None
     self.mtml_start_position = None
     self.mtml_end_position = None
     
     self.initialize_psms_initialized = 30
     self.__DEBUG_GRAPHICS__ = False

 def __init__(self):
     self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
     self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
     self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
     self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
     self.psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
     self.psm2_kin = KDLKinematics(self.psm2_robot, self.psm2_robot.links[0].name, self.psm2_robot.links[-1].name)
     
     self.zoom_level_positions = {'l1':None, 'r1':None, 'l2':None, 'r2':None, 'lm':None, 'rm':None}
     self.logerror("autocamera_initialized")

def runDiff(original_file, new_file, output_file):
    original_robot = URDF.from_xml_file(original_file)
    new_robot = URDF.from_xml_file(new_file)
    # only joint and link are considered
    diffs = dict()
    for j in original_robot.joints:
        new_j = findJointByName(new_robot, j.name)
        # check origin difference
        if new_j:
            diff = jointDiff(j, new_j)
            if diff:
                diffs[j.name] = diff
    with open(output_file, "w") as f:
        f.write(yaml.dump(diffs))
        print yaml.dump(diffs)

    def __init__(self):
        rospy.init_node('hybrid_node')
        self.freq = 100
        self.rate = rospy.Rate(self.freq)  # 100 hz

        # pub
        self.pub_test = rospy.Publisher('/hybrid/test', String)

        # sub
        self.sub_jr3 = rospy.Subscriber('/jr3/wrench', WrenchStamped, self.cb_jr3)
        self.sub_joy = rospy.Subscriber('/spacenav/joy', Joy, self.cb_joy)
        self.sub_enable = rospy.Subscriber('/hybrid/enable', Bool, self.cb_enable)
        self.sub_cmd = rospy.Subscriber('/hybrid/cmd', String, self.cb_cmd)

        # tf 
        self.ler = tf.TransformListener()  # listener
        self.ber = tf.TransformBroadcaster() # broadcaster

        # datas
        self.enabled = False
        self.cmdfrm = Frame()
        self.wrench = Wrench()

        self.cmdtwist = Twist()
        self.urdf = rospy.get_param('/wam/robot_description')
        print self.urdf
        
        self.robot = URDF()
        self.robot = self.robot.from_xml_string(self.urdf)
        self.chain = self.robot.get_chain('base_link',
                                          'cutter_tip_link')

        print self.chain
        pass

    def __init__(self, urdf_param = 'robot_description'):
        self._urdf = URDF.from_parameter_server(urdf_param)
        (parse_ok, self._kdl_tree) = treeFromUrdfModel(self._urdf)
        # Check @TODO Exception
        if not parse_ok:
            rospy.logerr('Error parsing URDF from parameter server ({0})'.format(urdf_param))
        else:
            rospy.logdebug('Parsing URDF succesful')

        self._base_link = self._urdf.get_root()
        # @TODO Hardcoded
        self._tip_link = 'link_6'
        self._tip_frame = PyKDL.Frame()
        self._arm_chain = self._kdl_tree.getChain(self._base_link,
                                                  self._tip_link)
        # @TODO Hardcoded
        self._joint_names = ['a1', 'a2', 'a3', 'a4', 'a5', 'a6']
        self._num_joints = len(self._joint_names)

        # KDL Solvers
        self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
        self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
        self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
        self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
                                                   self._fk_p_kdl,
                                                   self._ik_v_kdl)
        self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
        self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
                                            PyKDL.Vector.Zero())

    def __init__(self):
        #Loads the robot model, which contains the robot's kinematics information
        self.robot = URDF.from_parameter_server()

        # Publishes Cartesian goals
        self.pub_command = rospy.Publisher("/cartesian_command", geometry_msgs.msg.Transform, 
                                           queue_size=1)

        # Publishes Redundancy goals
        self.pub_red = rospy.Publisher("/redundancy_command", Float32, queue_size=1)

        # Publisher to set robot position
        self.pub_reset = rospy.Publisher("/joint_command", JointState, queue_size=1)

        #This is where we hold the most recent joint transforms
        self.joint_transforms = []
        self.x_current = tf.transformations.identity_matrix()
        self.R_base = tf.transformations.identity_matrix()

        #Create "Interactive Marker" that we can manipulate in RViz
        self.init_marker()
        self.ee_tracking = 0
        self.red_tracking = 0
        self.q_current = []

        self.x_target = tf.transformations.identity_matrix()
        self.q0_desired = 0

        self.mutex = Lock()        
        self.timer = rospy.Timer(rospy.Duration(0.3), self.timer_callback)

        #Subscribes to information about what the current joint values are.
        rospy.Subscriber("joint_states", JointState, self.joint_callback)

	def __init__(self, side):

		# Redirect annoying output of upcoming URDF command
		devnull = open(os.devnull, 'w')
		sys.stdout, sys.stderr = devnull, devnull
		
		self.robot = URDF.from_parameter_server()
		
		# Now point output back
		sys.stdout = sys.__stdout__
		sys.stderr = sys.__stderr__
		devnull.close()

		self.joint_list = {}
		for ndx, jnt in enumerate(self.robot.joints):
			self.joint_list[jnt.name] = ndx

		self.chain = list()

		# Query parameter server for joints
		arm_chain = '/' + side + '_arm_chain'
		joint_names = rospy.get_param(arm_chain)

		for joint in joint_names:
			self.chain.append(JointData(self, joint))

    def __init__(self, name):

        self.elevated_distance = 0.3  # .07

        # Find the baxter from the parameter server
        self.baxter = URDF.from_parameter_server()
        # Note:  A node that initializes and runs the baxter has to be running in the background for
        #       from_parameter_server to to find the parameter.
        #       Older versions of URDF label the function "load_from_parameter_server"

        # Subscribe to the "baxter1_joint_states" topic, which provides the joint states measured by the baxter in a
        # ROS message of the type sensor_msgs/JointState. Every time a message is published to that topic run the
        # callback function self.get_joint_states, which is defined below.
        self.joint_state_sub = rospy.Subscriber("/robot/joint_states", JointState, self.get_joint_states)
        self.end_eff_state_sub = rospy.Subscriber(
            "/robot/limb/left/endpoint_state", EndpointState, self.get_end_eff_state
        )

        self.listener = tf.TransformListener()
        # self.timer1 = rospy.Timer(rospy.Duration(0.01),)

        # calibrate the gripper
        self.initialize_gripper()

        self._action_name = name
        # The main function of BaxterWeigh is called whenever a client sends a goal to weigh_server
        self._as = actionlib.SimpleActionServer(
            self._action_name, baxter_pour.msg.WeighAction, execute_cb=self.main, auto_start=False
        )
        self._as.start()

        # To test the node separatly call self.main() manually
        # self.main()
        return

    def __init__(self, joint_names_vector, inactive_joint_names, js_pos):
        self.robot = URDF.from_parameter_server()
        self.tree, self.segment_map, self.segment_parent_map, self.segment_name_id_map = self.kdl_tree_from_urdf_model(self.robot, inactive_joint_names, js_pos)
        self.segment_id_name_map = {}
        for seg_name in self.segment_name_id_map:
            seg_id = self.segment_name_id_map[seg_name]
            self.segment_id_name_map[seg_id] = seg_name

        self.fk_solvers = {}

        self.createSegmentToJointMap(joint_names_vector, inactive_joint_names)

        joint_limit_map = {}
        for j in self.robot.joints:
            if j.limit != None:
                joint_limit_map[j.name] = j.limit

        self.lim_lower = np.empty(len(joint_names_vector))
        self.lim_lower_soft = np.empty(len(joint_names_vector))
        self.lim_upper = np.empty(len(joint_names_vector))
        self.lim_upper_soft = np.empty(len(joint_names_vector))
        q_idx = 0
        for joint_name in joint_names_vector:
            self.lim_lower[q_idx] = joint_limit_map[joint_name].lower
            self.lim_lower_soft[q_idx] = self.lim_lower[q_idx] + 15.0/180.0*math.pi
            self.lim_upper[q_idx] = joint_limit_map[joint_name].upper
            self.lim_upper_soft[q_idx] = self.lim_upper[q_idx] - 15.0/180.0*math.pi
            q_idx += 1

    def __init__(self, urdf_path, sem_path, name_suffix=None):
        self.nsmap = {
            "xsd": "http://www.w3.org/2001/XMLSchema#",
            "owl": "http://www.w3.org/2002/07/owl#",
            "xsd": "http://www.w3.org/2001/XMLSchema#",
            "srdl2": "http://knowrob.org/kb/srdl2.owl#",
            "owl2xml": "http://www.w3.org/2006/12/owl2-xml#",
            "knowrob": "http://knowrob.org/kb/knowrob.owl#",
            "rdfs": "http://www.w3.org/2000/01/rdf-schema#",
            "rdf": "http://www.w3.org/1999/02/22-rdf-syntax-ns#",
            "srdl2-comp": "http://knowrob.org/kb/srdl2-comp.owl#",
            "srdl2-cap": "http://knowrob.org/kb/srdl2-cap.owl#",
            "qudt-unit": "http://qudt.org/vocab/unit#",
        }
        self.imports = [
            "package://knowrob_srdl/owl/srdl2-comp.owl",
            "package://knowrob_common/owl/knowrob.owl",
        ]
        self.id_gen = UniqueStringGenerator()
        self.urdf = URDF.from_xml_file(urdf_path)
        self.urdf.check_valid()

        basename = os.path.basename(sem_path)
        namespace, _ = os.path.splitext(basename)
        self.map_ns = namespace
        if name_suffix is None:
            self.map_name = self.urdf.name + "_" + self.id_gen.gen()
        else:
            self.map_name = self.urdf.name + "_" + name_suffix
        self.map_uri_base = "http://knowrob.org/kb/%s" % basename
        self.map_uri = self.map_uri_base + "#"
        self.nsmap[self.map_ns] = self.map_uri
        self.transformations = {}

    def __init__(self):
        #Loads the robot model, which contains the robot's kinematics information
        self.robot = URDF.from_parameter_server()

        #Subscribes to information about what the current joint values are.
        rospy.Subscriber("/joint_states", JointState, self.joint_callback)

        #Subscribes to command for end-effector pose
        rospy.Subscriber("/cartesian_command", Transform, self.command_callback)

        #Subscribes to command for redundant dof
        rospy.Subscriber("/redundancy_command", Float32, self.redundancy_callback)

        # Publishes desired joint velocities
        self.pub_vel = rospy.Publisher("/joint_velocities", JointState, queue_size=1)

        #This is where we hold the most recent joint transforms
        self.joint_transforms = []
        self.q_current = []
        self.x_current = tf.transformations.identity_matrix()
        self.R_base = tf.transformations.identity_matrix()
        self.x_target = tf.transformations.identity_matrix()
        self.q0_desired = 0
        self.last_command_time = 0
        self.last_red_command_time = 0

        # Initialize timer that will trigger callbacks
        self.mutex = Lock()
        self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback)

	def __init__(self):
		
		#Create a tf listener
		self.tfListener = tf.TransformListener() 
		#tf.TransfromListener is a subclass that subscribes to the "/tf" message topic and adds transform 
		#data to the tf data structure. As a result the object is up to date with all current transforms

		#Create a publisher to a new topic name called "stylus_to_base_transf with a message type Twist"
		self.Tsb = rospy.Publisher('stylus_to_base_transf',Twist) 

		#Find the omni from the parameter server
		self.omni = URDF.from_parameter_server() 
		#Note: 	A node that initializes and runs the Phantom Omni has to be running in the background for 
		#		from_parameter_server to to find the parameter.
		#		Older versions of URDF label the function "load_from_parameter_server"


		#Subscribe to the "omni1_joint_states" topic, which provides the joint states measured by the omni in a
		# ROS message of the type sensor_msgs/JointState. Every time a message is published to that topic run the 
		#callback function self.get_joint_states, which is defined below.
		self.joint_state_sub = rospy.Subscriber("omni1_joint_states", JointState, self.get_joint_states)

		#OmniMiniProjTimers
		#Run a timer to manipulate the class structure as needed. The timer's 
		#callback function "timer_callback" then calls the desired functions
		self.timer1 = rospy.Timer(rospy.Duration(0.01), self.timer_callback) 
		
		#Create a separate slower timer on a lower frequency for a comfortable print out
		self.print_timer = rospy.Timer(rospy.Duration(1), self.print_output) 

		return

 def __init__(self):
   super(RobotHeadWidget, self).__init__()
   self.lock = Lock()
   try:
     robot_model = URDF.from_xml_string(rospy.get_param("/robot_description"))
   except Exception, e:
     self.showError("Failed to load /robot_description: " + e.message)
     return

    def __init__(self, sim=True):

        f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r')
        robot = URDF.from_xml_string(f.read())  # parsed URDF

        # robot = URDF.from_parameter_server()
        self.kin = KDLKinematics(robot, 'base', 'mconn')

        # Selection of end-effector parameters for WidowX
        # x, y, z, yaw, pitch
        self.cart_from_matrix = lambda T: np.array([
            T[0, 3],
            T[1, 3],
            T[2, 3],
            math.atan2(T[1, 0], T[0, 0]),
            -math.asin(T[2, 0])])

        self.home = np.array([0.05, 0, 0.25, 0, 0])     # home end-effector pose

        self.q = np.array([0, 0, 0, 0, 0])              # joint angles
        self.dt = 0.05                                  # algorithm time step
        self.sim = sim                                  # true if virtual robot

        def publish(eventStop):

            # for arbotix
            pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
            pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
            pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
            pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
            pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)

            # for visualization
            jointPub = rospy.Publisher(
                "/joint_states", JointState, queue_size=5)
            jmsg = JointState()
            jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']

            while not rospy.is_shutdown() and not eventStop.is_set():

                jmsg.header.stamp = rospy.Time.now()
                jmsg.position = self.q
                if self.sim:
                    jointPub.publish(jmsg)

                pub1.publish(self.q[0])
                pub2.publish(self.q[1])
                pub3.publish(self.q[2])
                pub4.publish(self.q[3])
                pub5.publish(self.q[4])

                eventStop.wait(self.dt)

        rospy.init_node("robot")
        eventStop = threading.Event()
        threadJPub = threading.Thread(target=publish, args=(eventStop,))
        threadJPub.daemon = True
        threadJPub.start()  # Update joint angles in a background process

    def execute_move(self, pos):
        rospy.loginfo('moving')
        # Read in pose data. Adjust the height to be above the block and the length so that the laser sees the table instead of the block
        pos.position.z += .1
        pos.position.x += .005
        q = [pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z]
        p =[[pos.position.x],[pos.position.y],[pos.position.z]]
        # Convert quaternion data to rotation matrix
        R = quat_to_so3(q);
        # Form transformation matrix
        robot = URDF.from_parameter_server()
        base_link = robot.get_root()
        kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
        # Create seed with current position
        q0 = kdl_kin.random_joint_angles()
        limb_interface = baxter_interface.limb.Limb('right')
        limb_interface.set_joint_position_speed(.3)
        current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
        for ind in range(len(q0)):
            q0[ind] = current_angles[ind]
        pose = kdl_kin.forward(q0)
        pose[0:3,0:3] = R
        pose[0:3,3] = p
        
        # Solve for joint angles, iterating if no solution is found
        seed = 0.3
        q_ik = kdl_kin.inverse(pose, q0+seed)
        while q_ik == None:
            seed += 0.3
            q_ik = kdl_kin.inverse(pose, q0+seed)
        rospy.loginfo(q_ik)
        
        # Calculate the joint trajectory with a quintic time scaling
        q_list = JointTrajectory(q0,q_ik,1,50,5)
        
        # Iterate through list
        for q in q_list:
            # Format joint angles as limb joint angle assignment      
            angles = limb_interface.joint_angles()
            for ind, joint in enumerate(limb_interface.joint_names()):
                angles[joint] = q[ind]
            rospy.sleep(.07)
            
            # Send joint move command
            limb_interface.set_joint_positions(angles)
        
        # Publish state and hand position

        rospy.sleep(1)
        rospy.loginfo(4) 
        self._pub_state.publish(4)                    
        rospy.loginfo(pos)
        self._pub_hand.publish(pos)  

        self._done = True
        print('Done')