def load_ee_cart_imped(self, isRightArm): ''' loads the ee_cart_imped controller on the arm specified. stops the cartesian controller first. @param isRightArm: if true then the controller will be loaded on the right arm, else the left arm ''' rospy.logwarn('starting ee_cart_imped controller on the '+ ('right' if isRightArm else 'left') + ' arm') if isRightArm: status = pr2cm_interface.stop_controller('r_arm_controller') #print 'stopping arm controller status:', status status = pr2cm_interface.stop_controller('r_arm_cartesian_trajectory_controller') #print 'stopping trajectory controller status:', status status = pr2cm_interface.stop_controller('r_arm_cartesian_pose_controller') #print 'stopping pose controller status:', status status = pr2cm_interface.start_controller('r_arm_cart_imped_controller') #print 'starting controller status:', status else: status = pr2cm_interface.stop_controller('l_arm_controller') #print 'stopping controller status:', status status = pr2cm_interface.stop_controller('l_arm_cartesian_trajectory_controller') #print 'stopping trajectory controller status:', status status = pr2cm_interface.stop_controller('l_arm_cartesian_pose_controller') #print 'stopping pose controller status:', status status = pr2cm_interface.start_controller('l_arm_cart_imped_controller') #print 'starting controller status:', status rospy.loginfo('done') return status
def set_controllers_state(self, controllers, start_controller): print "Dealing with " + str(controllers) # Need to activate physics or the calls to pr2_controller_manager will wait till end of time self.set_paused_physics(False) for controller in controllers: if start_controller: pr2_controller_manager_interface.start_controller(controller) else: pr2_controller_manager_interface.stop_controller(controller) # Deactivate physics to make more changes (eventually) self.set_paused_physics(True) print "Done with controllers states"
def main(): if len(sys.argv) < 2: print "Usage: effort.py <joint>" sys.exit(1) joint = sys.argv[1] rospy.init_node('effort', anonymous=True) # Override rospy's signal handling. We'll invoke rospy's handler after # we're done shutting down prev_handler = signal.getsignal(signal.SIGINT) signal.signal(signal.SIGINT, shutdown) load_joint_config(joint) pr2_controller_manager_interface.load_controller(CONTROLLER_NAME) pr2_controller_manager_interface.start_controller(CONTROLLER_NAME) pub = rospy.Publisher("%s/command" % CONTROLLER_NAME, Float64) print "Enter efforts:" while not rospy.is_shutdown(): effort = float(sys.stdin.readline().strip()) pub.publish(Float64(effort))
#! /usr/bin/env python import os, roslib, time roslib.load_manifest('openrave_actionlib') roslib.load_manifest('pr2_controller_manager') import rospy, sys import rosparam import yaml from pr2_controller_manager import pr2_controller_manager_interface from pr2_mechanism_msgs.msg import MechanismStatistics from pr2_mechanism_msgs.srv import * pr2_controller_manager_interface.stop_controller('l_arm_controller') pr2_controller_manager_interface.stop_controller('r_arm_controller') pr2_controller_manager_interface.stop_controller('torso_controller') name_yaml = yaml.load( open( os.path.join(roslib.packages.get_pkg_dir('openrave_actionlib'), 'pr2_midbody_controller.yaml'))) rosparam._set_param("", name_yaml) time.sleep(0.5) # wait for parameter server rospy.wait_for_service('pr2_controller_manager/load_controller') rospy.ServiceProxy('pr2_controller_manager/load_controller', LoadController)('midbody_controller') pr2_controller_manager_interface.start_controller('midbody_controller')
#! /usr/bin/env python import os, roslib, time roslib.load_manifest('openrave_actionlib') roslib.load_manifest('pr2_controller_manager') import rospy, sys import rosparam import yaml from pr2_controller_manager import pr2_controller_manager_interface from pr2_mechanism_msgs.msg import MechanismStatistics from pr2_mechanism_msgs.srv import * pr2_controller_manager_interface.stop_controller('l_arm_controller') pr2_controller_manager_interface.stop_controller('r_arm_controller') pr2_controller_manager_interface.stop_controller('torso_controller') name_yaml = yaml.load(open(os.path.join(roslib.packages.get_pkg_dir('openrave_actionlib'),'pr2_midbody_controller.yaml'))) rosparam._set_param("",name_yaml) time.sleep(0.5) # wait for parameter server rospy.wait_for_service('pr2_controller_manager/load_controller') rospy.ServiceProxy('pr2_controller_manager/load_controller', LoadController)('midbody_controller') pr2_controller_manager_interface.start_controller('midbody_controller')