import rospy from geometry_msgs.msg import PoseStamped from pr2_python.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) # wait for moves to complete handle1.wait() handle2.wait() # check whether movements were successful if handle1.reached_goal(): print 'Left arm reached the goal' else:
# # Author: Jon Binney import roslib roslib.load_manifest('pr2_python') import rospy from pr2_python.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 handle = arm_mover.move_to_goal(arm_name, js) if not handle.reached_goal(): print handle.get_errors()
# POSSIBILITY OF SUCH DAMAGE. # # 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' handle = arm_mover.move_to_goal(arm_name, goal_pose) if handle.reached_goal(): print 'Reached the goal!' else: print handle.get_errors()
class SensingPlacer: def __init__(self, arm_name): self.arm_name = arm_name self.mover = ArmMover(arm_name) self.planner = ArmPlanner(arm_name) self.gripper = Gripper(arm_name) self.base = base.Base() #self.gripper.close() def place(self, x, y, z): self.base.move_manipulable_pose(x, y, z, try_hard = True, group=self.arm_name) world_pose = Pose() world_pose.position.x = x world_pose.position.y = y world_pose.position.z = z world_pose.orientation.x = 0.0 world_pose.orientation.y = 0.0 world_pose.orientation.z = 0.0 world_pose.orientation.w = 1.0 new_pose = transform_pose('base_footprint', 'map', world_pose) hand_pose = self.planner.get_hand_frame_pose() #print hand_pose #print new_pose hand_pose.pose.position.x = new_pose.position.x hand_pose.pose.position.y = new_pose.position.y hand_pose.pose.position.z = new_pose.position.z + 0.1 self if self.arm_name == 'right_arm': joint_position = [-1.254, -0.325, -1.064, -1.525, 0.109, -1.185, 0.758] else: joint_position = [1.191, -0.295, 0.874, -1.610, 0.048, -1.069, -.988] joint_state = self.mover.get_joint_state() joint_state.position = joint_position self.mover.move_to_goal(joint_state) self.mover.move_to_goal_directly(hand_pose,collision_aware=False) #print "********" #print self.mover.get_exceptions() #print "********" still_in_free_space = True hand_pose = self.planner.get_hand_frame_pose() while still_in_free_space: goal_z = hand_pose.pose.position.z - 0.03 hand_pose.pose.position.z = goal_z self.mover.move_to_goal_directly(hand_pose,collision_aware=False) hand_pose = self.planner.get_hand_frame_pose() if abs(hand_pose.pose.position.z - goal_z) > 0.01: still_in_free_space = False #print self.mover.reached_goal() #if not self.mover.reached_goal(): # still_in_free_space = False self.gripper.open() if self.arm_name == 'right_arm': gripper_name = 'r_gripper_palm_link' else: gripper_name = 'l_gripper_palm_link' gripper_pose = transform_pose_stamped(gripper_name, hand_pose) gripper_pose.pose.position.x -= 0.20 hand_pose = transform_pose_stamped(hand_pose.header.frame_id, gripper_pose) self.mover.move_to_goal_directly(hand_pose, collision_aware=False) self.mover.move_to_goal(joint_state)