diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index b7f23a8..6e7ad40 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -347,8 +347,8 @@ class ArmCommandGroup(SimpleCommandGroup): robot.arm.move_by(extension_error_m, v_m=self.goal['velocity'], a_m=self.goal['acceleration'], - contact_thresh_pos_N=self.goal['contact_threshold'], - contact_thresh_neg_N=-self.goal['contact_threshold'] \ + contact_thresh_pos=self.goal['contact_threshold'], + contact_thresh_neg=-self.goal['contact_threshold'] \ if self.goal['contact_threshold'] is not None else None) self.retracted = extension_error_m < 0.0 @@ -392,8 +392,8 @@ class LiftCommandGroup(SimpleCommandGroup): robot.lift.move_by(self.update_execution(robot_status)[1], v_m=self.goal['velocity'], a_m=self.goal['acceleration'], - contact_thresh_pos_N=self.goal['contact_threshold'], - contact_thresh_neg_N=-self.goal['contact_threshold'] \ + contact_thresh_pos=self.goal['contact_threshold'], + contact_thresh_neg=-self.goal['contact_threshold'] \ if self.goal['contact_threshold'] is not None else None) def update_execution(self, robot_status, **kwargs): @@ -556,17 +556,17 @@ class MobileBaseCommandGroup(SimpleCommandGroup): robot.base.translate_by(mobile_base_error_m, v_m=self.goal_translate_mobile_base['velocity'], a_m=self.goal_translate_mobile_base['acceleration'], - contact_thresh_N=self.goal_translate_mobile_base['contact_threshold']) + contact_thresh=self.goal_translate_mobile_base['contact_threshold']) elif mobile_base_error_rad is not None: robot.base.rotate_by(mobile_base_error_rad, v_r=self.goal_rotate_mobile_base['velocity'], a_r=self.goal_rotate_mobile_base['acceleration'], - contact_thresh_N=self.goal_rotate_mobile_base['contact_threshold']) + contact_thresh=self.goal_rotate_mobile_base['contact_threshold']) else: robot.base.translate_by(self.update_execution(robot_status)[1], v_m=self.goal['velocity'], a_m=self.goal['acceleration'], - contact_thresh_N=self.goal['contact_threshold']) + contact_thresh=self.goal['contact_threshold']) def update_execution(self, robot_status, **kwargs): success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 49cfa77..95f5c87 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -6,6 +6,7 @@ import threading from rwlock import RWLock import stretch_body.robot as rb from stretch_body.hello_utils import ThreadServiceExit +import stretch_body import tf2_ros import tf_conversions @@ -338,6 +339,10 @@ class StretchBodyNode: rospy.loginfo("{0} started".format(self.node_name)) + if int(stretch_body.__version__.split('.')[1]) < 4: + rospy.logerr("ERROR: Found old stretch_body version. Please upgrade stretch_body to 0.4.0 or above.") + rospy.signal_shutdown('Found old stretch_body version.') + self.robot = rb.Robot() self.robot.startup() diff --git a/stretch_demos/nodes/clean_surface b/stretch_demos/nodes/clean_surface index 7590fa9..55aedd9 100755 --- a/stretch_demos/nodes/clean_surface +++ b/stretch_demos/nodes/clean_surface @@ -135,8 +135,8 @@ class CleanSurfaceNode(hm.HelloNode): self.lower_tool_until_contact() 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 = 33.7 #effort_pct #20.0 from funmap + extension_contact_effort = 16.45 #effort_pct #40.0 from funmap pose = {'joint_lift': (lift_m, lift_contact_effort)} self.move_to_pose(pose, custom_contact_thresholds=True) diff --git a/stretch_demos/nodes/hello_world b/stretch_demos/nodes/hello_world index cde2ab4..34bf832 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 = 18.5 #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 = 32.9 #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..9eedcfb 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 = 18.5 #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 = 32.5 #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 = 41.4 #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 = 64.4 #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