[Documentation] [TitleIndex] [WordIndex

Note: This tutorial assumes that you have completed the previous tutorials: Writing a Stiffness Controller (Python), Running the Controller.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Switching Controllers at Run Time (Python)

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.

Keywords: force, compliance, impedance, stiffness, controller, pr2 arms, multiple controllers, switching controllers

Tutorial Level: INTERMEDIATE

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 control_switcher.PR2CMClient available in the ee_cart_imped_control package, which itself is a wrapper around the 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 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:

   1 #!/usr/bin/env python
   2 import roslib; roslib.load_manifest('ee_cart_imped_tutorial')
   3 import rospy
   4 import ee_cart_imped_action
   5 import ee_cart_imped_control.control_switcher
   6 import arm_navigation_msgs.msg
   7 import arm_navigation_msgs.srv
   8 import actionlib
   9 import geometry_msgs.msg
  10 
  11 class ArmControl:
  12     def __init__(self, arm_name):
  13         self.arm_name = arm_name
  14         self.switchToForceImpedanceControl()
  15         self.force_control = ee_cart_imped_action.EECartImpedClient\
  16             (self.arm_name)
  17         self.switchToArmNavigationControl()
  18         self.move_arm_control =\
  19             actionlib.SimpleActionClient\
  20             ('/move_'+self.arm_name,\
  21                  arm_navigation_msgs.msg.MoveArmAction)
  22         rospy.loginfo('Waiting for move arm server')
  23         self.move_arm_control.wait_for_server()
  24         
  25         
  26     def switchToArmNavigationControl(self):
  27         rospy.loginfo('Switching to arm navigation control on arm %s',
  28                       self.arm_name)
  29         ee_cart_imped_control.control_switcher.PR2CMClient.load_cartesian\
  30             (self.arm_name)
  31     
  32     def switchToForceImpedanceControl(self):
  33         rospy.loginfo('Switching to force/impedance control on arm %s',
  34                       self.arm_name)
  35         ee_cart_imped_control.control_switcher.PR2CMClient.load_ee_cart_imped\
  36             (self.arm_name)
  37     
  38     def moveToPoseForceImpedanceControl(self, pose_stamped, time):
  39         self.switchToForceImpedanceControl()
  40         self.force_control.moveToPoseStamped(pose_stamped, time)
  41     
  42     def moveToPoseArmNavigationControl(self, pose_stamped):
  43         self.switchToArmNavigationControl()
  44         goal = arm_navigation_msgs.msg.MoveArmGoal()
  45         goal.planner_service_name = '/ompl_planning/plan_kinematic_path'
  46         pconstraint = arm_navigation_msgs.msg.PositionConstraint()
  47         pconstraint.header = pose_stamped.header
  48         pconstraint.position = pose_stamped.pose.position
  49         pconstraint.link_name = self.arm_name[0]+'_wrist_roll_link'
  50         pconstraint.constraint_region_shape.type =\
  51             pconstraint.constraint_region_shape.BOX
  52         for i in range(3):
  53             pconstraint.constraint_region_shape.dimensions.append(0.01)
  54         pconstraint.constraint_region_orientation.w = 1.0
  55         pconstraint.weight = 1.0
  56         oconstraint = arm_navigation_msgs.msg.OrientationConstraint()
  57         oconstraint.header = pose_stamped.header
  58         oconstraint.link_name = self.arm_name[0]+'_wrist_roll_link'
  59         oconstraint.type = oconstraint.HEADER_FRAME
  60         oconstraint.orientation = pose_stamped.pose.orientation
  61         oconstraint.absolute_roll_tolerance = 0.1
  62         oconstraint.absolute_pitch_tolerance = 0.1
  63         oconstraint.absolute_yaw_tolerance = 0.1
  64         oconstraint.weight = 1.0
  65         goal.motion_plan_request.goal_constraints.position_constraints.append\
  66             (pconstraint)
  67         goal.motion_plan_request.goal_constraints.orientation_constraints.\
  68             append(oconstraint)
  69         state_srv = rospy.ServiceProxy('/environment_server/get_robot_state',
  70                                        arm_navigation_msgs.srv.GetRobotState)
  71         state_resp = state_srv()
  72         goal.motion_plan_request.start_state = state_resp.robot_state
  73         goal.motion_plan_request.planner_id = 'SBLkConfig1'
  74         goal.motion_plan_request.group_name = self.arm_name
  75         goal.motion_plan_request.num_planning_attempts = 2
  76         goal.motion_plan_request.allowed_planning_time = rospy.Duration(45.0)
  77         self.move_arm_control.send_goal_and_wait(goal, rospy.Duration(60.0))
  78         result = self.move_arm_control.get_result()
  79         state = self.move_arm_control.get_state()
  80         return (result, state)
  81 
  82 def main():
  83     arm_control = ArmControl('right_arm')
  84     pose_stamped = geometry_msgs.msg.PoseStamped()
  85     pose_stamped.header.frame_id = '/torso_lift_link'
  86     pose_stamped.pose.position.x = 0.7
  87     pose_stamped.pose.position.y = 0
  88     pose_stamped.pose.position.z = 0.2
  89     pose_stamped.pose.orientation.w = 1.0
  90     rospy.loginfo('Moving to pose using arm navigation')
  91     arm_control.moveToPoseArmNavigationControl(pose_stamped)
  92     pose_stamped.pose.position.z = -0.3
  93     rospy.loginfo('Moving to pose using force/impedance control')
  94     arm_control.moveToPoseForceImpedanceControl(pose_stamped, 2.0)
  95 
  96 if __name__ == '__main__':
  97     rospy.init_node('control_switching_test')
  98     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.

   5 import ee_cart_imped_control.control_switcher

This is the file containing the PR2CMClient for switching between the controllers.

  14         self.switchToForceImpedanceControl()
  15         self.force_control = ee_cart_imped_action.EECartImpedClient\
  16             (self.arm_name)

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.

  26     def switchToArmNavigationControl(self):
  27         rospy.loginfo('Switching to arm navigation control on arm %s',
  28                       self.arm_name)
  29         ee_cart_imped_control.control_switcher.PR2CMClient.load_cartesian\
  30             (self.arm_name)

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.

  32     def switchToForceImpedanceControl(self):
  33         rospy.loginfo('Switching to force/impedance control on arm %s',
  34                       self.arm_name)
  35         ee_cart_imped_control.control_switcher.PR2CMClient.load_ee_cart_imped\
  36             (self.arm_name)

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.


2019-12-07 12:35