def insert6DofGlobalMarker(self, T_W_M):
        int_position_marker = InteractiveMarker()
        int_position_marker.header.frame_id = 'world'
        int_position_marker.name = 'obj_pose_marker'
        int_position_marker.scale = 0.1
        int_position_marker.pose = pm.toMsg(T_W_M)
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.ROTATE_AXIS,'x'));
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.ROTATE_AXIS,'y'));
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.ROTATE_AXIS,'z'));
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.MOVE_AXIS,'x'));
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.MOVE_AXIS,'y'));
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.MOVE_AXIS,'z'));
        self.mk_server.insert(int_position_marker, self.processFeedback)

        int_button_marker = InteractiveMarker()
        int_button_marker.header.frame_id = 'world'
        int_button_marker.name = 'obj_button_marker'
        int_button_marker.scale = 0.2
        int_button_marker.pose = pm.toMsg(PyKDL.Frame())
        box = self.createButtonMarkerControl(Point(0.05,0.05,0.05), Point(0.0, 0.0, 0.15) )
        box.interaction_mode = InteractiveMarkerControl.BUTTON
        box.name = 'button'
        int_button_marker.controls.append( box )
        self.mk_server.insert(int_button_marker, self.processFeedback)

        self.mk_server.applyChanges()

def irp6p_multi_trajectory2():
	irpos = IRPOS("IRpOS", "Irp6p", 6)

	irpos.move_to_motor_position([0.4, -1.5418065817051163, 0.0, 1.57, 1.57, -2.0], 10.0)
	irpos.move_to_motor_position([10.0, 10.0, 0.0, 10.57, 10.57, -20.0], 2.0)

	irpos.move_to_joint_position([0.4, -1.5418065817051163, 0.0, 1.5, 1.57, -2.0], 3.0)
	irpos.move_to_joint_position([0.0, -1.5418065817051163, 0.0, 1.5, 1.57, -2.0], 3.0)

	rot = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.0, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.18029263241))
	rot2 = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.3, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.181029263241))
	irpos.move_to_cartesian_pose(3.0, Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.675351045979, 0.0892025112399, 0.698321120995, 0.219753244928)))
	irpos.move_to_cartesian_pose(3.0,pm.toMsg(rot))
	irpos.move_to_cartesian_pose(3.0,pm.toMsg(rot2))

	toolParams = Pose(Point(0.0, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))
	irpos.set_tool_geometry_params(toolParams)

	rot = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.0, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.181029263241))
	irpos.move_to_cartesian_pose(3.0,Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.675351045979, 0.0892025112399, 0.698321120995, 0.219753244928)))
	irpos.move_to_cartesian_pose(3.0,pm.toMsg(rot))
	irpos.move_to_cartesian_pose(3.0,Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.63691, 0.096783, 0.75634, -0.11369)))

	toolParams = Pose(Point(0.0, 0.0, 0.25), Quaternion(0.0, 0.0, 0.0, 1.0))
	irpos.set_tool_geometry_params(toolParams)

	print "Irp6p 'multi_trajectory2' test compleated"

    def compute_net_transform(self, base_pose):

        base_to_odom = self.tfl.lookupTransform("/base_footprint", "/odom_combined", rospy.Time())

        bto = pm.fromTf(base_to_odom)
        

        net_t = base_pose * bto

        print "Setting "
        print pm.toMsg(net_t)
        self.broad.transform = pm.toTf(net_t)

def oplus(cur_estimate, step):
    result = deepcopy(cur_estimate)

    # loop over camera's
    for camera, res, camera_index in zip(cur_estimate.cameras, result.cameras, [r*pose_width for r in range(len(cur_estimate.cameras))]):
        res.pose = posemath.toMsg(pose_oplus(posemath.fromMsg(camera.pose), step[camera_index:camera_index+pose_width]))
    # loop over targets
    for i, target in enumerate(cur_estimate.targets): 
        target_index = (len(cur_estimate.cameras) + i) * pose_width
        result.targets[i] = posemath.toMsg(pose_oplus(posemath.fromMsg(target), step[target_index:target_index+pose_width]))

    return result

def omni_callback(joint_state):
    global update_pub, last_button_state
    sendTf(marker_tf, '/marker', fixed_frame)
    sendTf(zero_tf, '/base', fixed_frame)
    sendTf(marker_ref, '/marker_ref', fixed_frame)
    sendTf(stylus_ref, '/stylus_ref', fixed_frame)
        
    try:
        update = InteractionCursorUpdate()
        update.pose.header = std_msgs.msg.Header()
        update.pose.header.stamp = rospy.Time.now()
        update.pose.header.frame_id = 'marker_ref'
        if button_clicked and last_button_state == update.GRAB:
            update.button_state = update.KEEP_ALIVE
        elif button_clicked and last_button_state == update.KEEP_ALIVE:
            update.button_state = update.KEEP_ALIVE
        elif button_clicked:
            update.button_state = update.GRAB
        elif last_button_state == update.KEEP_ALIVE:
            update.button_state = update.RELEASE
        else:
            update.button_state = update.NONE
            updateRefs()
        update.key_event = 0

        control_tf = ((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, control_offset))
        if button_clicked:
            # Get pose corresponding to transform between stylus reference and current position.
            stylus_tf = listener.lookupTransform('/stylus_ref', '/stylus', rospy.Time(0))
            stylus_matrix = pm.toMatrix(pm.fromTf(stylus_tf))
            control_matrix = pm.toMatrix(pm.fromTf(control_tf))
            p = pm.toMsg(pm.fromMatrix(numpy.dot(control_matrix, stylus_matrix)))
        else:
            p = pm.toMsg(pm.fromTf(control_tf))
        
        # Simply scale this up a bit to increase the workspace.
        workspace = 4
        p.position.x = p.position.x * workspace
        p.position.y = p.position.y * workspace
        p.position.z = p.position.z * workspace

        update.pose.pose = p

        last_button_state = update.button_state

        # Publish feedback.
        update_pub.publish(update)
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        rospy.logerr("Couldn't look up transform. These things happen...")

def get_ik_simple(service, goal, seed, joint_names, tip_frame, tool=None):
    validate_quat(goal.pose.orientation)
    
    # Transforms the goal due to the tip frame
    if tool:
        tool_in_tip = tfl.transformPose(tip_frame, tool)
        tool_in_tip_t = pm.fromMsg(tool_in_tip.pose)
        goal_t = pm.fromMsg(goal.pose)
        #goal.pose = pm.toMsg(tool_in_tip_t.Inverse() * goal_t)
        #goal = PoseStamped(header = goal.header, pose = pm.toMsg(tool_in_tip_t.Inverse() * goal_t))
        goal = PoseStamped(header = goal.header, pose = pm.toMsg(goal_t * tool_in_tip_t.Inverse()))

    req = PositionIKRequest()
    req.ik_link_name = tip_frame
    req.pose_stamped = goal
    req.ik_seed_state.joint_state.name = joint_names
    req.ik_seed_state.joint_state.position = seed

    error_code = 0
    for i in range(10):
        resp = service(req, rospy.Duration(10.0))
        if resp.error_code.val != 1:
            rospy.logerr("Error in IK: %d" % resp.error_code.val)
        else:
            return list(resp.solution.joint_state.position)
    return []

def transform_wrist_frame(T_tool, ft, tool_x_offset=0.0):
    '''
    :param T_tool: desired  *_gripper_tool_frame pose w.r.t. some reference frame. Can be of type kdl.Frame or geometry_msgs.Pose.
    :param ft: bool. True if the arm has a force-torque sensor
    :param tool_x_offset: (double) offset in tool length
    :return: T_torso_wrist. geometry_msgs.Pose type. Wrist pose w.r.t. same ref frame as T_tool
    '''

    if ft:
        T_wrist_tool = kdl.Frame(kdl.Rotation.Identity(), kdl.Vector(0.216 + tool_x_offset, 0.0, 0.0))

    else:
        T_wrist_tool = kdl.Frame(kdl.Rotation.Identity(), kdl.Vector(0.180 + tool_x_offset, 0.0, 0.0))

    if type(T_tool) is Pose:

        T_tool_ = posemath.fromMsg(T_tool)

    elif type(T_tool) is tuple and len(T_tool) is 2:
        T_tool_ = posemath.fromTf(T_tool)

    else:
        T_tool_ = T_tool


    T_wrist_ = T_tool_*T_wrist_tool.Inverse()

    T_wrist = posemath.toMsg(T_wrist_)

    return T_wrist

 def get_new_waypoint(self,obj):
     try:
         (trans,rot) = self.listener.lookupTransform(obj,self.endpoint,rospy.Time(0))
         return pm.toMsg(pm.fromTf((trans,rot)))
     except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
         rospy.logerr('Failed to lookup transform from %s to %s'%(obj,self.endpoint))
     return None

	def project_pose(self, pose):
		frame = pm.fromMsg(pose)
		[roll, pitch, yaw] = frame.M.GetRPY()
		frame.M = PyKDL.Rotation.RPY(0, 0, yaw)
		projected = pm.toMsg(frame)
		projected.position.z = 0
		return projected

    def get_new_waypoint(self,obj):
        try:
            # TODO: make the snap configurable
            #(trans,rot) = self.listener.lookupTransform(obj,self.endpoint,rospy.Time(0))
            (eg_trans,eg_rot) = self.listener.lookupTransform(self.gripper_center,self.endpoint,rospy.Time(0))
            (og_trans,og_rot) = self.listener.lookupTransform(obj,self.gripper_center,rospy.Time(0))

            rospy.logwarn("gripper obj:" + str((og_trans, og_rot)))
            rospy.logwarn("endpoint gripper:" + str((eg_trans, eg_rot)))

            xyz, rpy = [], []
            for dim in og_trans:
                # TODO: test to see if we should re-enable this
                if abs(dim) < 0.0001:
                    xyz.append(0.)
                else:
                    xyz.append(dim)
            Rog = kdl.Rotation.Quaternion(*og_rot)
            for dim in Rog.GetRPY():
                rpy.append(np.round(dim / np.pi * 8.) * np.pi/8.)
            Rog_corrected = kdl.Rotation.RPY(*rpy)
            Vog_corrected = kdl.Vector(*xyz)
            Tog_corrected = kdl.Frame(Rog_corrected, Vog_corrected)

            rospy.logwarn(str(Tog_corrected) + ", " + str(Rog_corrected.GetRPY()))

            Teg = pm.fromTf((eg_trans, eg_rot))
            return pm.toMsg(Tog_corrected * Teg)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logerr('Failed to lookup transform from %s to %s'%(obj,self.endpoint))
        return None

def irp6p_multi_trajectory():
	irpos = IRPOS("IRpOS", "Irp6p", 6)

	motor_trajectory = [JointTrajectoryPoint([0.4, -1.5418065817051163, 0.0, 1.57, 1.57, -2.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(10.0)), JointTrajectoryPoint([10.0, 10.0, 0.0, 10.57, 10.57, -20.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(12.0))]
        irpos.move_along_motor_trajectory(motor_trajectory)

	joint_trajectory = [JointTrajectoryPoint([0.4, -1.5418065817051163, 0.0, 1.5, 1.57, -2.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(3.0)),JointTrajectoryPoint([0.0, -1.5418065817051163, 0.0, 1.5, 1.57, -2.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(6.0))]
	irpos.move_along_joint_trajectory(joint_trajectory)

	rot = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.0, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.18029263241))
	rot2 = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.3, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.181029263241))
	cartesianTrajectory = [CartesianTrajectoryPoint(rospy.Duration(3.0), Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.675351045979, 0.0892025112399, 0.698321120995, 0.219753244928)), Twist()), CartesianTrajectoryPoint(rospy.Duration(6.0), pm.toMsg(rot), Twist()),CartesianTrajectoryPoint(rospy.Duration(9.0), pm.toMsg(rot2), Twist())]
	irpos.move_along_cartesian_trajectory(cartesianTrajectory)

	toolParams = Pose(Point(0.0, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))
	irpos.set_tool_geometry_params(toolParams)

	rot = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.0, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.181029263241))
	cartesianTrajectory = [CartesianTrajectoryPoint(rospy.Duration(3.0), Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.675351045979, 0.0892025112399, 0.698321120995, 0.219753244928)), Twist()),
CartesianTrajectoryPoint(rospy.Duration(6.0), pm.toMsg(rot), Twist()),CartesianTrajectoryPoint(rospy.Duration(9.0), Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.63691, 0.096783, 0.75634, -0.11369)), Twist())]
	irpos.move_along_cartesian_trajectory(cartesianTrajectory)

	toolParams = Pose(Point(0.0, 0.0, 0.25), Quaternion(0.0, 0.0, 0.0, 1.0))
	irpos.set_tool_geometry_params(toolParams)

	print "Irp6p 'multi_trajectory' test compleated"

    def moveEffector(self, prefix, T_B_Td, t, max_wrench, start_time=0.01, stamp=None, path_tol=None):
        behaviour = self.getControllerBehaviour()
        if behaviour != self.BEHAVOIUR_CART_IMP and behaviour != self.BEHAVOIUR_CART_IMP_FT:
            print "moveEffector " + prefix + ": wrong behaviour " + self.getBehaviourName(behaviour)
            return False

        self.joint_traj_active = False
        wrist_pose = pm.toMsg(T_B_Td)
#        self.br.sendTransform([wrist_pose.position.x, wrist_pose.position.y, wrist_pose.position.z], [wrist_pose.orientation.x, wrist_pose.orientation.y, wrist_pose.orientation.z, wrist_pose.orientation.w], rospy.Time.now(), "dest", "torso_base")

        action_trajectory_goal = CartesianTrajectoryGoal()
        if stamp != None:
            action_trajectory_goal.trajectory.header.stamp = stamp
        else:
            action_trajectory_goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(start_time)
        action_trajectory_goal.trajectory.points.append(CartesianTrajectoryPoint(
        rospy.Duration(t),
        wrist_pose,
        Twist()))
        action_trajectory_goal.wrench_constraint = self.wrenchKDLtoROS(max_wrench)
        if path_tol != None:
            action_trajectory_goal.path_tolerance.position = geometry_msgs.msg.Vector3( path_tol.vel.x(), path_tol.vel.y(), path_tol.vel.z() )
            action_trajectory_goal.path_tolerance.rotation = geometry_msgs.msg.Vector3( path_tol.rot.x(), path_tol.rot.y(), path_tol.rot.z() )
        self.current_max_wrench = self.wrenchKDLtoROS(max_wrench)
        self.action_cart_traj_client[prefix].send_goal(action_trajectory_goal, feedback_cb = self.action_right_cart_traj_feedback_cb)
        return True

def execute_cb(goal):
  rospy.loginfo("Action server received goal")
  preempt_timeout = rospy.Duration(5.0)

  for i in range(1,5):
    rospy.loginfo('Detecting plug in gripper from position %i'%i)

    # move to joint space position
    rospy.loginfo("Move in joint space...")
    if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/detect_plug_in_gripper%i'%i), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
      rospy.logerr('Move retract in joint space failed')
      server.set_aborted()
      return

    # call vision plug detection
    rospy.loginfo("Detecting plug...")
    detect_plug_goal = VisionPlugDetectionGoal()
    detect_plug_goal.camera_name = "/r_forearm_cam"
    #detect_plug_goal.prior.pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(pi/2, 0.0, -pi/9), PyKDL.Vector(-.03, 0, 0)).Inverse())
    detect_plug_goal.prior.pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(-pi/2, pi/9, 0.0), PyKDL.Vector(0.03, 0, -0.01)))
    detect_plug_goal.prior.header.stamp = rospy.Time.now()
    detect_plug_goal.prior.header.frame_id = "r_gripper_tool_frame"
    detect_plug_goal.origin_on_right = True
    detect_plug_goal.prior.header.stamp = rospy.Time.now()
    if detect_plug_client.send_goal_and_wait(detect_plug_goal, rospy.Duration(5.0), preempt_timeout) == GoalStatus.SUCCEEDED:
      server.set_succeeded(DetectPlugInGripperResult(detect_plug_client.get_result().plug_pose))      
      rospy.loginfo("Action server goal finished")  
      return

  # Failure
  rospy.logerr("Failed to detect plug in gripper")      
  server.set_aborted(DetectPlugInGripperResult(detect_plug_goal.prior))      

    def get_waypoints(self,frame_type,predicates,transforms,names):
        self.and_srv.wait_for_service()

        type_predicate = PredicateStatement()
        type_predicate.predicate = frame_type
        type_predicate.params = ['*','','']

        res = self.and_srv([type_predicate]+predicates)

        print "Found matches: " + str(res.matching)
        #print res

        if (not res.found) or len(res.matching) < 1:
          return None

        poses = []
        for tform in transforms:
            poses.append(pm.fromMsg(tform))

        #print poses
        new_poses = []
        new_names = []

        for match in res.matching:
            try:
                (trans,rot) = self.listener.lookupTransform(self.world,match,rospy.Time(0))
                for (pose, name) in zip(poses,names):
                    #resp.waypoints.poses.append(pm.toMsg(pose * pm.fromTf((trans,rot))))
                    new_poses.append(pm.toMsg(pm.fromTf((trans,rot)) * pose))
                    new_names.append(match + "/" + name)
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                rospy.logwarn('Could not find transform from %s to %s!'%(self.world,match))
        
        return (new_poses, new_names)

    def __frame_to_tfxPose(self, frame):
        """ Function converts a PyKDL Frame object to a tfx object """
        """ We convert a PyKDL.Frame object to a ROS posemath object and then reconvert it to a tfx.canonical.CanonicalTransform object """

        """:returns: tfx pose """
        rosMsg = posemath.toMsg(frame)
        return tfx.pose(rosMsg) 

    def getPlanWaypoints(self,waypoints_in_kdl_frame,q,obj=None):
      cartesian_path_req = GetCartesianPathRequest()
      cartesian_path_req.header.frame_id = self.base_link
      cartesian_path_req.start_state = RobotState()
      cartesian_path_req.start_state.joint_state.name = self.joint_names
      if type(q) is list:
        cartesian_path_req.start_state.joint_state.position = q
      else:
        cartesian_path_req.start_state.joint_state.position = q.tolist()
      cartesian_path_req.group_name = self.group
      cartesian_path_req.link_name = self.joint_names[-1]
      cartesian_path_req.avoid_collisions = False
      cartesian_path_req.max_step = 50
      cartesian_path_req.jump_threshold = 0
      # cartesian_path_req.path_constraints = Constraints()

      if obj is not None:
        self.updateAllowedCollisions(obj,True)
      
      cartesian_path_req.waypoints = list()

      for T in waypoints_in_kdl_frame:
        cartesian_path_req.waypoints.append(pm.toMsg(T))

      res = self.cartesian_path_plan.call(cartesian_path_req)

      if obj is not None:
        self.updateAllowedCollisions(obj,False)

      return (res.error_code.val, res)

	def move_rel_to_cartesian_pose_with_contact(self, time_from_start, rel_pose, wrench_constraint):
		print self.BCOLOR+"[IRPOS] Move relative to cartesian trajectory with contact"+self.ENDC
				
		self.conmanSwitch([self.robot_name+'mPoseInt', self.robot_name+'mForceTransformation'], [], True)
		time.sleep(0.05)

		actual_pose = self.get_cartesian_pose()
		
		# Transform poses to frames.
		actualFrame = pm.fromMsg(actual_pose)
		
		relativeFrame = pm.fromMsg(rel_pose)
		
		desiredFrame = actualFrame * relativeFrame
		
		pose = pm.toMsg(desiredFrame)

		cartesianGoal = CartesianTrajectoryGoal()
		cartesianGoal.wrench_constraint = wrench_constraint
		cartesianGoal.trajectory.points.append(CartesianTrajectoryPoint(rospy.Duration(time_from_start), pose, Twist()))
		cartesianGoal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.1)

		self.pose_client.send_goal(cartesianGoal)
		self.pose_client.wait_for_result()

		result = self.pose_client.get_result()
		code = self.cartesian_error_code_to_string(result.error_code)
		print self.BCOLOR+"[IRPOS] Result: "+str(code)+self.ENDC

		self.conmanSwitch([], [self.robot_name+'mPoseInt', self.robot_name+'mForceTransformation'], True)

    def get_desired_cartesian_position(self):
        """Get the :ref:`desired cartesian position <currentvdesired>` of the robot in terms of caretsian space.

        :returns: the desired position of the robot in cartesian space
        :rtype: `PyKDL.Frame <http://docs.ros.org/diamondback/api/kdl/html/python/geometric_primitives.html>`_"""
        frame = self.__position_cartesian_desired
        tfx_pose = tfx.pose(posemath.toMsg(frame))
        return tfx_pose

 def forward_kinematics_cb(self, req):
     ''' Run forward kinematics on a joint space pose and return the result.
     '''
     q = req.joint_state.position
     kdl_pose = pm.fromMatrix(self.kdl_kin.forward(q))
     pose_msg = pm.toMsg(kdl_pose)
     response = ForwardKinematicsResponse(pose=pose_msg, ack="SUCCESS")
     return response

def moveTool(tool_frame, t):
    global action_tool_client
    tool_pose = pm.toMsg(tool_frame)
    action_tool_goal = CartesianTrajectoryGoal()
    action_tool_goal.trajectory.header.stamp = rospy.Time.now()
    action_tool_goal.trajectory.points.append(CartesianTrajectoryPoint(
    rospy.Duration(t),
    tool_pose,
    Twist()))
    action_tool_client.send_goal(action_tool_goal)