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