def __init__(self):
     self._joint_state_listener = JointStateListener()
     self._arm_planners = {}
     self.side_joint_trajectory = {}
     self._arm_mover = ArmMover()
     for arm in ['left_arm', 'right_arm']:
         self._arm_planners[arm] = ArmPlanner(arm)
         self.side_joint_trajectory[arm] = rospy.get_param('/arm_configurations/side_tuck/trajectory/'+arm,
                                                           DEFAULT_SIDE_JOINT_TRAJECTORY[arm])
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Jon Binney

import roslib; 
roslib.load_manifest('pr2_plan_utils')
import rospy

from geometry_msgs.msg import PoseStamped

from pr2_plan_utils.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'

handle = arm_mover.move_to_goal(arm_name, goal_pose)
if handle.reached_goal():
    print 'Reached the goal!'
else:
    print handle.get_errors()

class ArmTasks:
    '''
    Performs high level tasks for the PR2 arms.  

    This class does high level tasks for the arms alone. 

    **Attributes:**
       
        **side_joint_trajectory ([[double]]):** Joint trajectory for moving to the side
    '''
    def __init__(self):
        self._joint_state_listener = JointStateListener()
        self._arm_planners = {}
        self.side_joint_trajectory = {}
        self._arm_mover = ArmMover()
        for arm in ['left_arm', 'right_arm']:
            self._arm_planners[arm] = ArmPlanner(arm)
            self.side_joint_trajectory[arm] = rospy.get_param('/arm_configurations/side_tuck/trajectory/'+arm,
                                                              DEFAULT_SIDE_JOINT_TRAJECTORY[arm])

    def move_arm_to_side(self, arm_name):
        '''
        A function that persistently tries to move an arm to the side.  

        Unless the arm is physically stuck in the world, the arm will be at the robot's side at the end of this 
        function.  However, as last resort, the robot uses an open-loop movement to a specific joint state so there 
        is no guarantee the arm's path will be collision-free.

        Specifically, this function tries in order to:
        
            1. Move the arm to the side joint pose using collision-free planning (calls move arm)
            2. If the starting state is in collision, it tries twice to move the state out of collision by
               executing a short open-loop trajectory to a free state.
            3. If the starting state is outside joint angles, it tries twice to move the state into joint angles by
               executing a short open-loop trajectory to a state within joint angle limits.
            4. Moves open loop to each of the joint states along the side joint trajectory.  By defalut there are
               three of these points.  The first is high, near the center of the robot's body.  The second is
               high and to the robot's side.  The last is low and at the robots side.  After each of the first two
               moves, the robot tries to move to the final joint state collision free before executing the next
               trajectory open-loop.

        **Args:**
            **arm_name (string):** The name of the arm ('right_arm' or 'left_arm') to be moved to side.
        '''

        if self._at_side(arm_name):
            rospy.loginfo(arm_name + ' is already at the side position')
            return
        self._arm_mover.move_into_joint_limits(arm_name)
        joint_state = self._arm_planners[arm_name].joint_positions_to_joint_state(
            self.side_joint_trajectory[arm_name][-1])

        # first try to move directly there using move arm
        handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0)
        if handle.reached_goal():
            return
        for t in range(2):
            checked_side = False
            for e in handle.get_errors():
                rospy.loginfo('When moving arm, error was:' + str(e))
                if type(e) != ArmNavError or not e.error_code:
                    if not checked_side and self._at_side(arm_name):
                        return
                    checked_side = True
                elif e.error_code.val == e.error_code.START_STATE_IN_COLLISION:
                    self._arm_mover.move_out_of_collision(arm_name)
                    handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0)
                    if handle.reached_goal():
                        return
                    rospy.loginfo('Even after moving out of collision, we are unable to move the arm to the side')
                    break
                elif e.error_code.val == e.error_code.JOINT_LIMITS_VIOLATED:
                    self._arm_mover.move_into_joint_limits(arm_name)
                    handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0)
                    if handle.reached_goal():
                        return
                    rospy.loginfo('Moved into joint limits but still unable to move the arm to the side')
                    break
       
        # now try to move to each point on our open loop trajectories
        # when there, see if we can move to side
        for p in range(len(self.side_joint_trajectory[arm_name])):
            joint_state.position = self.side_joint_trajectory[arm_name][p]
            self._arm_mover.move_to_goal(arm_name, joint_state, try_hard=True)
            if p < len(self.side_joint_trajectory[arm_name]) - 1:
                # try to go directly to side from here
                joint_state.position = self.side_joint_trajectory[arm_name][-1]
                handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0)
                if handle.reached_goal() or self._at_side(arm_name):
                    return

    def move_arms_to_side(self):
        '''
        Moves both arms to side by calling move_arm_to_side on each arm.
        '''
        self.move_arm_to_side('right_arm')
        self.move_arm_to_side('left_arm')


    def _at_side(self, arm_name):
        # check if we're already pretty close to the side position
        # move arm only does this if we're not in collision (which is kind of dumb)
        joint_state = self._arm_planners[arm_name].joint_positions_to_joint_state(
            self.side_joint_trajectory[arm_name][-1])
        current_joint_state = self._joint_state_listener.get_joint_positions(self._arm_planners[arm_name].joint_names)
        already_there = True
        for p in range(len(current_joint_state)):
            if abs(gt.angular_distance(current_joint_state[p], joint_state.position[p])) > 0.2:
                already_there = False
                rospy.loginfo('Joint '+joint_state.name[p]+' is out of position by '
                              + str(gt.angular_distance(current_joint_state[p], joint_state.position[p])))
                break
        return already_there
	def __init__(self, name):
		self._arm_mover = ArmMover()
		#self._tasks = Tasks()
		self._arm_tasks = ArmTasks()
		self._base = base.Base()
		self._head = head.Head()
		self._torso = torso.Torso()
		self._tf = tf.TransformListener( True, rospy.Duration(100) )
		self._tfBr = tf.TransformBroadcaster()

		#################### MOVE TORSO TO MAX HEIGHT ####################
		self._torso.up()
		rospy.loginfo('[gtp] Torso lifted!')
		
		#################### TUCK LEFT ARM #######################
		#self._tasks.move_arms_to_side()
		self._arm_tasks.move_arm_to_side('left_arm')
		rospy.loginfo('[gtp] Left arm moved to the side!')
		
		#action_name = 'tuck_arms'
		#tuck_arms_action_server = tuck_arms_main.TuckArmsActionServer(action_name)
		#tuck_arm_client = actionlib.SimpleActionClient(action_name, TuckArmsAction)

		#rospy.logdebug('Waiting for tuckarm action server to start')
		#tuck_arm_client.wait_for_server(rospy.Duration(10.0))

		#rospy.logdebug('Tucking left arm...')
		#goal = TuckArmsGoal()
		#goal.tuck_left = True
		#goal.tuck_right = False
		#tuck_arm_client.send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0))

		#################### MOVE RIGHT ARM TO INITAL POSE #######################
		#self._arm_tasks.move_arm_to_side('right_arm')
		#rospy.loginfo('[gtp] Right arm moved to the side!')		
		self._init_arm_pose = PoseStamped()
		self._init_arm_pose.pose.position.x = 0.282
		self._init_arm_pose.pose.position.y = -0.485 
		self._init_arm_pose.pose.position.z = 0.303
		self._init_arm_pose.pose.orientation.x = 0.030
		self._init_arm_pose.pose.orientation.y = 0.342
		self._init_arm_pose.pose.orientation.z = -0.057 
		self._init_arm_pose.pose.orientation.w = 0.937
		self._init_arm_pose.header.frame_id = 'torso_lift_link'
		
		for t in range(0,3):
			handle = self._arm_mover.move_to_goal( 'right_arm', self._init_arm_pose, 
									collision_aware_goal=True, planner_timeout=10.,
									bounds=(0.01, 0.01, 0.01, 0.1, 0.1, 0.1),
									try_hard=False )
			if handle.reached_goal():
				break
					
		if handle.reached_goal():
			rospy.loginfo('[gtp] Right arm init pose reached!')
		else:
			rospy.logerr('[gtp] Moving right arm to init pose failed!')
			rospy.logerr( handle.get_errors() )

		#################### STRAIGHTEN HEAD #################
		self._head.look_at_relative_point(0.8, 0.0, 0.5)
		rospy.loginfo('[gtp] Head straightened!')

		rospy.loginfo('[gtp] Attempting to reach initial pose...')
		init_goal = self._get_init_goal()
		success = self._go_to_pose( init_goal )
		if success:
			rospy.loginfo('[gtp] Initial pose successfully reached!')
		else:
			rospy.logwarn('[gtp] Initial pose was not reached :(')

		## Initialize action server
		self._action_name = name
		self._as = actionlib.SimpleActionServer( self._action_name, first_pr2_exp.msg.GoToPoseAction, execute_cb=self.execute_cb, auto_start = False )
		self._as.start()
		rospy.loginfo('[gtp] Action Server Initialized!')
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Sachin Chitta, Jon Binney

import roslib; 
roslib.load_manifest('pr2_plan_utils')
import rospy

from geometry_msgs.msg import PoseStamped
from pr2_plan_utils.arm_mover import ArmMover

# initialize
rospy.init_node('is_in_collision', anonymous=True)
right_arm = ArmMover('right_arm')

if right_arm.is_current_state_in_collision():
    print 'Right arm in collision'
else:
    print 'Right arm not in collision'

js = right_arm.get_joint_state()
js.position[0] = 1.0 #set shoulder pan (first joint) to swing into left arm

if right_arm.is_in_collision(js):
    print 'Right arm in collision'
else:
    print 'Right arm not in collision'
class BeAPoserAction(object):
	#result msg
	_result   = first_pr2_exp.msg.GoToPoseResult()


	def __init__(self, name):
		self._arm_mover = ArmMover()
		#self._tasks = Tasks()
		self._arm_tasks = ArmTasks()
		self._base = base.Base()
		self._head = head.Head()
		self._torso = torso.Torso()
		self._tf = tf.TransformListener( True, rospy.Duration(100) )
		self._tfBr = tf.TransformBroadcaster()

		#################### MOVE TORSO TO MAX HEIGHT ####################
		self._torso.up()
		rospy.loginfo('[gtp] Torso lifted!')
		
		#################### TUCK LEFT ARM #######################
		#self._tasks.move_arms_to_side()
		self._arm_tasks.move_arm_to_side('left_arm')
		rospy.loginfo('[gtp] Left arm moved to the side!')
		
		#action_name = 'tuck_arms'
		#tuck_arms_action_server = tuck_arms_main.TuckArmsActionServer(action_name)
		#tuck_arm_client = actionlib.SimpleActionClient(action_name, TuckArmsAction)

		#rospy.logdebug('Waiting for tuckarm action server to start')
		#tuck_arm_client.wait_for_server(rospy.Duration(10.0))

		#rospy.logdebug('Tucking left arm...')
		#goal = TuckArmsGoal()
		#goal.tuck_left = True
		#goal.tuck_right = False
		#tuck_arm_client.send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0))

		#################### MOVE RIGHT ARM TO INITAL POSE #######################
		#self._arm_tasks.move_arm_to_side('right_arm')
		#rospy.loginfo('[gtp] Right arm moved to the side!')		
		self._init_arm_pose = PoseStamped()
		self._init_arm_pose.pose.position.x = 0.282
		self._init_arm_pose.pose.position.y = -0.485 
		self._init_arm_pose.pose.position.z = 0.303
		self._init_arm_pose.pose.orientation.x = 0.030
		self._init_arm_pose.pose.orientation.y = 0.342
		self._init_arm_pose.pose.orientation.z = -0.057 
		self._init_arm_pose.pose.orientation.w = 0.937
		self._init_arm_pose.header.frame_id = 'torso_lift_link'
		
		for t in range(0,3):
			handle = self._arm_mover.move_to_goal( 'right_arm', self._init_arm_pose, 
									collision_aware_goal=True, planner_timeout=10.,
									bounds=(0.01, 0.01, 0.01, 0.1, 0.1, 0.1),
									try_hard=False )
			if handle.reached_goal():
				break
					
		if handle.reached_goal():
			rospy.loginfo('[gtp] Right arm init pose reached!')
		else:
			rospy.logerr('[gtp] Moving right arm to init pose failed!')
			rospy.logerr( handle.get_errors() )

		#################### STRAIGHTEN HEAD #################
		self._head.look_at_relative_point(0.8, 0.0, 0.5)
		rospy.loginfo('[gtp] Head straightened!')

		rospy.loginfo('[gtp] Attempting to reach initial pose...')
		init_goal = self._get_init_goal()
		success = self._go_to_pose( init_goal )
		if success:
			rospy.loginfo('[gtp] Initial pose successfully reached!')
		else:
			rospy.logwarn('[gtp] Initial pose was not reached :(')

		## Initialize action server
		self._action_name = name
		self._as = actionlib.SimpleActionServer( self._action_name, first_pr2_exp.msg.GoToPoseAction, execute_cb=self.execute_cb, auto_start = False )
		self._as.start()
		rospy.loginfo('[gtp] Action Server Initialized!')



	#
	#	_get_init_goal
	#
	def _get_init_goal(self):
		#tabOrigin = numpy.array([0.98, 0.0, 0.55])
		tabOrigin = numpy.array([1.475, 0.175, 0.85])
		sensorStart = tabOrigin + numpy.array([-1.0, -0.0, 0.5])
		offsets = numpy.array([0.0, -0.1, 0.1, -0.2, 0.2, -0.3, 0.3, -0.4, 0.4, -0.5, 0.5])


		dirVec = tabOrigin - sensorStart
		dirVec = dirVec / numpy.linalg.norm(dirVec)
		yaw_pitch_roll = numpy.array([numpy.arctan2(dirVec[1], dirVec[0]), 
												-numpy.arctan2(dirVec[2], numpy.hypot(dirVec[0], dirVec[1])), 0.0 ])
		
		# angle2quat
		cyaw = numpy.cos(yaw_pitch_roll[0]/2)
		cpitch = numpy.cos(yaw_pitch_roll[1]/2)
		croll = numpy.cos(yaw_pitch_roll[2]/2)
	
		syaw = numpy.sin(yaw_pitch_roll[0]/2)
		spitch = numpy.sin(yaw_pitch_roll[1]/2)
		sroll = numpy.sin(yaw_pitch_roll[2]/2)

		sensorOrient = numpy.array([ cyaw*cpitch*sroll - syaw*spitch*croll, 
					cyaw*spitch*croll + syaw*cpitch*sroll,
					syaw*cpitch*croll - cyaw*spitch*sroll,
					cyaw*cpitch*croll + syaw*spitch*sroll ])
		# end angle2quat

		init_goal = first_pr2_exp.msg.GoToPoseGoal()
		for i in range(len(offsets)):
			init_goal.goal_pose_arr.append(PoseStamped())
			init_goal.goal_pose_arr[i].pose.position.x = sensorStart[0] + offsets[i]*dirVec[0]
			init_goal.goal_pose_arr[i].pose.position.y = sensorStart[1] + offsets[i]*dirVec[1]
			init_goal.goal_pose_arr[i].pose.position.z = sensorStart[2] + offsets[i]*dirVec[2]
			init_goal.goal_pose_arr[i].pose.orientation.x = sensorOrient[0]
			init_goal.goal_pose_arr[i].pose.orientation.y = sensorOrient[1]
			init_goal.goal_pose_arr[i].pose.orientation.z = sensorOrient[2]
			init_goal.goal_pose_arr[i].pose.orientation.w = sensorOrient[3]
			init_goal.goal_pose_arr[i].header.frame_id = "map"
			init_goal.goal_pose_arr[i].header.stamp = rospy.Time.now()

		return init_goal
		

	
	#
	#	execute_cb
	#
	def execute_cb(self, goal):
		rospy.loginfo('[gtp] Starting action execution!')

		success = self._go_to_pose(goal)
		if success:
			rospy.loginfo('%s: Succeeded!' % (self._action_name) )
		else:
			rospy.loginfo('%s: Failed!' % (self._action_name) )
		self._result.success = success
		self._as.set_succeeded(self._result)
	



	#
	#	_go_to_pose
	#
	def _go_to_pose(self, goal):
		success = False
		bnds = (0.02, 0.02, 0.02, 0.1, 0.1, 0.1)
		num_arm_try = 3
		
		# Compute the camera to wrist transform
		while True:
			try:		
				(o_t_w, o_q_w) = self._tf.lookupTransform('/arm_kinect_depth_optical_frame', '/r_wrist_roll_link', rospy.Time(0))
				k_t_w = (o_t_w[2], -o_t_w[0], -o_t_w[1])
				k_q_w = self._quatmult([-0.5, 0.5, -0.5, 0.5],o_q_w)
				break
			except:
				rospy.logerr('[gtp] CANNOT LOOKUP camera to wrist transform')
				
		#################################################################
		########## Compute goal poses in /r_wrist_roll_link #############
		wrist_pose_arr = []
		for i in range(len(goal.goal_pose_arr)):
			map_t_kinect = numpy.array([goal.goal_pose_arr[i].pose.position.x, 
				                         goal.goal_pose_arr[i].pose.position.y,
				                         goal.goal_pose_arr[i].pose.position.z ])
			map_R_kinect = self._quat2rot( goal.goal_pose_arr[i].pose.orientation.x,
							 goal.goal_pose_arr[i].pose.orientation.y,
							 goal.goal_pose_arr[i].pose.orientation.z,
							 goal.goal_pose_arr[i].pose.orientation.w )
										 
			map_t_wrist = numpy.dot(map_R_kinect, k_t_w) + map_t_kinect		
			map_q_wrist = self._quatmult([goal.goal_pose_arr[i].pose.orientation.x,
							goal.goal_pose_arr[i].pose.orientation.y,
							goal.goal_pose_arr[i].pose.orientation.z,
							goal.goal_pose_arr[i].pose.orientation.w]
							, k_q_w)
			
			r_wrist_pose = goal.goal_pose_arr[i]
			r_wrist_pose.pose.position.x = map_t_wrist[0]
			r_wrist_pose.pose.position.y = map_t_wrist[1]
			r_wrist_pose.pose.position.z = map_t_wrist[2]
			r_wrist_pose.pose.orientation.x = map_q_wrist[0]
			r_wrist_pose.pose.orientation.y = map_q_wrist[1]
			r_wrist_pose.pose.orientation.z = map_q_wrist[2]
			r_wrist_pose.pose.orientation.w = map_q_wrist[3]
			r_wrist_pose.header.frame_id = 'map'
			r_wrist_pose.header.stamp = rospy.Time.now()
			wrist_pose_arr.append(r_wrist_pose)
		
		rospy.loginfo('[gtp] Done determining the wrist poses')
		
		#################################################################
		## Try each of the poses with the arm first
		needToMoveBase = True
		for i in range(len(wrist_pose_arr)):
			rospy.loginfo('[gtp] Trying pose %d of %d\n' % (i+1, len(wrist_pose_arr)) )
			# Broadcast for visualization
			self._broadcast_gpose( goal.goal_pose_arr[i] )
			rospy.loginfo('[gtp] Move end-effector to map: (%f, %f, %f | %f, %f, %f, %f)' % 
						(wrist_pose_arr[i].pose.position.x, 
				 		 wrist_pose_arr[i].pose.position.y, 
				 		 wrist_pose_arr[i].pose.position.z, 
				 		 wrist_pose_arr[i].pose.orientation.x, 
				 		 wrist_pose_arr[i].pose.orientation.y, 
				 		 wrist_pose_arr[i].pose.orientation.z, 
				 		 wrist_pose_arr[i].pose.orientation.w) )
			try:
				for t in range(0,num_arm_try):
					handle = self._arm_mover.move_to_goal( 'right_arm', wrist_pose_arr[i], 
				  										collision_aware_goal=True, planner_timeout=10.,
				  										bounds=bnds, try_hard=False )
				  	if handle.reached_goal():
				  		break
				  		
				if handle.reached_goal():
					needToMoveBase = False
					success = True
					break
			except:
				rospy.loginfo('Arm mover exception for goal %d of %d\n' % 
										(i+1, len(wrist_pose_arr)) )
		
		#################################################################
		## If the goal was not reached, we need to move the base
		if needToMoveBase:
			rospy.loginfo('[gtp] Moving arm from current position failed, need to move base.')
			# Raise arm first not to hit anything
			for t in range(0,num_arm_try):
				handle = self._arm_mover.move_to_goal('right_arm', self._init_arm_pose,
													collision_aware_goal=True, planner_timeout=10.,
													bounds=bnds, try_hard=False )
				if handle.reached_goal():
					break
									
			for i in range(len(wrist_pose_arr)):
				# Broadcast for visualization
				self._broadcast_gpose( goal.goal_pose_arr[i] )
				try:
					self._base.move_manipulable_pose( wrist_pose_arr[i].pose.position.x,
															 wrist_pose_arr[i].pose.position.y, 
															 wrist_pose_arr[i].pose.position.z, 
															 'torso', False, 
															 wrist_pose_arr[i].pose.orientation.x, 
															 wrist_pose_arr[i].pose.orientation.y, 
															 wrist_pose_arr[i].pose.orientation.z, 
															 wrist_pose_arr[i].pose.orientation.w )
				except:
					rospy.logerr('EXCEPTION DUDE: Move base failed!!!!\n')
					continue
				
				# Base pose was reached try to move the arm now
				rospy.loginfo('LALALBUBUBUB: DESIRED BASE POSE REACHED!');
				try:
					for t in range(0,num_arm_try):
						handle = self._arm_mover.move_to_goal( 'right_arm', wrist_pose_arr[i], 
				  										collision_aware_goal=True, planner_timeout=10.,
				  										bounds=bnds, try_hard=False )
				  		if handle.reached_goal():
				  			break
				  		
				  	if handle.reached_goal():
				  		success = True
				  		break
				except:
					rospy.loginfo('Arm mover exception for goal %d of %d\n' %
											(i+1, len(wrist_pose_arr)) )
					
		return success
				
	#
	# Broadcasts a PoseStamped
	#
	def _broadcast_gpose(self, gpose):
		self._tfBr.sendTransform( (gpose.pose.position.x, 
											gpose.pose.position.y, 
											gpose.pose.position.z),
										  (gpose.pose.orientation.x, gpose.pose.orientation.y,
											gpose.pose.orientation.z, gpose.pose.orientation.w),
										  rospy.Time.now(),
										  "kinect_goal_pose", "/map" )
										  
	def _quat2rot(self, q0, q1, q2, q3):
		R = numpy.array([[1.0-2.0*(q1*q1+q2*q2), 
		  		2.0*q0*q1-2.0*q3*q2,
		  		2.0*q3*q1+2.0*q0*q2],
    	  	  [2.0*q0*q1+2.0*q3*q2,
		  		1.0-2.0*(q0*q0+q2*q2),
		  		2.0*q1*q2-2.0*q3*q0],
    	  	  [2.0*q0*q2-2.0*q3*q1,
		  		2.0*q1*q2+2.0*q3*q0,
		  		1.0-2.0*(q0*q0+q1*q1)]])
		return R

	def _quatmult(self, q1, q2):
		x1, y1, z1, w1 = q1
		x2, y2, z2, w2 = q2
		x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
		y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
		z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
		w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
		return x, y, z, w
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Jon Binney

import roslib; 
roslib.load_manifest('pr2_plan_utils')
import rospy

from geometry_msgs.msg import PoseStamped

from pr2_plan_utils.arm_mover import ArmMover

rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()

# Tell the left arm to move to a cartesian goal
goal_pose = PoseStamped()
goal_pose.pose.position.x = 0.65
goal_pose.pose.position.y = 0.2
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = '/torso_lift_link'
handle1 = arm_mover.move_to_goal('left_arm', goal_pose, blocking=False)

# Tell the right arm to move to a joint space goal
joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338]
js = arm_mover.get_joint_state('right_arm')
js.position = joint_position
handle2 = arm_mover.move_to_goal('right_arm', js, blocking=False)
import roslib; roslib.load_manifest('pr2_plan_utils')
import rospy

from pr2_plan_utils.arm_mover import ArmMover

rospy.init_node('test_move_out_of_collision', anonymous=True)

arm_mover = ArmMover('right_arm')
arm_mover.move_out_of_collision(0.3)
#
# Author: Jon Binney

import roslib; 
roslib.load_manifest('pr2_plan_utils')
import rospy

from geometry_msgs.msg import PoseStamped

from pr2_plan_utils.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()

# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Jon Binney

import roslib
roslib.load_manifest('pr2_plan_utils')
import rospy

from pr2_plan_utils.arm_mover import ArmMover

# initialize
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()

# parameters
arm_name = 'right_arm'
joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338]

# get a joint state message already configured for this arm
js = arm_mover.get_joint_state(arm_name)

rospy.sleep(1.0)

# set desired joint positions
js.position = joint_position
print 'Moving to %s' % (str(joint_position))

# send out the command