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)