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)
