def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-l', '--limb', required=True, choices=['left', 'right'],
help='send joint trajectory to which limb'
)
args = parser.parse_args(rospy.myargv()[1:])
limb = args.limb
rospy.init_node('stackit')
iksvc, ns = ik_command.connect_service(limb)
moveit_commander.roscpp_initialize(sys.argv)
rate = rospy.Rate(1)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander(limb+"_arm")
group.allow_replanning(True)
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
rs.enable()
#Calibrate gripper
gripper_if = baxter_interface.Gripper(limb)
if not gripper_if.calibrated():
print "Calibrating gripper"
gripper_if.calibrate()
obj_manager = ObjectManager()
while len(obj_manager.collision_objects) <= 0:
rate.sleep()
objects = obj_manager.collision_objects
object_height = params['object_height']
"""if len(objects) > 1:
stack_pose = projectPose(objects[len(objects)-1].primitive_poses[0])
stack_pose = incrementPoseMsgZ(stack_pose, object_height*2.0)
objects.pop(len(objects)-1)
elif len(objects) == 1:
stack_pose = projectPose(objects[0].primitive_poses[0])"""
stack_pose = Pose(position=Point(0.593, -0.212, object_height-0.130),
orientation=Quaternion(0.6509160466, 0.758886809948,
-0.0180992582839, -0.0084573527776) )
processed_win = "Processed image"
raw_win = "Hand camera"
cv2.namedWindow(processed_win)
#cv2.namedWindow(raw_win)
rospy.on_shutdown(obj_manager.remove_known_objects)
for obj in objects:
obj_pose = obj.primitive_poses[0]
#group.pick(obj.id)
#group.place(obj.id, stack_pose)
print "Got pose:", obj.primitive_poses[0]
pose = projectPose(obj.primitive_poses[0])
pose = incrementPoseMsgZ(pose, object_height*1.7)
print "Modified pose:", pose
#if obj.id in obj_manager.id_operations:
# obj_manager.id_operations[obj.id] = CollisionObject.REMOVE
#obj_manager.publish(obj)
print "setting target to pose"
# Move to the next block
group.clear_pose_targets()
group.set_start_state_to_current_state()
group.set_pose_target(pose)
plan = group.plan()
# is there a better way of checking this?
plan_found = len(plan.joint_trajectory.points) > 0
if plan_found:
#print "============ Waiting while RVIZ displays plan1..."
#rospy.sleep(3)
group.go(wait=True)
imgproc = ObjectFinder("star", None, None)
imgproc.subscribe("/cameras/"+limb+"_hand_camera/image")
imgproc.publish(limb)
vc = VisualCommand(iksvc, limb)
vc.subscribe()
while (not vc.done) and (not rospy.is_shutdown()):
blobArray = []
for centroid, axis in zip(imgproc.centroids, imgproc.axes):
blob = BlobInfo()
centroid = Point(*centroid)
blob.centroid = centroid
if axis is None:
axis = -1*numpy.ones(6)
blob.axis = Polygon([Point(*axis[0:3].tolist()),
Point(*axis[3:6].tolist())])
blobArray.append(blob)
msg = BlobInfoArray()
msg.blobs = blobArray
imgproc.handler_pub.publish(msg)
if imgproc.processed is not None:
cv2.imshow(processed_win, imgproc.processed)
cv2.waitKey(10)
vc.unsubscribe()
imgproc.unsubscribe()
print "Adding attached message"
#Add attached message
#obj.primitive_poses[0] = incrementPoseMsgZ(obj.primitive_poses[0], *object_height) #this is in the base frame...?
obj_manager.publish_attached(obj, limb)
#touch_links = [limb+"_gripper", limb+"_gripper_base", limb+"_hand_camera", limb+"_hand_range"]
#group.attach_object(obj.id, limb+"_gripper", touch_links = touch_links)
# Carefully rise away from the object before we plan another path
pose = incrementPoseMsgZ(pose, 2*object_height) # test this
ik_command.service_request_pose(iksvc, pose, limb, blocking = True)
else:
print "Unable to plan path"
continue
# what to do?
# Move to the stacking position
group.clear_pose_targets()
group.set_start_state_to_current_state()
group.set_pose_target(stack_pose)
plan = group.plan()
plan_found = len(plan.joint_trajectory.points) > 0
if plan_found:
#print "============ Waiting while RVIZ displays plan2..."
#rospy.sleep(3)
group.go(wait=True)
gripper_if.open(block=True)
group.detach_object(obj.id)
# Carefully rise away from the object before we plan another path
pose = incrementPoseMsgZ(stack_pose, 2*object_height)
ik_command.service_request_pose(iksvc, pose, limb, blocking = True)
obj.operation = CollisionObject.REMOVE
obj_manager.publish(obj)
# Get the next stack pose
stack_pose = incrementPoseMsgZ(stack_pose, object_height*3/4)
"""if obj.id in obj_manager.id_operations:
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-l', '--limb', required=True, choices=['left', 'right'],
help='send joint trajectory to which limb'
)
required.add_argument(
'-f', '--folder', help='path to assets/ folder containing help images'
)
args = parser.parse_args(rospy.myargv()[1:])
print args
limb = args.limb
print("Initializing node... ")
rospy.init_node("servo_to_object_%s" % (limb,))
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
rs.enable()
#Calibrate gripper
gripper_if = baxter_interface.Gripper(limb)
if not gripper_if.calibrated():
print "Calibrating gripper"
gripper_if.calibrate()
if args.folder is None:
args.folder = "/home/jackie/ros_ws/src/baxter_demos/assets/"
# First, get start and end configurations from the user
filenames = ["getpoint1.png", "getpoint2.png", "executing_grasp.png"]
filenames = [args.folder+filename for filename in filenames]
for filename in filenames:
if not os.access(filename, os.R_OK):
rospy.logerr("Cannot read file at '%s'" % (filename,))
return 1
limbInterface = baxter_interface.Limb(limb)
#rospy.on_shutdown(display)
common.send_image(filenames[0])
#Get the current position
buttonpress = common.ButtonListener()
buttonpress.subscribe("/robot/digital_io/"+limb+"_lower_button/state")
points = []
while not buttonpress.pressed:
rospy.sleep(params['button_rate'])
points.append( buttonpress.getButtonPress(limbInterface))
print "Got first position:"
print points[0]
buttonpress.pressed = False
common.send_image(filenames[1])
while not buttonpress.pressed:
rospy.sleep(params['button_rate'])
points.append( buttonpress.getButtonPress(limbInterface))
print "Got second position:"
print points[1]
common.send_image(filenames[2])
# Command start configuration
limbInterface.move_to_joint_positions(points[0])
iksvc, ns = ik_command.connect_service(limb)
rate = rospy.Rate(params['rate'])
dc = DepthCaller(limb, iksvc)
# Subscribe to estimate_depth
# Move to pose published by estimate_depth
while (not dc.done) and (not rospy.is_shutdown()):
rate.sleep()
print "Starting visual servoing"
# Subscribe to object_finder and start visual servoing/grasping
vc = VisualCommand(iksvc, limb)
vc.subscribe()
while (not vc.done) and (not rospy.is_shutdown()):
rate.sleep()
limbInterface.move_to_joint_positions(points[1])
# Let go
gripper_if.open()
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-l', '--limb', required=True, choices=['left', 'right'],
help='send joint trajectory to which limb'
)
args = parser.parse_args(rospy.myargv()[1:])
print args
limb = args.limb
print("Initializing node... ")
rospy.init_node("super_stacker_%s" % (limb,))
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
rs.enable()
#Calibrate gripper
gripper_if = baxter_interface.Gripper(limb)
if not gripper_if.calibrated():
print "Calibrating gripper"
gripper_if.calibrate()
limbInterface = baxter_interface.Limb(limb)
iksvc, ns = ik_command.connect_service(limb)
rate = rospy.Rate(100)
# Get goal poses of each object in the scene
dc = DepthCaller(limb)
# Subscribe to estimate_depth
# Move to pose published by estimate_depth
while (not dc.done) and (not rospy.is_shutdown()):
rate.sleep()
#pass
# Subscribe to object_finder and start visual servoing/grasping
# The stack pose is the pose of the smallest object with a z-offset
# accounting for the height of the object (this assumption makes
# it really important that the segmentation is accurate)
stack_pose = [0.594676466827, -0.296644499519, -0.0322744943164, 0.971805911045, -0.22637170004, 0.065946440385, 0.000437813100735]
#stack_pose = dc.object_poses[-1]
#stack_pose = incrementPoseZ(stack_pose, object_height)
#dc.object_poses.pop(len(dc.object_poses)-1)
for pose in dc.object_poses:
ik_command.service_request(iksvc, pose, limb, blocking=True)
vc = VisualCommand(iksvc, limb)
vc.subscribe()
while (not vc.done) and (not rospy.is_shutdown()):
rate.sleep()
ik_command.service_request(iksvc, stack_pose, limb, blocking=True)
#limbInterface.move_to_joint_positions(stack_pose)
# Let go
gripper_if.open(block=True)
stack_pose = incrementPoseZ(pose, object_height)
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument('-l',
'--limb',
required=True,
choices=['left', 'right'],
help='send joint trajectory to which limb')
args = parser.parse_args(rospy.myargv()[1:])
limb = args.limb
rospy.init_node('stackit')
iksvc, ns = ik_command.connect_service(limb)
moveit_commander.roscpp_initialize(sys.argv)
rate = rospy.Rate(1)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander(limb + "_arm")
group.allow_replanning(True)
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
rs.enable()
#Calibrate gripper
gripper_if = baxter_interface.Gripper(limb)
if not gripper_if.calibrated():
print "Calibrating gripper"
gripper_if.calibrate()
obj_manager = ObjectManager()
while len(obj_manager.collision_objects) <= 0:
rate.sleep()
objects = obj_manager.collision_objects
object_height = params['object_height']
"""if len(objects) > 1:
stack_pose = projectPose(objects[len(objects)-1].primitive_poses[0])
stack_pose = incrementPoseMsgZ(stack_pose, object_height*2.0)
objects.pop(len(objects)-1)
elif len(objects) == 1:
stack_pose = projectPose(objects[0].primitive_poses[0])"""
stack_pose = Pose(position=Point(0.593, -0.212, object_height - 0.130),
orientation=Quaternion(0.6509160466, 0.758886809948,
-0.0180992582839,
-0.0084573527776))
processed_win = "Processed image"
raw_win = "Hand camera"
cv2.namedWindow(processed_win)
#cv2.namedWindow(raw_win)
rospy.on_shutdown(obj_manager.remove_known_objects)
for obj in objects:
obj_pose = obj.primitive_poses[0]
#group.pick(obj.id)
#group.place(obj.id, stack_pose)
print "Got pose:", obj.primitive_poses[0]
pose = projectPose(obj.primitive_poses[0])
pose = incrementPoseMsgZ(pose, object_height * 1.7)
print "Modified pose:", pose
#if obj.id in obj_manager.id_operations:
# obj_manager.id_operations[obj.id] = CollisionObject.REMOVE
#obj_manager.publish(obj)
print "setting target to pose"
# Move to the next block
group.clear_pose_targets()
group.set_start_state_to_current_state()
group.set_pose_target(pose)
plan = group.plan()
# is there a better way of checking this?
plan_found = len(plan.joint_trajectory.points) > 0
if plan_found:
#print "============ Waiting while RVIZ displays plan1..."
#rospy.sleep(3)
group.go(wait=True)
imgproc = ObjectFinder("star", None, None)
imgproc.subscribe("/cameras/" + limb + "_hand_camera/image")
imgproc.publish(limb)
vc = VisualCommand(iksvc, limb)
vc.subscribe()
while (not vc.done) and (not rospy.is_shutdown()):
blobArray = []
for centroid, axis in zip(imgproc.centroids, imgproc.axes):
blob = BlobInfo()
centroid = Point(*centroid)
blob.centroid = centroid
if axis is None:
axis = -1 * numpy.ones(6)
blob.axis = Polygon([
Point(*axis[0:3].tolist()),
Point(*axis[3:6].tolist())
])
blobArray.append(blob)
msg = BlobInfoArray()
msg.blobs = blobArray
imgproc.handler_pub.publish(msg)
if imgproc.processed is not None:
cv2.imshow(processed_win, imgproc.processed)
cv2.waitKey(10)
vc.unsubscribe()
imgproc.unsubscribe()
print "Adding attached message"
#Add attached message
#obj.primitive_poses[0] = incrementPoseMsgZ(obj.primitive_poses[0], *object_height) #this is in the base frame...?
obj_manager.publish_attached(obj, limb)
#touch_links = [limb+"_gripper", limb+"_gripper_base", limb+"_hand_camera", limb+"_hand_range"]
#group.attach_object(obj.id, limb+"_gripper", touch_links = touch_links)
# Carefully rise away from the object before we plan another path
pose = incrementPoseMsgZ(pose, 2 * object_height) # test this
ik_command.service_request_pose(iksvc, pose, limb, blocking=True)
else:
print "Unable to plan path"
continue
# what to do?
# Move to the stacking position
group.clear_pose_targets()
group.set_start_state_to_current_state()
group.set_pose_target(stack_pose)
plan = group.plan()
plan_found = len(plan.joint_trajectory.points) > 0
if plan_found:
#print "============ Waiting while RVIZ displays plan2..."
#rospy.sleep(3)
group.go(wait=True)
gripper_if.open(block=True)
group.detach_object(obj.id)
# Carefully rise away from the object before we plan another path
pose = incrementPoseMsgZ(stack_pose, 2 * object_height)
ik_command.service_request_pose(iksvc, pose, limb, blocking=True)
obj.operation = CollisionObject.REMOVE
obj_manager.publish(obj)
# Get the next stack pose
stack_pose = incrementPoseMsgZ(stack_pose, object_height * 3 / 4)
"""if obj.id in obj_manager.id_operations: