From 75f1f168d8d47af20696447dc0eea7c7b14a9f64 Mon Sep 17 00:00:00 2001 From: Charlie Kemp <31106448+hello-ck@users.noreply.github.com> Date: Wed, 24 Jun 2020 11:51:55 -0400 Subject: [PATCH 1/3] simple keyboard teleoperation launch file --- stretch_core/launch/keyboard_teleop.launch | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 stretch_core/launch/keyboard_teleop.launch diff --git a/stretch_core/launch/keyboard_teleop.launch b/stretch_core/launch/keyboard_teleop.launch new file mode 100644 index 0000000..c68713a --- /dev/null +++ b/stretch_core/launch/keyboard_teleop.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + From 64b17dfcaba6c5384678282b2a20b9606007500e Mon Sep 17 00:00:00 2001 From: Charlie Kemp <31106448+hello-ck@users.noreply.github.com> Date: Wed, 24 Jun 2020 18:39:14 -0400 Subject: [PATCH 2/3] object grasping demo improvements --- stretch_demos/nodes/grasp_object | 32 +++++++++++++------ .../stretch_funmap/manipulation_planning.py | 2 +- 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index 9b28adb..d1b2eb0 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -101,6 +101,9 @@ class GraspObjectNode(hm.HelloNode): def trigger_grasp_object_callback(self, request): actually_move = True + max_lift_m = 1.09 + min_extension_m = 0.01 + max_extension_m = 0.5 use_default_mode = False if use_default_mode: @@ -137,7 +140,8 @@ class GraspObjectNode(hm.HelloNode): if actually_move: rospy.loginfo('Raise tool to pregrasp height.') - lift_to_pregrasp_m = self.lift_position + pregrasp_lift_m + lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1) + lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m) pose = {'joint_lift': lift_to_pregrasp_m} self.move_to_pose(pose) @@ -147,7 +151,6 @@ class GraspObjectNode(hm.HelloNode): if actually_move: rospy.loginfo('Rotate the gripper for grasping.') - lift_to_pregrasp_m = self.lift_position + pregrasp_lift_m pose = {'joint_wrist_yaw': pregrasp_yaw} self.move_to_pose(pose) @@ -168,12 +171,13 @@ class GraspObjectNode(hm.HelloNode): self.drive(pregrasp_mobile_base_m) if pregrasp_wrist_extension_m > 0.0: + extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m) + extension_m = min(extension_m, max_extension_m) rospy.loginfo('Extend tool above surface.') - pose = {'wrist_extension': self.wrist_position + pregrasp_wrist_extension_m} + pose = {'wrist_extension': extension_m} self.move_to_pose(pose) else: print('negative wrist extension for pregrasp, so not extending or retracting.') - # sleep to make sure the joint poses are up to date - this # should be changed to look at time stamps of the joints... @@ -183,9 +187,16 @@ class GraspObjectNode(hm.HelloNode): if actually_move: rospy.loginfo('Move the grasp pose from the pregrasp pose.') + + lift_m = max(self.lift_position + grasp_lift_m, 0.1) + lift_m = min(lift_m, max_lift_m) + + extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m) + extension_m = min(extension_m, max_extension_m) + pose = {'translate_mobile_base': grasp_mobile_base_m, - 'joint_lift': self.lift_position + grasp_lift_m, - 'wrist_extension': self.wrist_position + grasp_wrist_extension_m} + 'joint_lift': lift_m, + 'wrist_extension': extension_m} self.move_to_pose(pose) rospy.loginfo('Attempt to close the gripper on the object.') @@ -200,10 +211,11 @@ class GraspObjectNode(hm.HelloNode): rospy.loginfo('Attempt to lift the object.') object_lift_height_m = 0.1 - lift_height_m = self.lift_position + object_lift_height_m - if lift_height_m > 0.94: - lift_height_m = 0.94 - pose = {'joint_lift': lift_height_m} + + lift_m = max(self.lift_position + object_lift_height_m, 0.2) + lift_m = min(lift_m, max_lift_m) + + pose = {'joint_lift': lift_m} self.move_to_pose(pose) rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.') diff --git a/stretch_funmap/src/stretch_funmap/manipulation_planning.py b/stretch_funmap/src/stretch_funmap/manipulation_planning.py index fa786c8..5591941 100755 --- a/stretch_funmap/src/stretch_funmap/manipulation_planning.py +++ b/stretch_funmap/src/stretch_funmap/manipulation_planning.py @@ -252,7 +252,7 @@ class ManipulationView(): def move_head(self, move_to_pose): tilt = -0.8 - pan = -1.5 + pan = -1.6 # This head configuration can reduce seeing the hand or arm when they are held high, which can avoid noise due to the hand and arm being to close to the head. #tilt = -0.6 #pan = -0.9 From e43621b33faf57eb61f36d929c40b02d4949d95d Mon Sep 17 00:00:00 2001 From: Charlie Kemp <31106448+hello-ck@users.noreply.github.com> Date: Wed, 24 Jun 2020 22:41:39 -0400 Subject: [PATCH 3/3] grasp_object code used for new autonomy video --- stretch_demos/nodes/grasp_object | 14 ++------------ .../src/stretch_funmap/manipulation_planning.py | 2 +- 2 files changed, 3 insertions(+), 13 deletions(-) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index d1b2eb0..cb8bb62 100755 --- a/stretch_demos/nodes/grasp_object +++ b/stretch_demos/nodes/grasp_object @@ -73,7 +73,7 @@ class GraspObjectNode(hm.HelloNode): def look_at_surface(self, scan_time_s=None): self.manipulation_view = mp.ManipulationView(self.tf2_buffer, self.debug_directory) manip = self.manipulation_view - head_settle_time_s = 1.0 + head_settle_time_s = 0.02 #1.0 manip.move_head(self.move_to_pose) rospy.sleep(head_settle_time_s) if scan_time_s is None: @@ -120,10 +120,6 @@ class GraspObjectNode(hm.HelloNode): rospy.loginfo('Reorient the wrist.') pose = {'joint_wrist_yaw': 0.0} self.move_to_pose(pose) - - # sleep to make sure the joint poses are up to date - this - # should be changed to look at time stamps of the joints... - rospy.sleep(1.0) self.look_at_surface(scan_time_s = 3.0) @@ -158,9 +154,6 @@ class GraspObjectNode(hm.HelloNode): pose = {'gripper_aperture': 0.125} self.move_to_pose(pose) - # sleep to make sure the joint poses are up to date - this - # should be changed to look at time stamps of the joints... - rospy.sleep(1.0) pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer) print('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m)) @@ -178,10 +171,7 @@ class GraspObjectNode(hm.HelloNode): self.move_to_pose(pose) else: print('negative wrist extension for pregrasp, so not extending or retracting.') - - # sleep to make sure the joint poses are up to date - this - # should be changed to look at time stamps of the joints... - rospy.sleep(1.0) + grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer) print('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m)) diff --git a/stretch_funmap/src/stretch_funmap/manipulation_planning.py b/stretch_funmap/src/stretch_funmap/manipulation_planning.py index 5591941..a7063d7 100755 --- a/stretch_funmap/src/stretch_funmap/manipulation_planning.py +++ b/stretch_funmap/src/stretch_funmap/manipulation_planning.py @@ -252,7 +252,7 @@ class ManipulationView(): def move_head(self, move_to_pose): tilt = -0.8 - pan = -1.6 + pan = -1.8 #-1.6 # This head configuration can reduce seeing the hand or arm when they are held high, which can avoid noise due to the hand and arm being to close to the head. #tilt = -0.6 #pan = -0.9