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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+