## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= [[ee_cart_imped/Tutorials/Writing a Stiffness Controller (Python)|Writing a Stiffness Controller (Python)]]
## note.1= [[ee_cart_imped/Tutorials/Running the Controller|Running the Controller]]
## descriptive title for the tutorial
## title = Switching Controllers at Run Time (Python)
## multi-line description to be displayed in search 
## description = Shows how to switch between the force/impedance controller and the arm_navigation suite of controllers at run time.  This tutorial is only available in python.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= IntermediateCategory
## keywords = force, compliance, impedance, stiffness, controller, pr2 arms, multiple controllers, switching controllers
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

In this tutorial we will show how to switch between the force/impedance controller and the usual set of arm navigation controllers at run time.  This allows you to position the arm using the collision-free arm movement available in the [[arm_navigation]] stack and then use the force control once the arm is properly positioned.  This builds on the [[http://www.ros.org/doc/api/ee_cart_imped_control/html/python/ee_cart_imped_control.control_switcher.PR2CMClient-class.html|control_switcher.PR2CMClient]] available in the [[ee_cart_imped_control]] package, which itself is a wrapper around the
[[http://www.ros.org/doc/electric/api/pr2_controller_manager/html/pr2__controller__manager__interface_8py.html|controller interface available in the pr2_controller_manager package]].

Before switching control to the arm_navigation stack, we first must add those dependencies to our package.  Change directories to the package you created when [[ee_cart_imped/Tutorials/Writing a Stiffness Controller (Python)|writing the stiffness controller]]:
{{{
roscd ee_cart_imped_tutorial
}}}
and open up the manifest.xml file that was created automatically when you made the package.  To this file add the following dependencies:
{{{
<depend package="ee_cart_imped_control"/>
<depend package="arm_navigation_msgs"/>
<depend package="actionlib"/>
<depend package="geometry_msgs"/>
}}}

=== The Code ===

Within the ee_cart_imped_tutorial package, create a file scripts/switch_control.py and paste the following into it:
{{{
#!python block=switch_control
#!/usr/bin/env python
import roslib; roslib.load_manifest('ee_cart_imped_tutorial')
import rospy
import ee_cart_imped_action
import ee_cart_imped_control.control_switcher
import arm_navigation_msgs.msg
import arm_navigation_msgs.srv
import actionlib
import geometry_msgs.msg

class ArmControl:
    def __init__(self, arm_name):
        self.arm_name = arm_name
        self.switchToForceImpedanceControl()
        self.force_control = ee_cart_imped_action.EECartImpedClient\
            (self.arm_name)
        self.switchToArmNavigationControl()
        self.move_arm_control =\
            actionlib.SimpleActionClient\
            ('/move_'+self.arm_name,\
                 arm_navigation_msgs.msg.MoveArmAction)
        rospy.loginfo('Waiting for move arm server')
        self.move_arm_control.wait_for_server()
        
        
    def switchToArmNavigationControl(self):
        rospy.loginfo('Switching to arm navigation control on arm %s',
                      self.arm_name)
        ee_cart_imped_control.control_switcher.PR2CMClient.load_cartesian\
            (self.arm_name)
    
    def switchToForceImpedanceControl(self):
        rospy.loginfo('Switching to force/impedance control on arm %s',
                      self.arm_name)
        ee_cart_imped_control.control_switcher.PR2CMClient.load_ee_cart_imped\
            (self.arm_name)
    
    def moveToPoseForceImpedanceControl(self, pose_stamped, time):
        self.switchToForceImpedanceControl()
        self.force_control.moveToPoseStamped(pose_stamped, time)
    
    def moveToPoseArmNavigationControl(self, pose_stamped):
        self.switchToArmNavigationControl()
        goal = arm_navigation_msgs.msg.MoveArmGoal()
        goal.planner_service_name = '/ompl_planning/plan_kinematic_path'
        pconstraint = arm_navigation_msgs.msg.PositionConstraint()
        pconstraint.header = pose_stamped.header
        pconstraint.position = pose_stamped.pose.position
        pconstraint.link_name = self.arm_name[0]+'_wrist_roll_link'
        pconstraint.constraint_region_shape.type =\
            pconstraint.constraint_region_shape.BOX
        for i in range(3):
            pconstraint.constraint_region_shape.dimensions.append(0.01)
        pconstraint.constraint_region_orientation.w = 1.0
        pconstraint.weight = 1.0
        oconstraint = arm_navigation_msgs.msg.OrientationConstraint()
        oconstraint.header = pose_stamped.header
        oconstraint.link_name = self.arm_name[0]+'_wrist_roll_link'
        oconstraint.type = oconstraint.HEADER_FRAME
        oconstraint.orientation = pose_stamped.pose.orientation
        oconstraint.absolute_roll_tolerance = 0.1
        oconstraint.absolute_pitch_tolerance = 0.1
        oconstraint.absolute_yaw_tolerance = 0.1
        oconstraint.weight = 1.0
        goal.motion_plan_request.goal_constraints.position_constraints.append\
            (pconstraint)
        goal.motion_plan_request.goal_constraints.orientation_constraints.\
            append(oconstraint)
        state_srv = rospy.ServiceProxy('/environment_server/get_robot_state',
                                       arm_navigation_msgs.srv.GetRobotState)
        state_resp = state_srv()
        goal.motion_plan_request.start_state = state_resp.robot_state
        goal.motion_plan_request.planner_id = 'SBLkConfig1'
        goal.motion_plan_request.group_name = self.arm_name
        goal.motion_plan_request.num_planning_attempts = 2
        goal.motion_plan_request.allowed_planning_time = rospy.Duration(45.0)
        self.move_arm_control.send_goal_and_wait(goal, rospy.Duration(60.0))
        result = self.move_arm_control.get_result()
        state = self.move_arm_control.get_state()
        return (result, state)

def main():
    arm_control = ArmControl('right_arm')
    pose_stamped = geometry_msgs.msg.PoseStamped()
    pose_stamped.header.frame_id = '/torso_lift_link'
    pose_stamped.pose.position.x = 0.7
    pose_stamped.pose.position.y = 0
    pose_stamped.pose.position.z = 0.2
    pose_stamped.pose.orientation.w = 1.0
    rospy.loginfo('Moving to pose using arm navigation')
    arm_control.moveToPoseArmNavigationControl(pose_stamped)
    pose_stamped.pose.position.z = -0.3
    rospy.loginfo('Moving to pose using force/impedance control')
    arm_control.moveToPoseForceImpedanceControl(pose_stamped, 2.0)

if __name__ == '__main__':
    rospy.init_node('control_switching_test')
    main()
}}}

Make the code executable:
{{{
roscd ee_cart_imped_tutorial/scripts
chmod +x switch_control.py
}}}

=== Examining the Code ===

Now, let's break the code down.
<<CodeRef(switch_control, 5, 5)>>
This is the file containing the PR2CMClient for switching between the controllers.

<<CodeRef(switch_control, 14, 16)>>
Here we initialize the EECartImpedClient.  Note that the force/impedance controller must be started before initialization or the action client will hang waiting for it.

<<CodeRef(switch_control, 26, 30)>>
This is the function for switching the control to the arm navigation suite of controllers.  The PR2CMClient is a static class, meaning its functions can be called without an instantiation of the class.

<<CodeRef(switch_control, 32, 36)>>
This is the function for switching the control to the force/impedance controller.

=== Running the Code ===
If you have not yet done so, start the robot
{{{
robot start
}}}
or the simulator
{{{
roslaunch pr2_gazebo pr2_empty_world.launch
}}}
We must launch several files in order to run the switch_control.py file (you may, in fact, want to create your own launch file consisting of these files).  Firstly, we must launch the force/impedance controller.  If you currently have it running on either (or both) arms, kill it and re-launch it using
{{{
roslaunch ee_cart_imped_launch load_ee_cart_imped.launch
}}}
This file loads '''but does not start''' the force/impedance controller.  When using multiple controllers, you must use this launch file.  The other launch files available for the controller also continuously inhibit the standard arm controller.  If you are running one of them, you will not be able to change control.

We must also launch the arm navigation controllers:
{{{
roslaunch pr2_object_manipulation_launch pr2_manipulation_prerequisites.launch
roslaunch pr2_object_manipulation_launch pr2_manipulation.launch
}}}
(Note: the second launch file is not necessary for this example, but if you omit it you will get a series of error messages about stopping the r_arm_cartesian controllers.)

Now we are ready to try the code:
{{{
rosrun ee_cart_imped_tutorial switch_control.py
}}}
You should see the robot first move to a pose in front of itself using the arm navigation control and then move to a point 40 cm below using the force/impedance controller.

## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE