objects = object_recognizer.look_for_objects() while not objects.recognized_objects.objects: objects = object_recognizer.look_for_objects() rate = rospy.Rate(10.0) listener.waitForTransform("/head_camera_rgb_optical_frame", "/base_link", rospy.Time(0), rospy.Duration(5.0)) old_point = PointStamped() old_point.header.frame_id = "head_camera_rgb_optical_frame" old_point.point.x = objects.recognized_objects.objects[0].pose.pose.pose.position.x old_point.point.y = objects.recognized_objects.objects[0].pose.pose.pose.position.y old_point.point.z = objects.recognized_objects.objects[0].pose.pose.pose.position.z new_point = listener.transformPoint("base_link", old_point) arm_mover.set_pose_reference_frame("base_link") euler = euler_from_quaternion([0.94, -0.3, -0.035, 0.03]) # go to pre-grasp pose pose = [new_point.point.x, new_point.point.y, new_point.point.z + 0.45, euler[0], euler[1], euler[2]] print "I want to move to: %s" % (str(pose)) arm_mover.set_pose_target(pose, "wrist_roll_link") if not arm_mover.go(): continue rospy.sleep(1.0) # move down to grasp new_pose = pose new_pose[2] = new_pose[2] - 0.18 new_pose[1] = new_pose[1] + 0.02
listener.waitForTransform("/head_camera_rgb_optical_frame", "/base_link", rospy.Time(0), rospy.Duration(5.0)) old_point = PointStamped() old_point.header.frame_id = 'head_camera_rgb_optical_frame' old_point.point.x = objects.recognized_objects.objects[ 0].pose.pose.pose.position.x old_point.point.y = objects.recognized_objects.objects[ 0].pose.pose.pose.position.y old_point.point.z = objects.recognized_objects.objects[ 0].pose.pose.pose.position.z new_point = listener.transformPoint('base_link', old_point) arm_mover.set_pose_reference_frame('base_link') euler = euler_from_quaternion([0.94, -0.3, -0.035, 0.03]) #go to pre-grasp pose pose = [ new_point.point.x, new_point.point.y, new_point.point.z + 0.45, euler[0], euler[1], euler[2] ] print "I want to move to: %s" % (str(pose)) arm_mover.set_pose_target(pose, "wrist_roll_link") if not arm_mover.go(): continue rospy.sleep(1.0) #move down to grasp
from moveit_commander.commander import MoveGroupCommander # initialize rospy.init_node('test_arm_control', anonymous=True) arm_mover = MoveGroupCommander('arm') # desired joint positions #msg = JointState() #joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338] #joints = [-0.199792813192, -0.0301778257579, -0.119474613742, 0.113024891124, -0.0663316698686, -0.497162338433, 0.488334857099] #msg.position = joints # get a joint state message already configured for this arm js = arm_mover.get_current_joint_values() print "Current joints: %s" % (str(js)) arm_mover.set_pose_reference_frame('base_link') pose = arm_mover.get_current_pose() print "Current pose: %s" % (str(pose)) # set desired joint positions #js.position = joints #print 'Moving to %s' % (str(joints)) # send out the command #reached_goal = arm_mover.move_to_goal(js) #if not reached_goal: # print arm_mover.get_exceptions() #reached_goal = arm_mover.go(joints) #if reached_goal: # print 'Success.' #else: # print 'Goal not reached.'