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():