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("visual_servo")
#Bit of cheating: move the robot's arms to a neutral position before running
dominant_joints=[1.187301128466797, 1.942403170440674, 0.08206797205810547, -0.996704015789795, -0.6734175651123048, 1.0266166411193849, 0.4985437554931641]
off_joints = [-1.1255584018249511, 1.4522963092712404, 0.6354515406555176, -0.8843399232055664, 0.6327670742797852, 1.2751215284729005, -0.4084223843078614, ]
names = ['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2']
off_limb = "right"
if limb == "right": off_limb = "left"
off_names = [off_limb+"_"+name for name in names]
jointlists = {}
jointlists[off_limb] = off_joints
jointlists[limb] = dominant_joints
for side in [limb, off_limb]:
limb_if = baxter_interface.Limb(side)
limb_names = [side+"_"+name for name in names]
joint_commands = dict(zip(limb_names, jointlists[side] ))
limb_if.move_to_joint_positions(joint_commands)
#Calibrate gripper
gripper_if = baxter_interface.Gripper(limb)
if not gripper_if.calibrated():
print "Calibrating gripper"
gripper_if.calibrate()
iksvc, ns = ik_command.connect_service(limb)
command = VisualCommand(iksvc, limb)
command.subscribe()
command.publish()
while not rospy.is_shutdown():
command.handler_pub.publish(command.done)
command.pub_rate.sleep()
def map_keyboard():
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
#These are from PyKDL and are needed for the Jacobian
left_kin = baxter_kinematics('left')
right_kin = baxter_kinematics('right')
#Connect with IK service
right_iksvc, right_ns = ik_command.connect_service('right')
left_iksvc, left_ns = ik_command.connect_service('left')
def command_jacobian(side, direction):
#UNUSED
if side == 'left':
limb = left
kin = left_kin
elif side == 'right':
limb = right
kin = right_kin
else:
raise Exception("Got wrong side in command_jacobian")
# current is the current position of end effector
current_p = np.array(limb.endpoint_pose()['position'] +
limb.endpoint_pose()['orientation'])
current_p = current_q.reshape((7, 1))
current = np.zeros((6, 1))
quaternion_to_euler(current_p, current)
# We need to convert the direction from the world frame to the hand frame
# Rotate direction by the inverse of the rotation matrix from the angular pose in current
R = tf.transformations.quaternion_matrix(
current_p[3:7].flatten().tolist())
R_inv = tf.transformations.inverse_matrix(R)
print R_inv[0:3, 0:3]
direction = np.array(direction).reshape((6, 1))
print direction[0:3]
dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3])
print dir_rot
#direction[0:3] = dir_rot
# Goal is the goal position, found by adding the requested direction from the user
goal = current + direction
current_angles = np.array(
[limb.joint_angles()[name] for name in limb.joint_names()])
# Get the Jacobian inverse and solve for the necessary change in joint angles
jacobian_inv = kin.jacobian_transpose()
delta = jacobian_inv * (goal - current)
commands = current_angles.flatten() + delta.flatten()
# Command the joints
command_list = commands.tolist()
joint_command = dict(zip(limb.joint_names(), command_list[0]))
limb.set_joint_positions(joint_command)
def command_ik(side, direction):
"""Use the Rethink IK service to figure out a desired joint position"""
if side == 'left':
limb = left
iksvc = left_iksvc
ns = left_ns
else:
limb = right
iksvc = right_iksvc
ns = right_ns
current_p = np.array(limb.endpoint_pose()['position'] +
limb.endpoint_pose()['orientation'])
direction = np.array(direction)
desired_p = current_p + direction
ik_command.service_request(iksvc, desired_p, side, False)
zeros = [0] * 4
inc = 0.1
bindings = {
'q': (command_ik, ['right', [inc, 0, 0] + zeros], "increase right x"),
'a': (command_ik, ['right', [-inc, 0, 0] + zeros], "decrease right x"),
'w': (command_ik, ['right', [0, inc, 0] + zeros], "increase right y"),
's': (command_ik, ['right', [0, -inc, 0] + zeros], "decrease right y"),
'e': (command_ik, ['right', [0, 0, inc] + zeros], "increase right z"),
'd': (command_ik, ['right', [0, 0, -inc] + zeros], "decrease right z"),
'u': (command_ik, ['left', [inc, 0, 0] + zeros], "increase left x"),
'j': (command_ik, ['left', [-inc, 0, 0] + zeros], "decrease left x"),
'i': (command_ik, ['left', [0, inc, 0] + zeros], "increase left y"),
'k': (command_ik, ['left', [0, -inc, 0] + zeros], "decrease left y"),
'o': (command_ik, ['left', [0, 0, inc] + zeros], "increase left z"),
'l': (command_ik, ['left', [0, 0, -inc] + zeros], "decrease left z"),
'z': (grip_right.close, [], "right: gripper close"),
'x': (grip_right.open, [], "right: gripper open"),
'c': (grip_right.calibrate, [], "right: gripper calibrate")
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "command_jacobian(left, [0.1, 0, 0])"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2], ))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
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 map_keyboard():
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
#These are from PyKDL and are needed for the Jacobian
left_kin = baxter_kinematics('left')
right_kin = baxter_kinematics('right')
#Connect with IK service
right_iksvc, right_ns = ik_command.connect_service('right')
left_iksvc, left_ns = ik_command.connect_service('left')
def command_jacobian(side, direction):
#UNUSED
if side == 'left':
limb = left
kin = left_kin
elif side == 'right':
limb = right
kin = right_kin
else:
raise Exception("Got wrong side in command_jacobian")
# current is the current position of end effector
current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation'])
current_p = current_q.reshape((7, 1))
current = np.zeros((6, 1))
quaternion_to_euler(current_p, current)
# We need to convert the direction from the world frame to the hand frame
# Rotate direction by the inverse of the rotation matrix from the angular pose in current
R = tf.transformations.quaternion_matrix(current_p[3:7].flatten().tolist())
R_inv = tf.transformations.inverse_matrix(R)
print R_inv[0:3, 0:3]
direction = np.array(direction).reshape((6, 1))
print direction[0:3]
dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3])
print dir_rot
#direction[0:3] = dir_rot
# Goal is the goal position, found by adding the requested direction from the user
goal = current + direction
current_angles = np.array([limb.joint_angles()[name] for name in limb.joint_names()])
# Get the Jacobian inverse and solve for the necessary change in joint angles
jacobian_inv = kin.jacobian_transpose()
delta = jacobian_inv*(goal-current)
commands = current_angles.flatten() + delta.flatten()
# Command the joints
command_list = commands.tolist()
joint_command = dict(zip(limb.joint_names(), command_list[0]))
limb.set_joint_positions(joint_command)
def command_ik(side, direction):
"""Use the Rethink IK service to figure out a desired joint position"""
if side == 'left':
limb = left
iksvc = left_iksvc
ns = left_ns
else:
limb = right
iksvc = right_iksvc
ns = right_ns
current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation'])
direction = np.array(direction)
desired_p = current_p + direction
ik_command.service_request(iksvc, desired_p, side)
zeros = [0]*4
inc = 0.1
bindings = {
'q': (command_ik, ['right', [inc, 0, 0]+zeros], "increase right x"),
'a': (command_ik, ['right', [-inc, 0, 0]+zeros], "decrease right x"),
'w': (command_ik, ['right', [0, inc, 0]+zeros], "increase right y"),
's': (command_ik, ['right', [0, -inc, 0]+zeros], "decrease right y"),
'e': (command_ik, ['right', [0, 0, inc]+zeros], "increase right z"),
'd': (command_ik, ['right', [0, 0, -inc]+zeros], "decrease right z"),
'u': (command_ik, ['left', [inc, 0, 0]+zeros], "increase left x"),
'j': (command_ik, ['left', [-inc, 0, 0]+zeros], "decrease left x"),
'i': (command_ik, ['left', [0, inc, 0]+zeros], "increase left y"),
'k': (command_ik, ['left', [0, -inc, 0]+zeros], "decrease left y"),
'o': (command_ik, ['left', [0, 0, inc]+zeros], "increase left z"),
'l': (command_ik, ['left', [0, 0, -inc]+zeros], "decrease left z"),
'z': (grip_right.close, [], "right: gripper close"),
'x': (grip_right.open, [], "right: gripper open"),
'c': (grip_right.calibrate, [], "right: gripper calibrate")
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "command_jacobian(left, [0.1, 0, 0])"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2],))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
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("visual_servo")
#Bit of cheating: move the robot's arms to a neutral position before running
dominant_joints = [
1.187301128466797, 1.942403170440674, 0.08206797205810547,
-0.996704015789795, -0.6734175651123048, 1.0266166411193849,
0.4985437554931641
]
off_joints = [
-1.1255584018249511,
1.4522963092712404,
0.6354515406555176,
-0.8843399232055664,
0.6327670742797852,
1.2751215284729005,
-0.4084223843078614,
]
names = ['e0', 'e1', 's0', 's1', 'w0', 'w1', 'w2']
off_limb = "right"
if limb == "right": off_limb = "left"
off_names = [off_limb + "_" + name for name in names]
jointlists = {}
jointlists[off_limb] = off_joints
jointlists[limb] = dominant_joints
for side in [limb, off_limb]:
limb_if = baxter_interface.Limb(side)
limb_names = [side + "_" + name for name in names]
joint_commands = dict(zip(limb_names, jointlists[side]))
limb_if.move_to_joint_positions(joint_commands)
#Calibrate gripper
gripper_if = baxter_interface.Gripper(limb)
if not gripper_if.calibrated():
print "Calibrating gripper"
gripper_if.calibrate()
iksvc, ns = ik_command.connect_service(limb)
command = VisualCommand(iksvc, limb)
command.subscribe()
command.publish()
while not rospy.is_shutdown():
command.handler_pub.publish(command.done)
command.pub_rate.sleep()
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 map_keyboard():
global desired_angles, inc, angle_inc
left = baxter_interface.Limb('left')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)qqqqqqqqqqqa
#These are from PyKDL and are needed for the Jacobian
left_kin = baxter_kinematics('left')
#Connect with IK service
left_iksvc, left_ns = ik_command.connect_service('left')
#Initialize desired angles from current angles
init_orient = left.endpoint_pose()['orientation']
quat = [0,0,0,0]
quat[3] = init_orient.w
quat[0] = init_orient.x
quat[1] = init_orient.y
quat[2] = init_orient.z
desired_angles = tf.transformations.euler_from_quaternion(quat)
#print init_orient
def command_jacobian(side, direction):
#UNUSED
if side == 'left':
limb = left
kin = left_kin
else:
raise Exception("Got wrong side in command_jacobian")
# current is the current position of end effector
current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation'])
current_p = current_q.reshape((7, 1))
current = np.zeros((6, 1))
quaternion_to_euler(current_p, current)
# We need to convert the direction from the world frame to the hand frame
# Rotate direction by the inverse of the rotation matrix from the angular pose in current
R = tf.transformations.quaternion_matrix(current_p[3:7].flatten().tolist())
R_inv = tf.transformations.inverse_matrix(R)
print R_inv[0:3, 0:3]
direction = np.array(direction).reshape((6, 1))
print direction[0:3]
dir_rot = np.dot(R_inv[0:3, 0:3], direction[0:3])
print dir_rot
#direction[0:3] = dir_rot
# Goal is the goal position, found by adding the requested direction from the user
goal = current + direction
current_angles = np.array([limb.joint_angles()[name] for name in limb.joint_names()])
# Get the Jacobian inverse and solve for the necessary change in joint angles
jacobian_inv = kin.jacobian_transpose()
delta = jacobian_inv*(goal-current)
commands = current_angles.flatten() + delta.flatten()
# Command the joints
command_list = commands.tolist()
joint_command = dict(zip(limb.joint_names(), command_list[0]))
limb.set_joint_positions(joint_command)
def command_ik(side, direction):
"""Use the Rethink IK service to figure out a desired joint position"""
if side == 'left':
limb = left
iksvc = left_iksvc
ns = left_ns
current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation'])
direction = np.array(direction)
print direction
desired_p = current_p + direction
ik_command.service_request(iksvc, desired_p, side)
zeros = [0]*4
def update_angles(change_angles):
global desired_angles
current_angles = np.array(desired_angles)
change_angles = np.array(change_angles)
#Note that adding np arrays does element-wise summation, but adding python lists concatenates them
new_angles = current_angles + change_angles
#Check that the angles are in bounds, angles are in radians
#for value in new_angles:
#Wrap crossing zero towards 2*pi
# if value < 0:
# value = (2* np.pi) + value
#Wrap crossing 2*pi towards zero
# if value > (2 * np.pi):
# value = value - (2*np.pi)
#Convert to a python array and store in the desired angles
desired_angles = new_angles.tolist()
#Convert the desired angles to a quaternion
quat = tf.transformations.quaternion_from_euler(desired_angles[0], desired_angles[1], desired_angles[2])
#This only does the left side, because the scooter only has a left arm
limb = left
current_p = np.array(limb.endpoint_pose()['position']+limb.endpoint_pose()['orientation'])
current_p[3:7] = quat
ik_command.service_request(left_iksvc, current_p, "left")
def update_inc(isSmall = True):
global inc, angle_inc
if isSmall:
inc = 0.1
angle_inc = 0.1
else:
inc = 0.5
angle_inc = 0.2
def toggle_gripper():
if grip_left.position() > 95:
#gripper is open, so close it. Can be confused by a big object, though.
print "Closing gripper"
grip_left.close()
elif grip_left.gripping() or grip_left.position() < 5:
print "Opening gripper"
#gripper is closed on something, or on nothing
grip_left.open()
else:
print "Unknown state, opening gripper"
#Unknown state, so open the gripper
grip_left.open()
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
#This had to be moved into the loop so the increment could be rebound when
#the -/= keys are used to change the increment between small and large
bindings = {
'u': (command_ik, ['left', [inc, 0, 0]+zeros], "increase left x"),
'j': (command_ik, ['left', [-inc, 0, 0]+zeros], "decrease left x"),
'i': (command_ik, ['left', [0, inc, 0]+zeros], "increase left y"),
'k': (command_ik, ['left', [0, -inc, 0]+zeros], "decrease left y"),
'o': (command_ik, ['left', [0, 0, inc]+zeros], "increase left z"),
'l': (command_ik, ['left', [0, 0, -inc]+zeros], "decrease left z"),
'q': (update_angles, [[angle_inc, 0, 0]], "increase left roll"),
'a': (update_angles, [[-angle_inc, 0, 0]], "decrease left roll"),
'w': (update_angles, [[0,angle_inc,0]], "increase left pitch"),
's': (update_angles, [[0,-angle_inc,0]], "decrease left pitch"),
'e': (update_angles, [[0,0,angle_inc]], "increase left yaw"),
'd': (update_angles, [[0,0,-angle_inc]], "decrease left yaw"),
'-': (update_inc, [True], "small motion increments"),
'=': (update_inc, [False], "large motion increments"),
' ': (toggle_gripper, [], "toggle gripper state")
}
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "command_jacobian(left, [0.1, 0, 0])"
cmd[0](*cmd[1])
print("command: %s with params %s" % (cmd[2], cmd[1]))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
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:])
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:
#!/usr/bin/env python
import baxter_interface
import rospy
import copy
from baxter_interface import CHECK_VERSION
import ik_command
import baxter_coordinates
right_iksvc, right_ns = ik_command.connect_service('right')
left_iksvc, left_ns = ik_command.connect_service('left')
delta_z = 0.3
def pick(side, pos):
if side not in ["left", "right"]:
raise Exception("Invalid side " + side)
pos_map = baxter_coordinates.coords[side]
if pos not in pos_map:
raise Exception(side + " can not reach " + str(pos))
gripper = baxter_interface.Gripper(side, CHECK_VERSION)
coords = pos_map[pos]
pre_grasp_coords = {}
pre_grasp_coords['position'] = baxter_interface.limb.Limb.Point(
x=coords['position'].x,
y=coords['position'].y,
z=coords['position'].z + delta_z)
pre_grasp_coords['orientation'] = coords['orientation']
