From 38aec0ffd2b10552ac8cd55bc6bfa50509b37c2d Mon Sep 17 00:00:00 2001 From: Charlie Kemp <31106448+hello-ck@users.noreply.github.com> Date: Fri, 3 Jul 2020 10:13:10 -0400 Subject: [PATCH] used for the rest of the drawer opening autonomy videos --- stretch_demos/nodes/open_drawer | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/stretch_demos/nodes/open_drawer b/stretch_demos/nodes/open_drawer index a37076c..412cb1f 100755 --- a/stretch_demos/nodes/open_drawer +++ b/stretch_demos/nodes/open_drawer @@ -67,10 +67,20 @@ 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 = 18.0 #20.0 #20.0 from funmap + lift_contact_effort = 16.0 #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) + use_correction = True + if use_correction: + # raise due to drop down after contact detection + #rospy.sleep(0.5) # wait for new lift position + rospy.sleep(0.2) # wait for new lift position + lift_m = self.lift_position + 0.015 + pose = {'joint_lift': lift_m} + self.move_to_pose(pose) + rospy.sleep(0.2) # wait for new lift position + def raise_hook_until_contact(self): rospy.loginfo('raise_hook_until_contact') max_raise_m = 0.15 @@ -108,7 +118,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 = 100.0 #40.0 from funmap + extension_contact_effort = 120 #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