diff --git a/stretch_demos/nodes/clean_surface b/stretch_demos/nodes/clean_surface index 7590fa9..eb41b9e 100755 --- a/stretch_demos/nodes/clean_surface +++ b/stretch_demos/nodes/clean_surface @@ -26,7 +26,6 @@ import hello_helpers.hello_misc as hm import stretch_funmap.navigate as nv import stretch_funmap.manipulation_planning as mp - class CleanSurfaceNode(hm.HelloNode): def __init__(self): @@ -40,18 +39,18 @@ class CleanSurfaceNode(hm.HelloNode): self.lift_position = None self.manipulation_view = None self.debug_directory = None - + def joint_states_callback(self, joint_states): - with self.joint_states_lock: + with self.joint_states_lock: self.joint_states = joint_states wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(joint_states) self.wrist_position = wrist_position lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states) self.lift_position = lift_position - + def lower_tool_until_contact(self): rospy.loginfo('lower_tool_until_contact') - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = self.trigger_lower_until_contact_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) @@ -91,15 +90,15 @@ class CleanSurfaceNode(hm.HelloNode): manip.save_scan(dirname + filename) else: rospy.loginfo('CleanSurfaceNode: No debug directory provided, so debugging data will not be saved.') - + def trigger_clean_surface_callback(self, request): tool_width_m = 0.08 tool_length_m = 0.08 - step_size_m = 0.04 + step_size_m = 0.04 min_extension_m = 0.01 max_extension_m = 0.5 - + self.look_at_surface() strokes, simple_plan, lift_to_surface_m = self.manipulation_view.get_surface_wiping_plan(self.tf2_buffer, tool_width_m, tool_length_m, step_size_m) @@ -107,10 +106,10 @@ class CleanSurfaceNode(hm.HelloNode): if True and (strokes is not None) and (len(strokes) > 0): - if (self.lift_position is not None) and (self.wrist_position is not None): + if (self.lift_position is not None) and (self.wrist_position is not None): above_surface_m = 0.1 - + lift_above_surface_m = self.lift_position + lift_to_surface_m + above_surface_m pose = {'joint_lift': lift_above_surface_m} rospy.loginfo('Raise tool above surface.') @@ -122,7 +121,7 @@ class CleanSurfaceNode(hm.HelloNode): rospy.loginfo('Drive to the start of the cleaning region.') self.move_to_pose(pose) - initial_wrist_position = self.wrist_position + initial_wrist_position = self.wrist_position extension_m = start['start_wrist_extension_m'] start_extension_m = initial_wrist_position + extension_m @@ -131,15 +130,15 @@ class CleanSurfaceNode(hm.HelloNode): self.move_to_pose(pose) use_old_code = False - if use_old_code: + if use_old_code: self.lower_tool_until_contact() - else: + else: lift_m = self.lift_position - (above_surface_m - 0.02) - lift_contact_effort = 20.0 #20.0 from funmap - extension_contact_effort = 40.0 #40.0 from funmap + lift_contact_effort = 0.061 #effort_pct 20.0 pseudo_N from funmap + extension_contact_effort = 0.164 #effort_pct 40.0 pseudo_N from funmap pose = {'joint_lift': (lift_m, lift_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) - + use_correction = True if use_correction: # raise due to drop down after contact detection @@ -198,15 +197,15 @@ class CleanSurfaceNode(hm.HelloNode): message='Completed surface cleaning!' ) - + def main(self): hm.HelloNode.main(self, 'clean_surface', 'clean_surface', wait_for_first_pointcloud=False) - + self.debug_directory = rospy.get_param('~debug_directory') rospy.loginfo('Using the following directory for debugging files: {0}'.format(self.debug_directory)) self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback) - + self.trigger_clean_surface_service = rospy.Service('/clean_surface/trigger_clean_surface', Trigger, self.trigger_clean_surface_callback) @@ -228,12 +227,12 @@ class CleanSurfaceNode(hm.HelloNode): rospy.wait_for_service(high_accuracy_service) rospy.loginfo('Node ' + self.node_name + ' connected to' + high_accuracy_service) self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger) - + rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): rate.sleep() - + if __name__ == '__main__': try: parser = ap.ArgumentParser(description='Clean Surface behavior for stretch.') diff --git a/stretch_demos/nodes/hello_world b/stretch_demos/nodes/hello_world index cde2ab4..8b138dc 100755 --- a/stretch_demos/nodes/hello_world +++ b/stretch_demos/nodes/hello_world @@ -69,7 +69,7 @@ class HelloWorldNode(hm.HelloNode): wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(self.joint_states) extension_m = wrist_position + max_reach_m extension_m = min(extension_m, max_extension_m) - extension_contact_effort = 45.0 + extension_contact_effort = 0.185 # effort_pct pose = {'wrist_extension': (extension_m, extension_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) @@ -83,7 +83,7 @@ class HelloWorldNode(hm.HelloNode): extension_m = wrist_position - backoff_m extension_m = min(extension_m, max_extension_m) extension_m = max(min_extension_m, extension_m) - extension_contact_effort = 80.0 # to avoid stopping due to contact + extension_contact_effort = 0.329 #effort_pct # to avoid stopping due to contact pose = {'wrist_extension': (extension_m, extension_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) rospy.sleep(1.0) # Give it time to backoff before the next move. Otherwise it may not backoff. diff --git a/stretch_demos/nodes/open_drawer b/stretch_demos/nodes/open_drawer index 6ecc2b2..3f7aea3 100755 --- a/stretch_demos/nodes/open_drawer +++ b/stretch_demos/nodes/open_drawer @@ -57,7 +57,7 @@ class OpenDrawerNode(hm.HelloNode): max_reach_m = 0.4 extension_m = self.wrist_position + max_reach_m extension_m = min(extension_m, max_extension_m) - extension_contact_effort = 45.0 #42.0 #40.0 from funmap + extension_contact_effort = 0.185 #effort_pct #42.0 #40.0 from funmap pose = {'wrist_extension': (extension_m, extension_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) @@ -65,7 +65,7 @@ class OpenDrawerNode(hm.HelloNode): rospy.loginfo('lower_hook_until_contact') max_drop_m = 0.15 lift_m = self.lift_position - max_drop_m - lift_contact_effort = 16.0 #18.0 #20.0 #20.0 from funmap + lift_contact_effort = 0.049 #effort_pct #18.0 #20.0 #20.0 from funmap pose = {'joint_lift': (lift_m, lift_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) @@ -82,7 +82,7 @@ class OpenDrawerNode(hm.HelloNode): rospy.loginfo('raise_hook_until_contact') max_raise_m = 0.15 lift_m = self.lift_position + max_raise_m - lift_contact_effort = 45.0 + lift_contact_effort = 0.138 #effort_pct pose = {'joint_lift': (lift_m, lift_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) @@ -113,7 +113,7 @@ class OpenDrawerNode(hm.HelloNode): extension_m = self.wrist_position - 0.2 extension_m = min(extension_m, max_extension_m) extension_m = max(0.01, extension_m) - extension_contact_effort = 120 #100.0 #40.0 from funmap + extension_contact_effort = 0.493 #effort_pct #100.0 #40.0 from funmap pose = {'wrist_extension': (extension_m, extension_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) return True