From 5a0f7dc19a638693506206cadf4fc1617355f451 Mon Sep 17 00:00:00 2001 From: luis-camero <88782189+luis-camero@users.noreply.github.com> Date: Tue, 14 Jan 2025 13:48:09 -0500 Subject: [PATCH] Ewellix Lift (#136) Ewellix Lift - Remove upper_joint - Add moveit jpc - Add control for joint position controller - Add hardware parameters - Add lifts to generators - Initial add of Ewellix lift description files --- .../description/generator.py | 25 ++++++ .../description/manipulators.py | 16 ++++ .../param/manipulators.py | 67 +++++++++++++++ .../param/platform.py | 20 +++++ .../semantic_description/generator.py | 27 +++++- .../config/lift/ewellix/control.yaml | 14 ++++ .../config/lift/ewellix/control_jpc.yaml | 14 ++++ .../config/lift/ewellix/control_jtc.yaml | 25 ++++++ .../lift/ewellix/initial_positions.yaml | 3 + .../config/lift/ewellix/joint_limits.yaml | 10 +++ .../lift/ewellix/moveit_controllers.yaml | 12 +++ .../lift/ewellix/moveit_controllers_jpc.yaml | 12 +++ .../lift/ewellix/moveit_controllers_jtc.yaml | 12 +++ .../srdf/lift/ewellix.srdf.xacro | 23 +++++ .../urdf/lift/ewellix.urdf.xacro | 84 +++++++++++++++++++ 15 files changed, 363 insertions(+), 1 deletion(-) create mode 100644 clearpath_manipulators_description/config/lift/ewellix/control.yaml create mode 100644 clearpath_manipulators_description/config/lift/ewellix/control_jpc.yaml create mode 100644 clearpath_manipulators_description/config/lift/ewellix/control_jtc.yaml create mode 100644 clearpath_manipulators_description/config/lift/ewellix/initial_positions.yaml create mode 100644 clearpath_manipulators_description/config/lift/ewellix/joint_limits.yaml create mode 100644 clearpath_manipulators_description/config/lift/ewellix/moveit_controllers.yaml create mode 100644 clearpath_manipulators_description/config/lift/ewellix/moveit_controllers_jpc.yaml create mode 100644 clearpath_manipulators_description/config/lift/ewellix/moveit_controllers_jtc.yaml create mode 100644 clearpath_manipulators_description/srdf/lift/ewellix.srdf.xacro create mode 100644 clearpath_manipulators_description/urdf/lift/ewellix.urdf.xacro 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/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..b96d54b9 --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/control.yaml @@ -0,0 +1,14 @@ +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}_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..b96d54b9 --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/control_jpc.yaml @@ -0,0 +1,14 @@ +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}_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..9d0b6e28 --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/control_jtc.yaml @@ -0,0 +1,25 @@ +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}_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..7f1e34ff --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers.yaml @@ -0,0 +1,12 @@ +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}_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..791de354 --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers_jpc.yaml @@ -0,0 +1,12 @@ +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}_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..7f1e34ff --- /dev/null +++ b/clearpath_manipulators_description/config/lift/ewellix/moveit_controllers_jtc.yaml @@ -0,0 +1,12 @@ +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}_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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +