Skip to content

Commit 40af6e3

Browse files
luis-cameroluis-camero
authored
Feature: Kinova Jazzy Support (#265)
* Remove prefix from Kinova gripper joint * Switch mesh paths from file to package * Add custom description generator for Robotiq gripper on Kinova * Replace 'PI' property with 'math.pi' * Add ComposableNodeContainer to launch generator * Remove output from ComposableNode * Fix generator test
1 parent 25c3ef0 commit 40af6e3

File tree

6 files changed

+181
-51
lines changed

6 files changed

+181
-51
lines changed

‎clearpath_generator_common/clearpath_generator_common/common.py‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -105,6 +105,47 @@ def __init__(self,
105105
self.arguments = arguments
106106
self.remappings = remappings
107107

108+
class ComposableNode(LaunchComponent):
109+
110+
def __init__(self,
111+
name: str,
112+
package: Package,
113+
plugin: str,
114+
namespace: str = '',
115+
parameters: List[dict] | List[str] = [],
116+
extra_arguments: List[list] | List[str] = [],
117+
remappings: List[tuple] = []) -> None:
118+
super().__init__(name)
119+
self.declaration = 'node_' + self.name
120+
self.package = package
121+
self.plugin = plugin
122+
self.namespace = namespace
123+
self.parameters = parameters
124+
self.extra_arguments = extra_arguments
125+
self.remappings = remappings
126+
127+
class ComposableNodeContainer(Node):
128+
129+
def __init__(self,
130+
name: str,
131+
executable: str = 'component_container',
132+
package: Package = 'rclcpp_components',
133+
namespace: str = '',
134+
parameters: List[dict] | List[str] = [],
135+
arguments: List[list] | List[str] = [],
136+
remappings: List[tuple] = [],
137+
composable_node_descriptions: List = []) -> None:
138+
super().__init__(
139+
name,
140+
package,
141+
executable,
142+
namespace,
143+
parameters,
144+
arguments,
145+
remappings,
146+
)
147+
self.composable_node_descriptions = composable_node_descriptions
148+
108149
@staticmethod
109150
def get_static_tf_node(name: str,
110151
namespace: str,

‎clearpath_generator_common/clearpath_generator_common/description/manipulators.py‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
UniversalRobots
4141
)
4242
from clearpath_config.manipulators.types.grippers import (
43+
BaseGripper,
4344
FrankaGripper,
4445
Kinova2FLite,
4546
Robotiq2F140,
@@ -141,6 +142,15 @@ def __init__(self, gripper: FrankaGripper):
141142
super().__init__(gripper)
142143
self.parameters[Franka.ARM_ID] = gripper.arm_id
143144

145+
class BaseRobotiqGripperDescription(BaseDescription):
146+
147+
def __init__(self, gripper: BaseGripper):
148+
assert isinstance(gripper, Robotiq2F85) or isinstance(gripper, Robotiq2F140)
149+
super().__init__(gripper)
150+
if (gripper.arm.MANIPULATOR_MODEL == KinovaGen3Dof6.MANIPULATOR_MODEL or
151+
gripper.arm.MANIPULATOR_MODEL == KinovaGen3Dof7.MANIPULATOR_MODEL):
152+
self.parameters[gripper.USE_CONTROLLERS] = False
153+
144154
class LiftDescription(BaseDescription):
145155

146156
def __init__(self, lift: BaseLift) -> None:
@@ -160,6 +170,8 @@ def __init__(self, lift: BaseLift) -> None:
160170
KinovaGen3Lite.MANIPULATOR_MODEL: KinovaArmDescription,
161171
UniversalRobots.MANIPULATOR_MODEL: UniversalRobotsDescription,
162172
Ewellix.MANIPULATOR_MODEL: EwellixDescription,
173+
Robotiq2F85.MANIPULATOR_MODEL: BaseRobotiqGripperDescription,
174+
Robotiq2F140.MANIPULATOR_MODEL: BaseRobotiqGripperDescription,
163175
}
164176

165177
def __new__(cls, manipulator: BaseManipulator) -> BaseManipulator:

‎clearpath_generator_common/clearpath_generator_common/launch/writer.py‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,8 @@ def __init__(self, launch_file: LaunchFile):
4141
self.included_packages: List[Package] = []
4242
self.included_launch_files: List[LaunchFile] = []
4343
self.nodes: List[LaunchFile.Node] = []
44+
self.composable_nodes: List[LaunchFile.ComposableNode] = []
45+
self.composable_node_containers: List[LaunchFile.ComposableNodeContainer] = []
4446
self.declared_launch_args: List[LaunchFile.LaunchArg] = []
4547
self.processes: List[LaunchFile.Process] = []
4648
self.file = open(self.launch_file.get_full_path(), 'w+')
@@ -141,11 +143,23 @@ def add_process(self, process: LaunchFile.Process):
141143
if process not in self.processes:
142144
self.processes.append(process)
143145

146+
def add_composable_node(self, composable_node: LaunchFile.ComposableNode):
147+
if composable_node not in self.composable_nodes:
148+
self.composable_nodes.append(composable_node)
149+
150+
def add_composable_node_container(self, container: LaunchFile.ComposableNodeContainer):
151+
if container not in self.composable_node_containers:
152+
self.composable_node_containers.append(container)
153+
144154
def add(self, component: LaunchFile | LaunchFile.LaunchComponent):
145155
if isinstance(component, LaunchFile.LaunchArg):
146156
self.add_launch_arg(component)
147157
elif isinstance(component, LaunchFile):
148158
self.add_launch_file(component)
159+
elif isinstance(component, LaunchFile.ComposableNode):
160+
self.add_composable_node(component)
161+
elif isinstance(component, LaunchFile.ComposableNodeContainer):
162+
self.add_composable_node_container(component)
149163
elif isinstance(component, LaunchFile.Node):
150164
self.add_node(component)
151165
elif isinstance(component, LaunchFile.Process):
@@ -166,6 +180,17 @@ def initialize_file(self):
166180
0)
167181
self.write(
168182
'from launch_ros.actions import Node', 0)
183+
composable_node_imported = False
184+
if len(self.composable_node_containers) > 0:
185+
self.write(
186+
'from launch_ros.actions import ComposableNodeContainer', 0)
187+
if any(bool(i.composable_node_descriptions) for i in self.composable_node_containers):
188+
composable_node_imported = True
189+
self.write(
190+
'from launch_ros.descriptions import ComposableNode', 0)
191+
if len(self.composable_nodes) > 0 and not composable_node_imported:
192+
self.write(
193+
'from launch_ros.descriptions import ComposableNode', 0)
169194
self.write(
170195
'from launch_ros.substitutions import FindPackageShare', 0)
171196
self.write_newline()
@@ -261,6 +286,56 @@ def generate_file(self):
261286
self.write(')')
262287
self.write_newline()
263288

289+
if len(self.composable_node_containers) > 0:
290+
self.write_comment('Composable Node Containers')
291+
for container in self.composable_node_containers:
292+
self.write('{0} = ComposableNodeContainer('.format(container.declaration))
293+
self.write("name='{0}',".format(container.name), indent_level=2)
294+
self.write("executable='{0}',".format(container.executable), indent_level=2)
295+
self.write("package='{0}',".format(container.package), indent_level=2)
296+
self.write("namespace='{0}',".format(container.namespace), indent_level=2)
297+
self.write("output='screen',", indent_level=2)
298+
# Arguments
299+
if len(container.arguments) > 0:
300+
self.write('arguments=', indent_level=2)
301+
self.write_obj(container.arguments, indent_level=3)
302+
self.write(',', indent_level=2)
303+
# Remappings
304+
if len(container.remappings) > 0:
305+
self.write('remappings=', indent_level=2)
306+
self.write_obj(container.remappings, indent_level=3)
307+
self.write(',', indent_level=2)
308+
# Parameters
309+
if len(container.parameters) > 0:
310+
self.write('parameters=', indent_level=2)
311+
self.write_obj(container.parameters, indent_level=3)
312+
self.write(',', indent_level=2)
313+
# Composable Nodes
314+
if len(container.composable_node_descriptions) > 0:
315+
self.write('composable_node_descriptions=[', indent_level=2)
316+
for node in container.composable_node_descriptions:
317+
self.write('ComposableNode(', indent_level=3)
318+
self.write("name='{0}',".format(node.name), indent_level=4)
319+
self.write("plugin='{0}',".format(node.plugin), indent_level=4)
320+
self.write("package='{0}',".format(node.package), indent_level=4)
321+
self.write("namespace='{0}',".format(node.namespace), indent_level=4)
322+
if len(node.extra_arguments) > 0:
323+
self.write('extra_arguments=', indent_level=4)
324+
self.write_obj(node.extra_arguments, indent_level=5)
325+
self.write(',', indent_level=4)
326+
if len(node.remappings) > 0:
327+
self.write('remappings=', indent_level=4)
328+
self.write_obj(node.remappings, indent_level=5)
329+
self.write(',', indent_level=4)
330+
if len(node.parameters) > 0:
331+
self.write('parameters=', indent_level=4)
332+
self.write_obj(node.parameters, indent_level=5)
333+
self.write(',', indent_level=4)
334+
self.write('),', indent_level=3)
335+
self.write(']', indent_level=2)
336+
self.write(')')
337+
self.write_newline()
338+
264339
if len(self.processes) > 0:
265340
self.write_comment('Processes')
266341
for process in self.processes:
@@ -282,6 +357,8 @@ def generate_file(self):
282357
self.write('ld.add_action({0})'.format(node.declaration))
283358
for process in self.processes:
284359
self.write('ld.add_action({0})'.format(process.declaration))
360+
for container in self.composable_node_containers:
361+
self.write('ld.add_action({0})'.format(container.declaration))
285362
self.write('return ld')
286363

287364
self.close_file()

‎clearpath_generator_common/test/test_generator_description.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -76,7 +76,7 @@ def test_samples(self):
7676
try:
7777
xacro.process_file(os.path.join(os.path.dirname(dst), 'robot.urdf.xacro')).toxml()
7878
except xacro.XacroException as e:
79-
if 'stereolabs' in src and 'package not found' in e.args[0]:
79+
if 'stereolabs' in src:
8080
continue
8181
errors.append("Sample '%s' xacro failed to load: '%s'" % (
8282
sample,

‎clearpath_manipulators_description/urdf/arm/kinova_gen3_6dof.urdf.xacro‎

Lines changed: 17 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@
4646
<visual>
4747
<origin xyz="0 0 0" rpy="0 0 0" />
4848
<geometry>
49-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/base_link.STL" />
49+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/base_link.STL" />
5050
</geometry>
5151
<material name="">
5252
<color rgba="0.75294 0.75294 0.75294 1" />
@@ -55,7 +55,7 @@
5555
<collision>
5656
<origin xyz="0 0 0" rpy="0 0 0" />
5757
<geometry>
58-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/base_link.STL" />
58+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/base_link.STL" />
5959
</geometry>
6060
</collision>
6161
</link>
@@ -68,7 +68,7 @@
6868
<visual>
6969
<origin xyz="0 0 0" rpy="0 0 0" />
7070
<geometry>
71-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/shoulder_link.STL" />
71+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/shoulder_link.STL" />
7272
</geometry>
7373
<material name="">
7474
<color rgba="0.75294 0.75294 0.75294 1" />
@@ -77,7 +77,7 @@
7777
<collision>
7878
<origin xyz="0 0 0" rpy="0 0 0" />
7979
<geometry>
80-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/shoulder_link.STL" />
80+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/shoulder_link.STL" />
8181
</geometry>
8282
</collision>
8383
</link>
@@ -133,7 +133,7 @@
133133
xyz="0 0 0"
134134
rpy="0 0 0" />
135135
<geometry>
136-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/bicep_link.STL" />
136+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/bicep_link.STL" />
137137
</geometry>
138138
<material name="">
139139
<color rgba="0.75294 0.75294 0.75294 1" />
@@ -144,7 +144,7 @@
144144
xyz="0 0 0"
145145
rpy="0 0 0" />
146146
<geometry>
147-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/bicep_link.STL" />
147+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/bicep_link.STL" />
148148
</geometry>
149149
</collision>
150150
</link>
@@ -164,7 +164,7 @@
164164
<visual>
165165
<origin xyz="0 0 0" rpy="0 0 0" />
166166
<geometry>
167-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/forearm_link.STL" />
167+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/forearm_link.STL" />
168168
</geometry>
169169
<material name="">
170170
<color rgba="0.75294 0.75294 0.75294 1" />
@@ -173,7 +173,7 @@
173173
<collision>
174174
<origin xyz="0 0 0" rpy="0 0 0" />
175175
<geometry>
176-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/forearm_link.STL" />
176+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/forearm_link.STL" />
177177
</geometry>
178178
</collision>
179179
</link>
@@ -193,7 +193,7 @@
193193
<visual>
194194
<origin xyz="0 0 0" rpy="0 0 0" />
195195
<geometry>
196-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/spherical_wrist_1_link.STL" />
196+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/spherical_wrist_1_link.STL" />
197197
</geometry>
198198
<material name="">
199199
<color rgba="0.75294 0.75294 0.75294 1" />
@@ -202,7 +202,7 @@
202202
<collision>
203203
<origin xyz="0 0 0" rpy="0 0 0" />
204204
<geometry>
205-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/spherical_wrist_1_link.STL" />
205+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/spherical_wrist_1_link.STL" />
206206
</geometry>
207207
</collision>
208208
</link>
@@ -233,7 +233,7 @@
233233
<visual>
234234
<origin xyz="0 0 0" rpy="0 0 0" />
235235
<geometry>
236-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/spherical_wrist_2_link.STL" />
236+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/spherical_wrist_2_link.STL" />
237237
</geometry>
238238
<material name="">
239239
<color rgba="0.75294 0.75294 0.75294 1" />
@@ -242,7 +242,7 @@
242242
<collision>
243243
<origin xyz="0 0 0" rpy="0 0 0" />
244244
<geometry>
245-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/spherical_wrist_2_link.STL" />
245+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/spherical_wrist_2_link.STL" />
246246
</geometry>
247247
</collision>
248248
</link>
@@ -264,7 +264,7 @@
264264
<visual>
265265
<origin xyz="0 0 0" rpy="0 0 0" />
266266
<geometry>
267-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/bracelet_with_vision_link.STL" />
267+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/bracelet_with_vision_link.STL" />
268268
</geometry>
269269
<material name="">
270270
<color rgba="0.75294 0.75294 0.75294 1" />
@@ -273,7 +273,7 @@
273273
<collision>
274274
<origin xyz="0 0 0" rpy="0 0 0" />
275275
<geometry>
276-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/bracelet_with_vision_link.STL" />
276+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/bracelet_with_vision_link.STL" />
277277
</geometry>
278278
</collision>
279279
</link>
@@ -289,7 +289,7 @@
289289
<visual>
290290
<origin xyz="0 0 0" rpy="0 0 0" />
291291
<geometry>
292-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/bracelet_no_vision_link.STL" />
292+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/bracelet_no_vision_link.STL" />
293293
</geometry>
294294
<material name="">
295295
<color rgba="0.75294 0.75294 0.75294 1" />
@@ -298,7 +298,7 @@
298298
<collision>
299299
<origin xyz="0 0 0" rpy="0 0 0" />
300300
<geometry>
301-
<mesh filename="file://$(find kortex_description)/arms/gen3/${dof}dof/meshes/bracelet_no_vision_link.STL" />
301+
<mesh filename="package://kortex_description/arms/gen3/${dof}dof/meshes/bracelet_no_vision_link.STL" />
302302
</geometry>
303303
</collision>
304304
</link>
@@ -486,7 +486,7 @@
486486
</joint>
487487
<xacro:unless value="${use_fake_hardware or sim_gazebo or sim_ignition or sim_isaac}">
488488
<xacro:if value="${use_internal_bus_gripper_comm}">
489-
<joint name="${name}_${gripper_joint_name}">
489+
<joint name="${gripper_joint_name}">
490490
<command_interface name="position" />
491491
<state_interface name="position"/>
492492
<state_interface name="velocity"/>

0 commit comments

Comments
 (0)