#
# Author: Jon Binney
import roslib;
roslib.load_manifest('pr2_python')
import rospy
from geometry_msgs.msg import PoseStamped
from pr2_python.arm_mover import ArmMover
arm_name = 'right_arm'
# initialize
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()
# generate desired arm pose
goal_pose = PoseStamped()
goal_pose.pose.position.x = 0.65
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = '/torso_lift_link'
goal_pose.header.stamp = rospy.Time(0)
handle = arm_mover.move_to_goal_using_cartesian_control(arm_name, goal_pose, timeout=5.0, blocking=True)
if handle.reached_goal():
print 'Reached the goal!'
else:
print handle.get_errors()
