From 873434c298bfc1c4d2bc23c7db00fc51437f48e2 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Fri, 4 Aug 2023 01:25:43 -0700 Subject: [PATCH] Add vel ctrl to head_pan cmd group --- stretch_core/nodes/command_groups.py | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index a8502dd..7b7bb5c 100644 --- a/stretch_core/nodes/command_groups.py +++ b/stretch_core/nodes/command_groups.py @@ -30,17 +30,25 @@ class HeadPanCommandGroup(SimpleCommandGroup): self.range = range_rad def init_execution(self, robot, robot_status, **kwargs): + robot_mode = kwargs['robot_mode'] if self.active: - _, pan_error = self.update_execution(robot_status) - robot.head.move_by('head_pan', pan_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration']) - self.looked_left = pan_error > 0.0 + _, pan_error = self.update_execution(robot_status, robot_mode=robot_mode) + if robot_mode == 'position': + robot.head.move_by('head_pan', pan_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration']) + self.looked_left = pan_error > 0.0 + elif robot_mode == 'velocity': + robot.head.get_joint('head_pan').set_velocity(self.goal['velocity'], a_des=self.goal['acceleration']) def update_execution(self, robot_status, **kwargs): + robot_mode = kwargs['robot_mode'] self.error = None if self.active: - pan_backlash_correction = self.looked_left_offset_rad if self.looked_left else 0.0 - pan_current = robot_status['head']['head_pan']['pos'] + self.calibrated_offset_rad + pan_backlash_correction - self.error = self.goal['position'] - pan_current + if robot_mode == 'position': + pan_backlash_correction = self.looked_left_offset_rad if self.looked_left else 0.0 + pan_current = robot_status['head']['head_pan']['pos'] + self.calibrated_offset_rad + pan_backlash_correction + self.error = self.goal['position'] - pan_current + elif robot_mode == 'velocity': + self.error = 0.0 return self.name, self.error return None