diff --git a/clearpath_common/launch/platform.launch.py b/clearpath_common/launch/platform.launch.py index d45986c9..92b882f3 100644 --- a/clearpath_common/launch/platform.launch.py +++ b/clearpath_common/launch/platform.launch.py @@ -66,10 +66,18 @@ def generate_launch_description(): description='Robot namespace' ) + arg_enable_ekf = DeclareLaunchArgument( + 'enable_ekf', + default_value='true', + choices=['true', 'false'], + description='Enable localization via EKF node' + ) + # Launch Configurations setup_path = LaunchConfiguration('setup_path') use_sim_time = LaunchConfiguration('use_sim_time') namespace = LaunchConfiguration('namespace') + enable_ekf = LaunchConfiguration('enable_ekf') # Launch files launch_file_platform_description = PathJoinSubstitution([ @@ -104,9 +112,9 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(launch_file_platform_description), launch_arguments=[ - ('setup_path', setup_path), - ('use_sim_time', use_sim_time), - ('namespace', namespace), + ('setup_path', setup_path), + ('use_sim_time', use_sim_time), + ('namespace', namespace), ] ), @@ -114,8 +122,8 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(launch_file_control), launch_arguments=[ - ('setup_path', setup_path), - ('use_sim_time', use_sim_time), + ('setup_path', setup_path), + ('use_sim_time', use_sim_time), ] ), @@ -123,8 +131,10 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(launch_file_localization), launch_arguments=[ - ('setup_path', setup_path), - ('use_sim_time', use_sim_time)] + ('setup_path', setup_path), + ('use_sim_time', use_sim_time), + ('enable_ekf', enable_ekf) + ] ), # Launch clearpath_control/teleop_base.launch.py which is various ways to tele-op @@ -132,8 +142,9 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(launch_file_teleop_base), launch_arguments=[ - ('setup_path', setup_path), - ('use_sim_time', use_sim_time)] + ('setup_path', setup_path), + ('use_sim_time', use_sim_time), + ] ), # Launch clearpath_control/teleop_joy.launch.py which is tele-operation using a @@ -141,8 +152,9 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(launch_file_teleop_joy), launch_arguments=[ - ('setup_path', setup_path), - ('use_sim_time', use_sim_time)] + ('setup_path', setup_path), + ('use_sim_time', use_sim_time), + ] ), ] ) @@ -151,5 +163,6 @@ def generate_launch_description(): ld.add_action(arg_setup_path) ld.add_action(arg_use_sim_time) ld.add_action(arg_namespace) + ld.add_action(arg_enable_ekf) ld.add_action(group_platform_action) return ld diff --git a/clearpath_control/launch/localization.launch.py b/clearpath_control/launch/localization.launch.py index a5078456..35902599 100644 --- a/clearpath_control/launch/localization.launch.py +++ b/clearpath_control/launch/localization.launch.py @@ -34,6 +34,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -41,15 +42,21 @@ def generate_launch_description(): # Launch Configurations + enable_ekf = LaunchConfiguration('enable_ekf') setup_path = LaunchConfiguration('setup_path') use_sim_time = LaunchConfiguration('use_sim_time') # Launch Arguments + arg_enable_ekf = DeclareLaunchArgument( + 'enable_ekf', + default_value='true', + choices=['true', 'false'], + description='Enable localization via EKF node' + ) arg_setup_path = DeclareLaunchArgument( 'setup_path', default_value='/etc/clearpath/' ) - arg_use_sim_time = DeclareLaunchArgument( 'use_sim_time', choices=['true', 'false'], @@ -69,20 +76,22 @@ def generate_launch_description(): # Localization node_localization = Node( - package='robot_localization', - executable='ekf_node', - name='ekf_node', - output='screen', - parameters=[config_localization], - remappings=[ - ('odometry/filtered', 'platform/odom/filtered'), - ('/diagnostics', 'diagnostics'), - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ] - ) + package='robot_localization', + executable='ekf_node', + name='ekf_node', + output='screen', + parameters=[config_localization], + remappings=[ + ('odometry/filtered', 'platform/odom/filtered'), + ('/diagnostics', 'diagnostics'), + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ], + condition=IfCondition(enable_ekf), + ) ld = LaunchDescription() + ld.add_action(arg_enable_ekf) ld.add_action(arg_setup_path) ld.add_action(arg_use_sim_time) ld.add_action(node_localization) diff --git a/clearpath_generator_common/clearpath_generator_common/description/generator.py b/clearpath_generator_common/clearpath_generator_common/description/generator.py index 91b4222f..d462aa81 100644 --- a/clearpath_generator_common/clearpath_generator_common/description/generator.py +++ b/clearpath_generator_common/clearpath_generator_common/description/generator.py @@ -205,6 +205,7 @@ def generate_manipulators(self) -> None: self.xacro_writer.write_comment('Manipulators') self.xacro_writer.write_newline() self.generate_arms() + self.generate_lifts() self.generate_grippers() def generate_arms(self) -> None: @@ -257,6 +258,30 @@ def generate_grippers(self) -> None: self.xacro_writer.write_newline() + def generate_lifts(self) -> None: + lifts = self.clearpath_config.manipulators.get_all_lifts() + for lift in lifts: + lift_description = ManipulatorDescription(lift) + + self.xacro_writer.write_comment( + '{0}'.format(lift_description.name) + ) + + self.xacro_writer.write_include( + package=lift_description.package, + file=lift_description.model, + path=lift_description.path + ) + + self.xacro_writer.write_macro( + macro='{0}'.format(lift_description.model), + parameters=lift_description.parameters, + blocks=XacroWriter.add_origin( + lift_description.xyz, lift_description.rpy) + ) + + self.xacro_writer.write_newline() + def generate_extras(self) -> None: self.xacro_writer.write_comment('Extras') self.xacro_writer.write_newline() diff --git a/clearpath_generator_common/clearpath_generator_common/description/manipulators.py b/clearpath_generator_common/clearpath_generator_common/description/manipulators.py index f06dece2..8471f77f 100644 --- a/clearpath_generator_common/clearpath_generator_common/description/manipulators.py +++ b/clearpath_generator_common/clearpath_generator_common/description/manipulators.py @@ -43,6 +43,10 @@ Robotiq2F140, Robotiq2F85 ) +from clearpath_config.manipulators.types.lifts import ( + BaseLift, + Ewellix +) from clearpath_config.manipulators.types.manipulator import BaseManipulator @@ -119,11 +123,23 @@ def __init__(self, arm: BaseArm) -> None: self.parameters.pop(self.PORT) self.parameters.update(arm.get_urdf_parameters()) + class LiftDescription(BaseDescription): + + def __init__(self, lift: BaseLift) -> None: + super().__init__(lift) + + class EwellixDescription(LiftDescription): + + def __init__(self, lift: BaseLift) -> None: + super().__init__(lift) + self.parameters.update(lift.get_urdf_parameters()) + MODEL = { KinovaGen3Dof6.MANIPULATOR_MODEL: KinovaArmDescription, KinovaGen3Dof7.MANIPULATOR_MODEL: KinovaArmDescription, KinovaGen3Lite.MANIPULATOR_MODEL: KinovaArmDescription, UniversalRobots.MANIPULATOR_MODEL: UniversalRobotsDescription, + Ewellix.MANIPULATOR_MODEL: EwellixDescription, } def __new__(cls, manipulator: BaseManipulator) -> BaseManipulator: diff --git a/clearpath_generator_common/clearpath_generator_common/launch/generator.py b/clearpath_generator_common/clearpath_generator_common/launch/generator.py index e4f2ab3c..99a92b12 100644 --- a/clearpath_generator_common/clearpath_generator_common/launch/generator.py +++ b/clearpath_generator_common/clearpath_generator_common/launch/generator.py @@ -68,9 +68,10 @@ def __init__(self, name='platform', package=self.pkg_clearpath_common, args=[ - ('setup_path', self.setup_path), - ('use_sim_time', 'false'), - ('namespace', self.namespace), + ('setup_path', self.setup_path), + ('use_sim_time', 'false'), + ('namespace', self.namespace), + ('enable_ekf', str(self.clearpath_config.platform.enable_ekf).lower()), ]) self.manipulators_launch_file = LaunchFile( diff --git a/clearpath_generator_common/clearpath_generator_common/param/manipulators.py b/clearpath_generator_common/clearpath_generator_common/param/manipulators.py index d9dd6def..9df65190 100644 --- a/clearpath_generator_common/clearpath_generator_common/param/manipulators.py +++ b/clearpath_generator_common/clearpath_generator_common/param/manipulators.py @@ -127,6 +127,25 @@ def generate_parameters(self, use_sim_time: bool = False) -> None: self.param_file.parameters = merge_dict( self.param_file.parameters, updated_parameters) + # Lifts + for lift in self.clearpath_config.manipulators.get_all_lifts(): + # Lift Control Parameter File + lift_param_file = ParamFile( + name='control', + package=Package('clearpath_manipulators_description'), + path='config/%s/%s' % ( + lift.get_manipulator_type(), + lift.get_manipulator_model()), + parameters={} + ) + lift_param_file.read() + updated_parameters = replace_dict_items( + lift_param_file.parameters, + {r'${name}': lift.name} + ) + self.param_file.parameters = merge_dict( + self.param_file.parameters, updated_parameters) + def generate_parameter_file(self): param_writer = ParamWriter(self.param_file) param_writer.write_file() @@ -258,6 +277,18 @@ def get_kinematics_parameters(self): r'${name}': gripper.name }) parameter_file += kinematics_file + # Lifts + for lift in self.clearpath_config.manipulators.get_all_lifts(): + kinematics_file = MoveItParamFile( + name=lift.get_manipulator_type(), + path=parameter_directory, + package=parameter_package + ) + kinematics_file.read() + kinematics_file.replace({ + r'${name}': lift.name + }) + parameter_file += kinematics_file parameter_file.add_header('robot_description_kinematics') return parameter_file @@ -328,6 +359,26 @@ def get_moveit_controller_parameters(self, use_sim_time: bool = False) -> None: r'${name}': gripper.name }) parameter_file += controller_file + # Lifts + for lift in self.clearpath_config.manipulators.get_all_lifts(): + controller_file = MoveItParamFile( + name=parameter_name, + path=os.path.join( + parameter_directory, + lift.get_manipulator_type(), + lift.get_manipulator_model() + ), + package=parameter_package + ) + controller_name = lift.name + if not use_sim_time: + controller_name = 'manipulators/' + controller_name + controller_file.read() + controller_file.replace({ + r'${controller_name}': controller_name, + r'${name}': lift.name + }) + parameter_file += controller_file return parameter_file # Joint Limits @@ -375,6 +426,22 @@ def get_joint_limits_parameters(self): r'${name}': gripper.name }) parameter_file += controller_file + # Lifts + for lift in self.clearpath_config.manipulators.get_all_lifts(): + controller_file = MoveItParamFile( + name=parameter_name, + path=os.path.join( + parameter_directory, + lift.get_manipulator_type(), + lift.get_manipulator_model() + ), + package=parameter_package + ) + controller_file.read() + controller_file.replace({ + r'${name}': lift.name + }) + parameter_file += controller_file parameter_file.add_header('robot_description_planning') return parameter_file diff --git a/clearpath_generator_common/clearpath_generator_common/param/platform.py b/clearpath_generator_common/clearpath_generator_common/param/platform.py index bad93227..152e6a0f 100644 --- a/clearpath_generator_common/clearpath_generator_common/param/platform.py +++ b/clearpath_generator_common/clearpath_generator_common/param/platform.py @@ -151,6 +151,26 @@ def generate_parameters(self, use_sim_time: bool = False) -> None: self.param_file.parameters = merge_dict( self.param_file.parameters, updated_parameters) + # Lift Control + if self.parameter == PlatformParam.CONTROL and use_sim_time: + for lift in self.clearpath_config.manipulators.get_all_lifts(): + # Arm Control Parameter File + lift_param_file = ParamFile( + name='control', + package=Package('clearpath_manipulators_description'), + path='config/%s/%s' % ( + lift.get_manipulator_type(), + lift.get_manipulator_model()), + parameters={} + ) + lift_param_file.read() + updated_parameters = replace_dict_items( + lift_param_file.parameters, + {r'${name}': lift.name} + ) + self.param_file.parameters = merge_dict( + self.param_file.parameters, updated_parameters) + # Get extra ros parameters from config extras = self.clearpath_config.platform.extras.ros_parameters for node in extras: diff --git a/clearpath_generator_common/clearpath_generator_common/semantic_description/generator.py b/clearpath_generator_common/clearpath_generator_common/semantic_description/generator.py index 66a61e28..653a809b 100644 --- a/clearpath_generator_common/clearpath_generator_common/semantic_description/generator.py +++ b/clearpath_generator_common/clearpath_generator_common/semantic_description/generator.py @@ -51,6 +51,10 @@ def generate(self) -> None: self.generate_grippers() self.xacro_writer.write_newline() + # Lifts + self.generate_lifts() + self.xacro_writer.write_newline() + self.xacro_writer.close_file() print(f'Generated {self.xacro_writer.file_path}robot.srdf.xacro') @@ -105,4 +109,25 @@ def generate_grippers(self) -> None: self.xacro_writer.write_newline() def generate_lifts(self) -> None: - pass + self.xacro_writer.write_comment('Lifts') + self.xacro_writer.write_newline() + lifts = self.clearpath_config.manipulators.get_all_lifts() + for lift in lifts: + lift_semantic_description = ManipulatorSemanticDescription(lift) + + self.xacro_writer.write_comment( + '{0}'.format(lift_semantic_description.name) + ) + + self.xacro_writer.write_include( + package=lift_semantic_description.package, + file=lift_semantic_description.model, + path=lift_semantic_description.path, + ) + + self.xacro_writer.write_macro( + macro='{0}'.format(lift_semantic_description.model), + parameters=lift_semantic_description.parameters, + ) + + self.xacro_writer.write_newline() diff --git a/clearpath_manipulators_description/config/lift/ewellix/control.yaml b/clearpath_manipulators_description/config/lift/ewellix/control.yaml new file mode 100644 index 00000000..a97c951c --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/control.yaml @@ -0,0 +1,15 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + ${name}_position_controller: + type: position_controllers/JointGroupPositionController + +${name}_position_controller: + ros__parameters: + joints: + - ${name}_upper_joint + - ${name}_lower_joint diff --git a/clearpath_manipulators_description/config/lift/ewellix/control_jpc.yaml b/clearpath_manipulators_description/config/lift/ewellix/control_jpc.yaml new file mode 100644 index 00000000..a97c951c --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/control_jpc.yaml @@ -0,0 +1,15 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + ${name}_position_controller: + type: position_controllers/JointGroupPositionController + +${name}_position_controller: + ros__parameters: + joints: + - ${name}_upper_joint + - ${name}_lower_joint diff --git a/clearpath_manipulators_description/config/lift/ewellix/control_jtc.yaml b/clearpath_manipulators_description/config/lift/ewellix/control_jtc.yaml new file mode 100644 index 00000000..1e7d47f9 --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/control_jtc.yaml @@ -0,0 +1,26 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + ${name}_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +${name}_joint_trajectory_controller: + ros__parameters: + joints: + - ${name}_upper_joint + - ${name}_lower_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 10.0 + action_monitor_rate: 10.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 diff --git a/clearpath_manipulators_description/config/lift/ewellix/initial_positions.yaml b/clearpath_manipulators_description/config/lift/ewellix/initial_positions.yaml new file mode 100644 index 00000000..2eea0780 --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/initial_positions.yaml @@ -0,0 +1,3 @@ +initial_positions: + ${name}_upper_joint: 0.0 + ${name}_lower_joint: 0.0 diff --git a/clearpath_manipulators_description/config/lift/ewellix/joint_limits.yaml b/clearpath_manipulators_description/config/lift/ewellix/joint_limits.yaml new file mode 100644 index 00000000..fcba7a0c --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/joint_limits.yaml @@ -0,0 +1,10 @@ +default_velocity_scaling_factor: 0.5 +default_acceleration_scaling_factor: 0.5 + +joint_limits: + ${name}_upper_joint: + has_acceleration_limits: true + max_acceleration: 1.0 + ${name}_lower_joint: + has_acceleration_limits: true + max_acceleration: 1.0 diff --git a/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers.yaml b/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers.yaml new file mode 100644 index 00000000..f6fcd67b --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers.yaml @@ -0,0 +1,13 @@ +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - ${controller_name}_position_controller + + ${controller_name}_position_controller: + type: JointGroupPositionController + action_ns: position_controllers + default: true + joints: + - ${name}_upper_joint + - ${name}_lower_joint diff --git a/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers_jpc.yaml b/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers_jpc.yaml new file mode 100644 index 00000000..f6fcd67b --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers_jpc.yaml @@ -0,0 +1,13 @@ +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - ${controller_name}_position_controller + + ${controller_name}_position_controller: + type: JointGroupPositionController + action_ns: position_controllers + default: true + joints: + - ${name}_upper_joint + - ${name}_lower_joint diff --git a/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers_jtc.yaml b/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers_jtc.yaml new file mode 100644 index 00000000..4246789e --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers_jtc.yaml @@ -0,0 +1,13 @@ +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - ${controller_name}_joint_trajectory_controller + + ${controller_name}_joint_trajectory_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - ${name}_upper_joint + - ${name}_lower_joint diff --git a/clearpath_manipulators_description/srdf/lift/ewellix.srdf.xacro b/clearpath_manipulators_description/srdf/lift/ewellix.srdf.xacro new file mode 100644 index 00000000..45da8e94 --- /dev/null +++ b/clearpath_manipulators_description/srdf/lift/ewellix.srdf.xacro @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/clearpath_manipulators_description/urdf/lift/ewellix.urdf.xacro b/clearpath_manipulators_description/urdf/lift/ewellix.urdf.xacro new file mode 100644 index 00000000..d5e241a5 --- /dev/null +++ b/clearpath_manipulators_description/urdf/lift/ewellix.urdf.xacro @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/clearpath_platform_description/urdf/do100/do100.urdf.xacro b/clearpath_platform_description/urdf/do100/do100.urdf.xacro index 18adb1ee..276b7421 100644 --- a/clearpath_platform_description/urdf/do100/do100.urdf.xacro +++ b/clearpath_platform_description/urdf/do100/do100.urdf.xacro @@ -4,6 +4,7 @@ + @@ -185,36 +186,38 @@ - - - - ign_ros2_control/IgnitionSystem - - - clearpath_hardware_interfaces/PumaHardware - - - - - - - - - - - - - - - - - - - - - - - + + + + + ign_ros2_control/IgnitionSystem + + + clearpath_hardware_interfaces/PumaHardware + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/clearpath_platform_description/urdf/do150/do150.urdf.xacro b/clearpath_platform_description/urdf/do150/do150.urdf.xacro index 98045bdc..9541e0b0 100644 --- a/clearpath_platform_description/urdf/do150/do150.urdf.xacro +++ b/clearpath_platform_description/urdf/do150/do150.urdf.xacro @@ -4,6 +4,7 @@ + diff --git a/clearpath_platform_description/urdf/r100/r100.urdf.xacro b/clearpath_platform_description/urdf/r100/r100.urdf.xacro index 9fd330e1..90e56e39 100644 --- a/clearpath_platform_description/urdf/r100/r100.urdf.xacro +++ b/clearpath_platform_description/urdf/r100/r100.urdf.xacro @@ -4,6 +4,7 @@ + @@ -126,7 +127,7 @@ - + @@ -296,36 +297,38 @@ - - - - ign_ros2_control/IgnitionSystem - - - clearpath_hardware_interfaces/PumaHardware - - - - - - - - - - - - - - - - - - - - - - - + + + + + ign_ros2_control/IgnitionSystem + + + clearpath_hardware_interfaces/PumaHardware + + + + + + + + + + + + + + + + + + + + + + + +