Merge 6878c857b2 into 7397a97a78

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

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)

stretch_core/launch/stretch_driver.launch

@@ -2,6 +2,7 @@
   
   <param name="robot_description" textfile="$(find stretch_description)/urdf/stretch.urdf" />
   <arg name="calibrated_controller_yaml_file" value="$(find stretch_core)/config/controller_calibration_head.yaml"/>
+  <arg name="mode" default="position" doc="the initial mode for the driver" />
 
   <node
       name="joint_state_publisher"
@@ -34,6 +35,7 @@
   </node>
 
   <node name="stretch_driver" pkg="stretch_core" type="stretch_driver" output="screen">
+    <param name="mode" type="string" value="$(arg mode)"/>
     <param name="rate" type="double" value="30.0"/>
     <param name="timeout" type="double" value="0.5"/>
     <remap from="cmd_vel" to="/stretch/cmd_vel" />

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

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

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,