| 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
Contents
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.
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.
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.
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.
