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))
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')
