diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 5f8189d..d27d4aa 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -124,7 +124,7 @@ class HelloNode: else: pose_correct = all([isinstance(joint_position, numbers.Real) for joint_position in pose.values()]) if not pose_correct: - rospy.logerr(f"{self.node_name}'s HelloNode.move_to_pose: Not sending trajectory due to improper pose. The default option requires 1 value, pose_target as integer, for each joint name, but pose = {pose}") + rospy.logerr(f"{self.node_name}'s HelloNode.move_to_pose: Not sending trajectory due to improper pose. The default option requires 1 value, pose_target as integer/float, for each joint name, but pose = {pose}") return point.positions = [joint_position for joint_position in pose.values()] @@ -134,6 +134,35 @@ class HelloNode: self.trajectory_client.wait_for_result() rospy.logdebug(f"{self.node_name}'s HelloNode.move_to_pose: got result {self.trajectory_client.get_result()}") + def move_at_speed(self, speed, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False): + if self.dryrun: + return + + point = JointTrajectoryPoint() + point.time_from_start = rospy.Duration(0.0) + trajectory_goal = FollowJointTrajectoryGoal() + trajectory_goal.goal_time_tolerance = rospy.Time(1.0) + trajectory_goal.trajectory.header.stamp = rospy.Time.now() + trajectory_goal.trajectory.joint_names = list(speed.keys()) + trajectory_goal.trajectory.points = [point] + + # populate goal + if custom_full_goal: + raise NotImplementedError # TODO + elif custom_contact_thresholds: + raise NotImplementedError # TODO + else: + speed_correct = all([isinstance(joint_velocity, numbers.Real) for joint_velocity in speed.values()]) + if not speed_correct: + rospy.logerr(f"{self.node_name}'s HelloNode.move_at_speed: Not sending trajectory due to improper speed goal. The default option requires 1 value, speed_target as interger/float, for each joint name, but speed = {speed}") + return + point.velocities = [joint_velocity for joint_velocity in speed.values()] + + self.trajectory_client.send_goal(trajectory_goal) + if not return_before_done: + self.trajectory_client.wait_for_result() + rospy.logdebug(f"{self.node_name}'s HelloNode.move_at_speed: got result {self.trajectory_client.get_result()}") + def get_robot_floor_pose_xya(self, floor_frame='odom'): # Returns the current estimated x, y position and angle of the # robot on the floor. This is typically called with respect to diff --git a/hello_helpers/src/hello_helpers/simple_command_group.py b/hello_helpers/src/hello_helpers/simple_command_group.py index 2f2c23b..bfb8b35 100644 --- a/hello_helpers/src/hello_helpers/simple_command_group.py +++ b/hello_helpers/src/hello_helpers/simple_command_group.py @@ -103,21 +103,28 @@ class SimpleCommandGroup: bool False if commanded goal invalid, else True """ + robot_mode = kwargs['robot_mode'] self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None} if self.active: goal_pos = point.positions[self.index] if len(point.positions) > self.index else None - if goal_pos is None: + if robot_mode == 'position' and goal_pos is None: err_str = ("Received goal point with positions array length={0}. " "This joint ({1})'s index is {2}. Length of array must cover all joints listed " "in commanded_joint_names.").format(len(point.positions), self.name, self.index) invalid_goal_callback(err_str) return False + elif robot_mode == 'velocity' and goal_pos is not None: + err_str = (f"Received goal point with position for joint {self.name} (index {self.index}) " + "during velocity mode, which is not allowed.") + invalid_goal_callback(err_str) + return False - self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal) + if goal_pos is not None: + self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal) self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None - if self.goal['position'] is None: + if robot_mode == 'position' and self.goal['position'] is None: err_str = ("Received {0} goal point that is out of bounds. " "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos) invalid_goal_callback(err_str) diff --git a/stretch_core/launch/stretch_driver.launch b/stretch_core/launch/stretch_driver.launch index cc80f7a..ed15b5e 100644 --- a/stretch_core/launch/stretch_driver.launch +++ b/stretch_core/launch/stretch_driver.launch @@ -2,6 +2,7 @@ + + diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/nodes/command_groups.py index e59513a..4e43c0e 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 @@ -79,17 +87,25 @@ class HeadTiltCommandGroup(SimpleCommandGroup): self.range = range_rad def init_execution(self, robot, robot_status, **kwargs): + robot_mode = kwargs['robot_mode'] if self.active: - _, tilt_error = self.update_execution(robot_status) - robot.head.move_by('head_tilt', tilt_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration']) - self.looking_up = self.goal['position'] > (self.backlash_transition_angle_rad + self.calibrated_offset_rad) + _, tilt_error = self.update_execution(robot_status, robot_mode=robot_mode) + if robot_mode == 'position': + robot.head.move_by('head_tilt', tilt_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration']) + self.looking_up = self.goal['position'] > (self.backlash_transition_angle_rad + self.calibrated_offset_rad) + elif robot_mode == 'velocity': + robot.head.get_joint('head_tilt').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: - tilt_backlash_correction = self.looking_up_offset_rad if self.looking_up else 0.0 - tilt_current = robot_status['head']['head_tilt']['pos'] + self.calibrated_offset_rad + tilt_backlash_correction - self.error = self.goal['position'] - tilt_current + if robot_mode == 'position': + tilt_backlash_correction = self.looking_up_offset_rad if self.looking_up else 0.0 + tilt_current = robot_status['head']['head_tilt']['pos'] + self.calibrated_offset_rad + tilt_backlash_correction + self.error = self.goal['position'] - tilt_current + elif robot_mode == 'velocity': + self.error = 0.0 return self.name, self.error return None @@ -118,16 +134,25 @@ class WristYawCommandGroup(SimpleCommandGroup): self.range = range_rad def init_execution(self, robot, robot_status, **kwargs): + robot_mode = kwargs['robot_mode'] if self.active: - robot.end_of_arm.move_by('wrist_yaw', - self.update_execution(robot_status)[1], - v_r=self.goal['velocity'], - a_r=self.goal['acceleration']) + _, yaw_error = self.update_execution(robot_status, robot_mode=robot_mode) + if robot_mode == 'position': + robot.end_of_arm.move_by('wrist_yaw', + yaw_error, + v_r=self.goal['velocity'], + a_r=self.goal['acceleration']) + elif robot_mode == 'velocity': + robot.end_of_arm.get_joint('wrist_yaw').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: - self.error = self.goal['position'] - robot_status['end_of_arm']['wrist_yaw']['pos'] + if robot_mode == 'position': + self.error = self.goal['position'] - robot_status['end_of_arm']['wrist_yaw']['pos'] + elif robot_mode == 'velocity': + self.error = 0.0 return self.name, self.error return None @@ -387,6 +412,7 @@ class ArmCommandGroup(SimpleCommandGroup): return True def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs): + robot_mode = kwargs['robot_mode'] self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None} if self.active: if self.is_telescoping: @@ -408,15 +434,21 @@ class ArmCommandGroup(SimpleCommandGroup): self.goal['contact_threshold'] = point.effort[self.index] \ if len(point.effort) > self.index else None - if goal_pos is None: + if robot_mode == 'position' and goal_pos is None: err_str = ("Received goal point with positions array length={0}. " "This joint ({1})'s index is {2}. Length of array must cover all joints listed " "in commanded_joint_names.").format(len(point.positions), self.name if not self.is_named_wrist_extension else self.wrist_extension_name, self.index) invalid_goal_callback(err_str) return False + elif robot_mode == 'velocity' and goal_pos is not None: + err_str = (f"Received goal point with position for joint {self.name} (index {self.index}) " + "during velocity mode, which is not allowed.") + invalid_goal_callback(err_str) + return False - self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal) - if self.goal['position'] is None: + if goal_pos is not None: + self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal) + if robot_mode == 'position' and self.goal['position'] is None: err_str = ("Received {0} goal point that is out of bounds. " "Range = {1}, but goal point = {2}.").format(self.name if not self.is_named_wrist_extension else self.wrist_extension_name, self.range, goal_pos) invalid_goal_callback(err_str) @@ -425,17 +457,26 @@ class ArmCommandGroup(SimpleCommandGroup): return True def init_execution(self, robot, robot_status, **kwargs): + robot_mode = kwargs['robot_mode'] if self.active: - _, extension_error_m = self.update_execution(robot_status, force_single=True) - robot.arm.move_by(extension_error_m, - v_m=self.goal['velocity'], - a_m=self.goal['acceleration'], - 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 + _, extension_error_m = self.update_execution(robot_status, force_single=True, robot_mode=robot_mode) + if robot_mode == 'position': + robot.arm.move_by(extension_error_m, + v_m=self.goal['velocity'], + a_m=self.goal['acceleration'], + 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 + elif robot_mode == 'velocity': + robot.arm.set_velocity(self.goal['velocity'], + a_m=self.goal['acceleration'], + 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): + robot_mode = kwargs['robot_mode'] success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None force_single = kwargs['force_single'] if 'force_single' in kwargs.keys() else False self.error = None @@ -443,9 +484,12 @@ class ArmCommandGroup(SimpleCommandGroup): if success_callback and robot_status['arm']['motor']['in_guarded_event']: success_callback("{0} contact detected.".format(self.name if not self.is_named_wrist_extension else self.wrist_extension_name)) return True - arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0 - extension_current = robot_status['arm']['pos'] + arm_backlash_correction - self.error = self.goal['position'] - extension_current + if robot_mode == 'position': + arm_backlash_correction = self.retracted_offset_m if self.retracted else 0.0 + extension_current = robot_status['arm']['pos'] + arm_backlash_correction + self.error = self.goal['position'] - extension_current + elif robot_mode == 'velocity': + self.error = 0.0 if force_single: return self.name, self.error else: @@ -478,22 +522,35 @@ class LiftCommandGroup(SimpleCommandGroup): self.range = range_m def init_execution(self, robot, robot_status, **kwargs): + robot_mode = kwargs['robot_mode'] if self.active: - robot.lift.move_by(self.update_execution(robot_status)[1], - v_m=self.goal['velocity'], - a_m=self.goal['acceleration'], - contact_thresh_pos=self.goal['contact_threshold'], - contact_thresh_neg=-self.goal['contact_threshold'] \ - if self.goal['contact_threshold'] is not None else None) + _, lift_error = self.update_execution(robot_status, robot_mode=robot_mode) + if robot_mode == 'position': + robot.lift.move_by(lift_error, + v_m=self.goal['velocity'], + a_m=self.goal['acceleration'], + contact_thresh_pos=self.goal['contact_threshold'], + contact_thresh_neg=-self.goal['contact_threshold'] \ + if self.goal['contact_threshold'] is not None else None) + elif robot_mode == 'velocity': + robot.lift.set_velocity(self.goal['velocity'], + a_m=self.goal['acceleration'], + 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): + robot_mode = kwargs['robot_mode'] success_callback = kwargs['success_callback'] if 'success_callback' in kwargs.keys() else None self.error = None if self.active: if success_callback and robot_status['lift']['motor']['in_guarded_event']: success_callback("{0} contact detected.".format(self.name)) return True - self.error = self.goal['position'] - robot_status['lift']['pos'] + if robot_mode == 'position': + self.error = self.goal['position'] - robot_status['lift']['pos'] + elif robot_mode == 'velocity': + self.error = 0.0 return self.name, self.error return None diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index fc3f0bf..1f38087 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -53,7 +53,7 @@ class JointTrajectoryAction: # Escape stopped mode to execute trajectory self.node.stop_the_robot = False self.node.robot_mode_rwlock.acquire_read() - if self.node.robot_mode not in ['position', 'manipulation', 'navigation']: + if self.node.robot_mode not in ['position', 'manipulation', 'navigation', 'velocity']: self.invalid_goal_callback("Cannot execute goals while in mode={0}".format(self.node.robot_mode)) self.node.robot_mode_rwlock.release_read() return @@ -96,7 +96,8 @@ class JointTrajectoryAction: rospy.logdebug(("{0} joint_traj action: " "target point #{1} = <{2}>").format(self.node.node_name, pointi, point)) - valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.fail_out_of_range_goal) + valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.fail_out_of_range_goal, + robot_mode=self.node.robot_mode) for c in self.command_groups] if not all(valid_goals): # At least one of the goals violated the requirements @@ -115,7 +116,7 @@ class JointTrajectoryAction: self.node.robot.push_command() for c in self.command_groups: - c.init_execution(self.node.robot, robot_status) + c.init_execution(self.node.robot, robot_status, robot_mode=self.node.robot_mode) #self.node.robot.push_command() #Moved to an asynchronous call in stretch_driver self.node.dirty_command=True @@ -144,7 +145,8 @@ class JointTrajectoryAction: return robot_status = self.node.robot.get_status() - named_errors = [c.update_execution(robot_status, success_callback=self.success_callback) + named_errors = [c.update_execution(robot_status, success_callback=self.success_callback, + robot_mode=self.node.robot_mode) for c in self.command_groups] # It's not clear how this could ever happen. The # groups in command_groups.py seem to return diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 23751e3..50f0f1b 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -42,7 +42,7 @@ class StretchDriverNode: self.stop_the_robot = False self.robot_mode_rwlock = RWLock() self.robot_mode = None - self.control_modes = ['position', 'navigation'] + self.control_modes = ['position', 'navigation', 'velocity'] self.prev_runstop_state = None self.voltage_history = [] @@ -53,8 +53,8 @@ class StretchDriverNode: def set_mobile_base_velocity_callback(self, twist): self.robot_mode_rwlock.acquire_read() - if self.robot_mode != 'navigation': - error_string = '{0} action server must be in navigation mode to receive a twist on cmd_vel. Current mode = {1}.'.format(self.node_name, self.robot_mode) + if self.robot_mode not in ['navigation', 'velocity']: + error_string = '{0} action server must be in navigation or velocity modes to receive a twist on cmd_vel. Current mode = {1}.'.format(self.node_name, self.robot_mode) rospy.logerr(error_string) self.robot_mode_rwlock.release_read() return @@ -67,7 +67,7 @@ class StretchDriverNode: self.robot_mode_rwlock.acquire_read() # set new mobile base velocities if available - if self.robot_mode == 'navigation': + if self.robot_mode in ['navigation', 'velocity']: time_since_last_twist = rospy.get_time() - self.last_twist_time if time_since_last_twist < self.timeout: self.robot.base.set_velocity(self.linear_velocity_mps, self.angular_velocity_radps) @@ -322,6 +322,17 @@ class StretchDriverNode: self.robot.base.enable_pos_incr_mode() self.change_mode('position', code_to_run) + def turn_on_velocity_mode(self): + # Velocity mode enables velocity control of every joint except + # the gripper, which remains in position control. Mobile base + # velocity control happens via the cmd_vel topic, and velocity + # control of the remaining joints happens through the + # FollowJointTrajectory action server. + def code_to_run(): + self.linear_velocity_mps = 0.0 + self.angular_velocity_radps = 0.0 + self.change_mode('velocity', code_to_run) + def home_the_robot(self): self.robot_mode_rwlock.acquire_read() can_home = self.robot_mode in self.control_modes @@ -429,6 +440,13 @@ class StretchDriverNode: message='Now in position mode.' ) + def velocity_mode_service_callback(self, request): + self.turn_on_velocity_mode() + return TriggerResponse( + success=True, + message='Now in velocity mode.' + ) + def runstop_service_callback(self, request): self.runstop_the_robot(request.data) return SetBoolResponse( @@ -465,6 +483,8 @@ class StretchDriverNode: self.turn_on_position_mode() elif mode == "navigation": self.turn_on_navigation_mode() + elif mode == "velocity": + self.turn_on_velocity_mode() self.broadcast_odom_tf = rospy.get_param('~broadcast_odom_tf', False) rospy.loginfo('broadcast_odom_tf = ' + str(self.broadcast_odom_tf)) @@ -556,6 +576,10 @@ class StretchDriverNode: self.switch_to_position_mode_service = rospy.Service('/switch_to_position_mode', Trigger, self.position_mode_service_callback) + + self.switch_to_velocity_mode_service = rospy.Service('/switch_to_velocity_mode', + Trigger, + self.velocity_mode_service_callback) self.stop_the_robot_service = rospy.Service('/stop_the_robot', Trigger,