From b1f4ceb5113449454b39d1e242f3e2c0fc781be5 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Sat, 5 Mar 2022 19:02:22 -0800 Subject: [PATCH] funmap changes 2 --- stretch_funmap/src/stretch_funmap/mapping.py | 21 ++++---------------- 1 file changed, 4 insertions(+), 17 deletions(-) diff --git a/stretch_funmap/src/stretch_funmap/mapping.py b/stretch_funmap/src/stretch_funmap/mapping.py index 9349295..5609ab3 100644 --- a/stretch_funmap/src/stretch_funmap/mapping.py +++ b/stretch_funmap/src/stretch_funmap/mapping.py @@ -19,21 +19,8 @@ import stretch_funmap.segment_max_height_image as sm def stow_and_lower_arm(node): - pose = {'joint_gripper_finger_left': -0.15} - node.move_to_pose(pose) - pose = {'wrist_extension': 0.01} - node.move_to_pose(pose) - - # gripper backwards stow - pose = {'joint_wrist_yaw': 3.3} - - # gripper forward stow needs a better forward range of motion to work well - node.move_to_pose(pose) - - # avoid blocking the laser range finder with the gripper - pose = {'joint_lift': 0.22} - node.move_to_pose(pose) - return 'lowered' + pose = {'joint_gripper_finger_left': -0.15, 'wrist_extension': 0.0, 'joint_wrist_yaw': 3.4, 'joint_lift': 0.2} + return node.move_to_pose(pose) def draw_robot_pose(robot_xya_pix, image, m_per_pix, color=(0, 0, 255)): @@ -356,9 +343,9 @@ class HeadScan: pose = {'joint_head_pan': far_right_pan, 'joint_head_tilt': head_tilt} node.move_to_pose(pose) - pan_left = np.linspace(far_right_pan, far_left_pan, num_pan_steps) + pan_stops = np.linspace(far_right_pan, far_left_pan, num_pan_steps) - for pan_ang in pan_left: + for pan_ang in pan_stops: pose = {'joint_head_pan': pan_ang} self.capture_point_clouds(node, pose, capture_params)