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():
t = rospy.Time.now().to_sec()
x = .65 + 0.1 * np.sin(t)
