def init_params(self):
try:
self.grasp_height = get_ros_param(
"GRASP_HEIGHT", "Grasp height defaulting to 0.01")
self.drop_height = get_ros_param("DROP_HEIGHT",
"Drop height defaulting to 0.07")
self.cruising_height = get_ros_param(
"CRUISING_HEIGHT", "Cruising height defaulting to 0.1")
self.pointing_height = get_ros_param(
"POINT_HEIGHT", "Pointing height defaulting to 0.06")
except ValueError as e:
rospy.loginfo(e)
def init_service_clients(self):
self.is_simulation = False
try:
self.is_simulation = get_ros_param("IS_SIMULATION", "")
except:
self.is_simulation = False
if self.is_simulation is True:
# setup alternatives to jaco services for emergency stop, joint control, and finger control
pass
# Service to get TUI State
rospy.wait_for_service('get_tui_blocks')
self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState)
# Service for homing the arm
home_arm_service = '/j2s7s300_driver/in/home_arm'
self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm)
rospy.loginfo('Waiting for kinova home arm service')
rospy.wait_for_service(home_arm_service)
rospy.loginfo('Kinova home arm service server connected')
# Service for emergency stop
emergency_service = '/j2s7s300_driver/in/stop'
self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop)
rospy.loginfo('Waiting for Stop service')
rospy.wait_for_service(emergency_service)
rospy.loginfo('Stop service server connected')
# Service for restarting the arm
start_service = '/j2s7s300_driver/in/start'
self.restart_arm = rospy.ServiceProxy(start_service, Start)
rospy.loginfo('Waiting for Start service')
rospy.wait_for_service(start_service)
rospy.loginfo('Start service server connected')
def set_workspace_bounds(self, bounds=None):
if bounds is not None:
self.orbits_min = bounds["ORBITS_MIN"]
self.orbits_max = bounds["ORBITS_MAX"]
self.orbits_left = bounds["ORBITS_LEFT"]
self.orbits_right = bounds["ORBITS_RIGHT"]
else:
self.orbits_min = get_ros_param("ORBITS_MIN", "Orbit boundaries must be configured.")
self.orbits_max = get_ros_param("ORBITS_MAX", "Orbit boundaries must be configured.")
self.orbits_left = get_ros_param("ORBITS_LEFT", "Orbit boundaries must be configured.")
self.orbits_right = get_ros_param("ORBITS_RIGHT", "Orbit boundaries must be configured.")
self.orbit_height = (self.orbits_max-self.orbits_min)/self.NUM_ORBITS
self.orbits = []
for i in range(self.NUM_ORBITS):
#lower_bound = self.orbits_max-(i+1)*self.orbit_height
lower_bound = self.orbits_min+i*self.orbit_height
#upper_bound = self.orbits_max-i*self.orbit_height
upper_bound = self.orbits_min+(i+1)*self.orbit_height
self.orbits.append([lower_bound, upper_bound])
def get_zone(self, zone_id):
orbits_min = get_ros_param('ORBITS_MIN')
table_left = get_ros_param('TUIO_X_MAX')
table_height = get_ros_param('TUIO_Y_MAX') - get_ros_param(
'TUIO_Y_MIN')
table_midpoint_y = get_ros_param('TUIO_Y_MIN') + table_height / 2
orbits_left = get_ros_param('ORBITS_LEFT')
orbit_width = get_ros_param('ORBITS_LEFT') - get_ros_param(
'ORBITS_RIGHT')
orbit_height = (get_ros_param('ORBITS_MAX') -
get_ros_param('ORBITS_MIN')) / 5
orbits_midpoint_x = get_ros_param('ORBITS_LEFT') - orbit_width / 2
staging_right = orbits_left + get_ros_param('STAGING_BUFFER_WIDTH')
staging_width = table_left - staging_right
horizontal_scale = [1.0, 1.0, 1.0, 1.0, 1.0]
if zone_id >= 0 and zone_id < 5:
# return an orbit
return [{
'x': orbits_midpoint_x * horizontal_scale[zone_id],
'y': orbits_min + (2 * zone_id + 1) * orbit_height / 2
}, {
'x_tolerance': horizontal_scale[zone_id] * orbit_width / 2,
'y_tolerance': orbit_height / 2
}]
elif zone_id == -1:
# return staging
return [{
'x': table_left - staging_width / 2,
'y': table_midpoint_y
}, {
'x_tolerance': staging_width / 2,
'y_tolerance': table_height / 2
}]
else:
raise Exception("Did not get a valid zone.")
def move_block(self,
block_id,
pick_x,
pick_y,
pick_x_tolerance,
pick_y_tolerance,
place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
block_size=None,
safe_zone=None,
pick_tries=2,
place_tries=1,
point_at_block=True):
if block_size is None:
block_size = get_ros_param('DEFAULT_BLOCK_SIZE')
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
# check for a drop location before trying to pick, do this before checking source to prevent cases where we go for a block user
# moved while we are checking for a drop location
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
None, None,
Exception("no room in the target zone to drop the block"))
# here we are actually building a set of candidate blocks to pick
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
# select best block to pick and pick up
pick_location = None
if len(candidate_blocks) < 1:
raise Exception("no block of id " + str(block_id) +
" found within the source zone")
elif len(candidate_blocks) == 1:
pick_location = Point(candidate_blocks[0]['x'],
candidate_blocks[0]['y'], 0)
else:
pick_location = Point(*self.find_most_isolated_block(
candidate_blocks, current_block_state))
if point_at_block == True:
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.close_gripper()
self.point_at_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0))
self.point_at_block(location=Point(arm_drop_location[0],
arm_drop_location[1], 0))
self.home_arm()
except Exception as e:
rospy.loginfo(str(e))
rospy.loginfo("failed trying to point at block. giving up.")
self.home_arm()
self.move_block_server.set_succeeded(
MoveBlockResult(drop_location))
return
for i in range(pick_tries):
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.pick_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0),
check_grasp=True)
break
except Exception as e:
if i < pick_tries - 1:
rospy.loginfo("pick failed and trying again..." + str(e))
else:
rospy.loginfo("pick failed and out of attempts..." +
str(e))
self.handle_pick_failure(e)
if safe_zone == None:
if self.safe_zone == None:
safe_zone = [{
'x': pick_x,
'y': pick_y
}, {
'x_tolerance': pick_x_tolerance,
'y_tolerance': pick_y_tolerance
}]
else:
safe_zone = self.safe_zone
# calculate drop location
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
safe_zone, block_size,
Exception("no room in the target zone to drop the block"))
rospy.loginfo("tuio drop" + str(drop_location))
for i in range(place_tries):
try:
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
break
except Exception as e:
if i < place_tries - 1:
rospy.loginfo("place failed and trying again..." + str(e))
else:
rospy.loginfo("place failed and out of attempts..." +
str(e))
# check to see if we've defined a safe zone to drop the blocks
self.handle_place_failure(safe_zone, block_size, e)
# assume success if we made it this far
self.move_block_server.set_succeeded(MoveBlockResult(drop_location))
def init_params(self):
try:
self.grasp_height = get_ros_param("GRASP_HEIGHT", "Grasp height defaulting to 0.1")
self.drop_height = get_ros_param("DROP_HEIGHT", "Drop height defaulting to 0.2")
except ValueError as e:
rospy.loginfo(e)