import roslib; roslib.load_manifest('pr2_plan_utils') import sys from pr2_plan_utils.controller_manager_client import ControllerManagerClient controllers_to_start = [sys.argv[1]] controllers_to_stop = [] cmc = ControllerManagerClient() print 'Controllers:' print cmc.list_controllers() cmc.switch_controllers(controllers_to_start, controllers_to_stop) print '' print 'Controllers now:' print cmc.list_controllers()
import roslib; roslib.load_manifest('pr2_plan_utils') import numpy as np import rospy from geometry_msgs.msg import PoseStamped import tf from pr2_plan_utils.controller_manager_client import ControllerManagerClient from pr2_plan_utils.cartesian_controller_interface import CartesianControllerInterface arm_name = 'right_arm' goal_frame = '/cartesian_goal_frame' cartesian_controller = '%s_cart' % arm_name[0] # initialize rospy.init_node('test_cartesian_tracking', anonymous=True) controller_manager = ControllerManagerClient() controller_manager.switch_controllers([cartesian_controller]) cci = CartesianControllerInterface(arm_name, 'cci') # tell cartesian control interface to follow tracking frame goal_pose = PoseStamped() goal_pose.pose.position.z = 0.2 goal_pose.pose.orientation.w = 1.0 goal_pose.header.frame_id = goal_frame goal_pose.header.stamp = rospy.Time(0) cci.set_desired_pose(goal_pose, 0.02) # loop, broadcasting the transform to the tracking frame br = tf.TransformBroadcaster() r = rospy.Rate(100) while not rospy.is_shutdown():