From f78e46e64d5e9a34f7b3667388887029ae6166e4 Mon Sep 17 00:00:00 2001 From: hello-binit <64861565+hello-binit@users.noreply.github.com> Date: Fri, 16 Apr 2021 10:02:49 -0400 Subject: [PATCH] Arm group commandable from MoveIt2 --- stretch_core/launch/keyboard_teleop.launch.py | 1 + stretch_core/launch/stretch_driver.launch.py | 10 ++++- stretch_core/stretch_core/command_groups.py | 39 ++++++++++--------- stretch_core/stretch_core/stretch_driver.py | 28 +++++-------- .../config/moveit_simple_controllers.yaml | 36 +++++++++-------- stretch_moveit_config/launch/moveit.rviz | 2 +- 6 files changed, 59 insertions(+), 57 deletions(-) diff --git a/stretch_core/launch/keyboard_teleop.launch.py b/stretch_core/launch/keyboard_teleop.launch.py index 476bc9a..2b8f83a 100644 --- a/stretch_core/launch/keyboard_teleop.launch.py +++ b/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')]) diff --git a/stretch_core/launch/stretch_driver.launch.py b/stretch_core/launch/stretch_driver.launch.py index 430bdad..4772d49 100644 --- a/stretch_core/launch/stretch_driver.launch.py +++ b/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]) diff --git a/stretch_core/stretch_core/command_groups.py b/stretch_core/stretch_core/command_groups.py index 70cf621..343cfb2 100644 --- a/stretch_core/stretch_core/command_groups.py +++ b/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] diff --git a/stretch_core/stretch_core/stretch_driver.py b/stretch_core/stretch_core/stretch_driver.py index a85af59..1abaaab 100755 --- a/stretch_core/stretch_core/stretch_driver.py +++ b/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', diff --git a/stretch_moveit_config/config/moveit_simple_controllers.yaml b/stretch_moveit_config/config/moveit_simple_controllers.yaml index 77c7dc5..0812ce2 100644 --- a/stretch_moveit_config/config/moveit_simple_controllers.yaml +++ b/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 diff --git a/stretch_moveit_config/launch/moveit.rviz b/stretch_moveit_config/launch/moveit.rviz index db0e5fa..6adddbd 100644 --- a/stretch_moveit_config/launch/moveit.rviz +++ b/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: