Arm group commandable from MoveIt2

stretch_core/launch/keyboard_teleop.launch.py

@@ -15,4 +15,5 @@ def generate_launch_description():
         # KEYBOARD TELEOP
         Node(package='stretch_core',
              executable='keyboard_teleop',
+             prefix='xterm -e',
              output='screen')])

stretch_core/launch/stretch_driver.launch.py

@@ -40,7 +40,8 @@ def generate_launch_description():
                                       {'timeout': 0.5},
                                       {'controller_calibration_file': calibrated_controller_yaml_file},
                                       {'broadcast_odom_tf': LaunchConfiguration('broadcast_odom_tf')},
-                                      {'fail_out_of_range_goal': LaunchConfiguration('fail_out_of_range_goal')}])
+                                      {'fail_out_of_range_goal': LaunchConfiguration('fail_out_of_range_goal')},
+                                      {'mode': LaunchConfiguration('mode')}])
 
     declare_broadcast_odom_tf_arg = DeclareLaunchArgument(
         'broadcast_odom_tf',
@@ -54,8 +55,15 @@ def generate_launch_description():
         description="Whether the motion action servers fail on out-of-range commands"
     )
 
+    declare_mode_arg = DeclareLaunchArgument(
+        'mode',
+        default_value=str('position'),
+        description="The mode in which the ROS driver commands the robot"
+    )
+
     return LaunchDescription([declare_broadcast_odom_tf_arg,
                               declare_fail_out_of_range_goal_arg,
+                              declare_mode_arg,
                               joint_state_publisher,
                               robot_state_publisher,
                               stretch_driver])

stretch_core/stretch_core/command_groups.py

@@ -174,7 +174,7 @@ class HeadPanCommandGroup(SimpleCommandGroup):
     def set_trajectory_goals(self, points, robot):
         if self.active:
             for waypoint in points:
-                t = waypoint.time_from_start.to_sec()
+                t = waypoint.time_from_start.sec
                 x = None
                 if len(waypoint.positions) > self.index:
                     x = waypoint.positions[self.index]
@@ -223,7 +223,7 @@ class HeadTiltCommandGroup(SimpleCommandGroup):
     def set_trajectory_goals(self, points, robot):
         if self.active:
             for waypoint in points:
-                t = waypoint.time_from_start.to_sec()
+                t = waypoint.time_from_start.sec
                 x = None
                 if len(waypoint.positions) > self.index:
                     x = waypoint.positions[self.index]
@@ -267,7 +267,7 @@ class WristYawCommandGroup(SimpleCommandGroup):
     def set_trajectory_goals(self, points, robot):
         if self.active:
             for waypoint in points:
-                t = waypoint.time_from_start.to_sec()
+                t = waypoint.time_from_start.sec
                 x = None
                 if len(waypoint.positions) > self.index:
                     x = waypoint.positions[self.index]
@@ -422,19 +422,20 @@ class TelescopingCommandGroup(SimpleCommandGroup):
         return True
 
     def set_trajectory_goals(self, points, robot):
-        if self.active:
-            for waypoint in points:
-                t = waypoint.time_from_start.to_sec()
-                x = None
-                if len(waypoint.positions) > self.index:
-                    x = waypoint.positions[self.index]
-                v = None
-                if len(waypoint.velocities) > self.index:
-                    v = waypoint.velocities[self.index]
-                a = None
-                if len(waypoint.accelerations) > self.index:
-                    a = waypoint.accelerations[self.index]
-                robot.arm.trajectory.add_waypoint(t_s=t, x_m=x, v_m=v, a_m=a)
+        pass
+        # if self.active:
+        #     for waypoint in points:
+        #         t = waypoint.time_from_start.sec
+        #         x = None
+        #         if len(waypoint.positions) > self.index:
+        #             x = waypoint.positions[self.index]
+        #         v = None
+        #         if len(waypoint.velocities) > self.index:
+        #             v = waypoint.velocities[self.index]
+        #         a = None
+        #         if len(waypoint.accelerations) > self.index:
+        #             a = waypoint.accelerations[self.index]
+        #         robot.arm.trajectory.add_waypoint(t_s=t, x_m=x, v_m=v, a_m=a)
 
     def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
         self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
@@ -514,7 +515,7 @@ class LiftCommandGroup(SimpleCommandGroup):
     def set_trajectory_goals(self, points, robot):
         if self.active:
             for waypoint in points:
-                t = waypoint.time_from_start.to_sec()
+                t = waypoint.time_from_start.sec
                 x = None
                 if len(waypoint.positions) > self.index:
                     x = waypoint.positions[self.index]
@@ -627,7 +628,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
     def set_trajectory_goals(self, points, robot):
         if self.active_translate_mobile_base:
             for waypoint in points:
-                t = waypoint.time_from_start.to_sec()
+                t = waypoint.time_from_start.sec
                 x = None
                 if len(waypoint.positions) > self.index_translate_mobile_base:
                     x = waypoint.positions[self.index_translate_mobile_base]
@@ -640,7 +641,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
                 robot.base.trajectory.add_translate_waypoint(t_s=t, x_m=x, v_m=v, a_m=a)
         elif self.active_rotate_mobile_base:
             for waypoint in points:
-                t = waypoint.time_from_start.to_sec()
+                t = waypoint.time_from_start.sec
                 x = None
                 if len(waypoint.positions) > self.index_rotate_mobile_base:
                     x = waypoint.positions[self.index_rotate_mobile_base]

stretch_core/stretch_core/stretch_driver.py

@@ -294,15 +294,15 @@ class StretchBodyNode(Node):
 
         # set virtual joint for mobile base translation
         joint_state.name.append('joint_mobile_base_translation')
-        if self.robot_mode == 'manipulation':
-            manipulation_translation = x_raw - self.mobile_base_manipulation_origin['x']
-            positions.append(manipulation_translation)
-            velocities.append(x_vel_raw)
-            efforts.append(x_effort_raw)
-        else:
-            positions.append(0.0)
-            velocities.append(0.0)
-            efforts.append(0.0)
+        # if self.robot_mode == 'manipulation':
+        #     manipulation_translation = x_raw - self.mobile_base_manipulation_origin['x']
+        #     positions.append(manipulation_translation)
+        #     velocities.append(x_vel_raw)
+        #     efforts.append(x_effort_raw)
+        # else:
+        positions.append(0.0)
+        velocities.append(0.0)
+        efforts.append(0.0)
 
         # set joint_state
         joint_state.position = positions
@@ -398,14 +398,6 @@ class StretchBodyNode(Node):
             self.mobile_base_manipulation_origin = {'x':x, 'y':y, 'theta':theta}
         self.change_mode('manipulation', code_to_run)
 
-    def turn_on_experimental_manipulation_mode(self):
-        """Utilizes the new trajectory following interface
-        built into stretch_body.
-        """
-        def code_to_run():
-            pass
-        self.change_mode('experimental_manipulation', code_to_run)
-
     def turn_on_position_mode(self):
         # Position mode enables mobile base translation and rotation
         # using position control with sequential incremental rotations
@@ -606,8 +598,6 @@ class StretchBodyNode(Node):
             self.turn_on_navigation_mode()
         elif mode == "manipulation":
             self.turn_on_manipulation_mode()
-        elif mode == "experimental_manipulation":
-            self.turn_on_experimental_manipulation_mode()
 
         self.switch_to_manipulation_mode_service = self.create_service(Trigger,
                                                                        '/switch_to_manipulation_mode',

stretch_moveit_config/config/moveit_simple_controllers.yaml

@@ -1,9 +1,10 @@
 controller_names:
-  - stretch_arm_controller
-  - gripper_controller
+  - stretch_controller
+  # - stretch_base_controller
+  # - gripper_controller
 
-stretch_arm_controller:
-    action_ns: /stretch_controller/follow_joint_trajectory
+stretch_controller:
+    action_ns: "follow_joint_trajectory"
     default: True
     type: FollowJointTrajectory
     joints:
@@ -13,18 +14,19 @@ stretch_arm_controller:
       - joint_arm_l1
       - joint_arm_l0
       - joint_wrist_yaw
+      - position
 
-gripper_controller:
-    action_ns: /stretch_controller/follow_joint_trajectory
-    default: True
-    type: FollowJointTrajectory
-    joints:
-      - joint_gripper_finger_left
-      - joint_gripper_finger_right
+# gripper_controller:
+#     action_ns: "stretch_controller/follow_joint_trajectory"
+#     default: True
+#     type: FollowJointTrajectory
+#     joints:
+#       - joint_gripper_finger_left
+#       - joint_gripper_finger_right
 
-stretch_base_controller:
-    action_ns: /stretch_controller/follow_joint_trajectory
-    default: True
-    type: FollowMultiDOFJointTrajectory
-    joints:
-      - position
+# stretch_base_controller:
+#     action_ns: "follow_joint_trajectory"
+#     default: True
+#     type: FollowMultiDOFJointTrajectory
+#     joints:
+#       - position

stretch_moveit_config/launch/moveit.rviz

@@ -877,7 +877,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: map
+    Fixed Frame: odom
     Frame Rate: 30
   Name: root
   Tools: