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
clearpathrobotics · Jan 14, 2025 · 5a0f7dc · 5a0f7dc
1 parent b7a66ef
commit 5a0f7dc
Show file tree

Hide file tree

Showing 15 changed files with 363 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="ewellix" params="name">
+    <group name="${name}">
+        <joint name="${name}_upper_joint"/>
+        <joint name="${name}_lower_joint"/>
+    </group>
+    <group_state name="extended" group="${name}">
+        <joint name="${name}_upper_joint" value="0.25"/>
+        <joint name="${name}_lower_joint" value="0.25"/>
+    </group_state>
+    <group_state name="zero" group="${name}">
+        <joint name="${name}_upper_joint" value="0"/>
+        <joint name="${name}_lower_joint" value="0"/>
+    </group_state>
+
+    <disable_collisions link1="${name}_upper_link" link2="${name}_base_link" reason="User"/>
+    <disable_collisions link1="${name}_upper_link" link2="${name}_plate_link" reason="User"/>
+    <disable_collisions link1="${name}_upper_link" link2="${name}_lower_link" reason="User"/>
+    <disable_collisions link1="${name}_lower_link" link2="${name}_base_link" reason="User"/>
+    <disable_collisions link1="${name}_lower_link" link2="${name}_plate_link" reason="User"/>
+  </xacro:macro>
+</robot>