Skip to content

Commit 7987519

Browse files
luis-cameroluis-camero
authored
Feature: Manipulator Samples and Poses (#163) (#188)
* Add group_state macros to arm SRDF * Add group_state macros to grippers SRDF * Generate pose macros in URDF
1 parent 350f99c commit 7987519

File tree

9 files changed

+201
-78
lines changed

9 files changed

+201
-78
lines changed

‎clearpath_generator_common/clearpath_generator_common/semantic_description/generator.py

+17
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232
from clearpath_generator_common.common import BaseGenerator
3333
from clearpath_generator_common.description.writer import XacroWriter
3434
from clearpath_generator_common.semantic_description.manipulators import (
35+
ManipulatorPoseMacro,
3536
ManipulatorSemanticDescription
3637
)
3738

@@ -75,6 +76,14 @@ def generate_arms(self) -> None:
7576
path=arm_semantic_description.path,
7677
)
7778

79+
for pose in arm.poses:
80+
pose_macro = ManipulatorPoseMacro(arm, pose)
81+
self.xacro_writer.write_macro(
82+
macro=pose_macro.macro(),
83+
parameters=pose_macro.parameters(),
84+
blocks=pose_macro.blocks(),
85+
)
86+
7887
self.xacro_writer.write_macro(
7988
macro='{0}'.format(arm_semantic_description.model),
8089
parameters=arm_semantic_description.parameters,
@@ -101,6 +110,14 @@ def generate_grippers(self) -> None:
101110
path=gripper_semantic_description.path,
102111
)
103112

113+
for pose in arm.gripper.poses:
114+
pose_macro = ManipulatorPoseMacro(arm.gripper, pose)
115+
self.xacro_writer.write_macro(
116+
macro=pose_macro.macro(),
117+
parameters=pose_macro.parameters(),
118+
blocks=pose_macro.blocks(),
119+
)
120+
104121
self.xacro_writer.write_macro(
105122
macro='{0}'.format(gripper_semantic_description.model),
106123
parameters=gripper_semantic_description.parameters,

‎clearpath_generator_common/clearpath_generator_common/semantic_description/manipulators.py

+25-1
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,31 @@
2929
# Redistribution and use in source and binary forms, with or without
3030
# modification, is not permitted without the express permission
3131
# of Clearpath Robotics.
32-
from clearpath_config.manipulators.types.manipulator import BaseManipulator
32+
from clearpath_config.manipulators.types.manipulator import (
33+
BaseManipulator,
34+
ManipulatorPose
35+
)
36+
37+
38+
class ManipulatorPoseMacro():
39+
40+
def __init__(self, manipulator: BaseManipulator, pose: ManipulatorPose) -> None:
41+
self.manipulator = manipulator
42+
self.pose = pose
43+
44+
def macro(self) -> str:
45+
return f'{self.manipulator.MANIPULATOR_MODEL}_group_state'
46+
47+
def parameters(self) -> dict:
48+
str_joints = [f'{joint:.4f}' for joint in self.pose.joints]
49+
return {
50+
'name': self.manipulator.name,
51+
'group_state': self.pose.name,
52+
'joint_positions': f'${{[{", ".join(str_joints)}]}}'
53+
}
54+
55+
def blocks(self) -> str:
56+
return None
3357

3458

3559
class ManipulatorSemanticDescription():

‎clearpath_manipulators_description/srdf/arm/kinova_gen3_6dof.srdf.xacro

+25-16
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,17 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22
<robot xmlns:xacro="http://ros.org/wiki/xacro">
3+
4+
<xacro:macro name="kinova_gen3_6dof_group_state" params="name group_state joint_positions">
5+
<group_state name="${group_state}" group="${name}">
6+
<joint name="${name}_joint_1" value="${joint_positions[0]}"/>
7+
<joint name="${name}_joint_2" value="${joint_positions[1]}"/>
8+
<joint name="${name}_joint_3" value="${joint_positions[2]}"/>
9+
<joint name="${name}_joint_4" value="${joint_positions[3]}"/>
10+
<joint name="${name}_joint_5" value="${joint_positions[4]}"/>
11+
<joint name="${name}_joint_6" value="${joint_positions[5]}"/>
12+
</group_state>
13+
</xacro:macro>
14+
315
<xacro:macro name="kinova_gen3_6dof" params="name">
416
<group name="${name}">
517
<joint name="${name}_joint_1"/>
@@ -9,21 +21,18 @@
921
<joint name="${name}_joint_5"/>
1022
<joint name="${name}_joint_6"/>
1123
</group>
12-
<group_state name="home" group="${name}">
13-
<joint name="${name}_joint_1" value="0.0174"/>
14-
<joint name="${name}_joint_2" value="-0.5725"/>
15-
<joint name="${name}_joint_3" value="-0.9846"/>
16-
<joint name="${name}_joint_4" value="0.1562"/>
17-
<joint name="${name}_joint_5" value="-1.0719"/>
18-
<joint name="${name}_joint_6" value="-1.7183"/>
19-
</group_state>
20-
<group_state name="zero" group="${name}">
21-
<joint name="${name}_joint_1" value="0"/>
22-
<joint name="${name}_joint_2" value="0"/>
23-
<joint name="${name}_joint_3" value="0"/>
24-
<joint name="${name}_joint_4" value="0"/>
25-
<joint name="${name}_joint_5" value="0"/>
26-
<joint name="${name}_joint_6" value="0"/>
27-
</group_state>
24+
25+
<xacro:kinova_gen3_6dof_group_state
26+
name="${name}"
27+
group_state="home"
28+
joint_positions="${[0.0174, -0.5725, -0.9846, 0.1562, -1.0719, -1.7183]}"
29+
/>
30+
31+
<xacro:kinova_gen3_6dof_group_state
32+
name="${name}"
33+
group_state="zero"
34+
joint_positions="${[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
35+
/>
36+
2837
</xacro:macro>
2938
</robot>

‎clearpath_manipulators_description/srdf/arm/kinova_gen3_7dof.srdf.xacro

+26-18
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,18 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22
<robot xmlns:xacro="http://ros.org/wiki/xacro">
3+
4+
<xacro:macro name="kinova_gen3_7dof_group_state" params="name group_state joint_positions">
5+
<group_state name="${group_state}" group="${name}">
6+
<joint name="${name}_joint_1" value="${joint_positions[0]}"/>
7+
<joint name="${name}_joint_2" value="${joint_positions[1]}"/>
8+
<joint name="${name}_joint_3" value="${joint_positions[2]}"/>
9+
<joint name="${name}_joint_4" value="${joint_positions[3]}"/>
10+
<joint name="${name}_joint_5" value="${joint_positions[4]}"/>
11+
<joint name="${name}_joint_6" value="${joint_positions[5]}"/>
12+
<joint name="${name}_joint_7" value="${joint_positions[6]}"/>
13+
</group_state>
14+
</xacro:macro>
15+
316
<xacro:macro name="kinova_gen3_7dof" params="name">
417
<group name="${name}">
518
<joint name="${name}_joint_1"/>
@@ -10,23 +23,18 @@
1023
<joint name="${name}_joint_6"/>
1124
<joint name="${name}_joint_7"/>
1225
</group>
13-
<group_state name="home" group="${name}">
14-
<joint name="${name}_joint_1" value="0.0174"/>
15-
<joint name="${name}_joint_2" value="-0.5725"/>
16-
<joint name="${name}_joint_3" value="-0.9846"/>
17-
<joint name="${name}_joint_4" value="0.1562"/>
18-
<joint name="${name}_joint_5" value="-1.0719"/>
19-
<joint name="${name}_joint_6" value="-1.7183"/>
20-
<joint name="${name}_joint_7" value="-1.7183"/>
21-
</group_state>
22-
<group_state name="zero" group="${name}">
23-
<joint name="${name}_joint_1" value="0"/>
24-
<joint name="${name}_joint_2" value="0"/>
25-
<joint name="${name}_joint_3" value="0"/>
26-
<joint name="${name}_joint_4" value="0"/>
27-
<joint name="${name}_joint_5" value="0"/>
28-
<joint name="${name}_joint_6" value="0"/>
29-
<joint name="${name}_joint_7" value="0"/>
30-
</group_state>
26+
27+
<xacro:kinova_gen3_7dof_group_state
28+
name="${name}"
29+
group_state="home"
30+
joint_positions="${[0.0174, -0.5725, -0.9846, 0.1562, -1.0719, -1.7183, -1.7183]}"
31+
/>
32+
33+
<xacro:kinova_gen3_7dof_group_state
34+
name="${name}"
35+
group_state="zero"
36+
joint_positions="${[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
37+
/>
38+
3139
</xacro:macro>
3240
</robot>

‎clearpath_manipulators_description/srdf/arm/kinova_gen3_lite.srdf.xacro

+26
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,17 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22
<robot xmlns:xacro="http://ros.org/wiki/xacro">
3+
4+
<xacro:macro name="kinova_gen3_lite_group_state" params="name group_state joint_positions">
5+
<group_state name="${group_state}" group="${name}">
6+
<joint name="${name}_joint_1" value="${joint_positions[0]}"/>
7+
<joint name="${name}_joint_2" value="${joint_positions[1]}"/>
8+
<joint name="${name}_joint_3" value="${joint_positions[2]}"/>
9+
<joint name="${name}_joint_4" value="${joint_positions[3]}"/>
10+
<joint name="${name}_joint_5" value="${joint_positions[4]}"/>
11+
<joint name="${name}_joint_6" value="${joint_positions[5]}"/>
12+
</group_state>
13+
</xacro:macro>
14+
315
<xacro:macro name="kinova_gen3_lite" params="name">
416
<group name="${name}">
517
<joint name="${name}_joint_1"/>
@@ -33,5 +45,19 @@
3345
<joint name="${name}_joint_5" value="0"/>
3446
<joint name="${name}_joint_6" value="0"/>
3547
</group_state>
48+
49+
50+
<xacro:kinova_gen3_lite_group_state
51+
name="${name}"
52+
group_state="home"
53+
joint_positions="${[0.0, 0.0, math.pi * 1/2, math.pi * 1/2, math.pi * 1/2, math.pi * -1/2]}"
54+
/>
55+
56+
<xacro:kinova_gen3_lite_group_state
57+
name="${name}"
58+
group_state="zero"
59+
joint_positions="${[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
60+
/>
61+
3662
</xacro:macro>
3763
</robot>

‎clearpath_manipulators_description/srdf/arm/universal_robots.srdf.xacro

+25-24
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,17 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22
<robot xmlns:xacro="http://ros.org/wiki/xacro">
3+
4+
<xacro:macro name="universal_robots_group_state" params="name group_state joint_positions">
5+
<group_state name="${group_state}" group="${name}">
6+
<joint name="${name}_shoulder_pan_joint" value="${joint_positions[0]}"/>
7+
<joint name="${name}_shoulder_lift_joint" value="${joint_positions[1]}"/>
8+
<joint name="${name}_elbow_joint" value="${joint_positions[2]}"/>
9+
<joint name="${name}_wrist_1_joint" value="${joint_positions[3]}"/>
10+
<joint name="${name}_wrist_2_joint" value="${joint_positions[4]}"/>
11+
<joint name="${name}_wrist_3_joint" value="${joint_positions[5]}"/>
12+
</group_state>
13+
</xacro:macro>
14+
315
<xacro:macro name="universal_robots" params="name">
416
<group name="${name}">
517
<joint name="${name}_shoulder_pan_joint"/>
@@ -9,29 +21,18 @@
921
<joint name="${name}_wrist_2_joint"/>
1022
<joint name="${name}_wrist_3_joint"/>
1123
</group>
12-
<group_state name="home" group="${name}">
13-
<joint name="${name}_shoulder_pan_joint" value="0.0"/>
14-
<joint name="${name}_shoulder_lift_joint" value="-1.5707"/>
15-
<joint name="${name}_elbow_joint" value="1.5707"/>
16-
<joint name="${name}_wrist_1_joint" value="-1.5707"/>
17-
<joint name="${name}_wrist_2_joint" value="-1.5707"/>
18-
<joint name="${name}_wrist_3_joint" value="0.0"/>
19-
</group_state>
20-
<group_state name="stow" group="${name}">
21-
<joint name="${name}_shoulder_pan_joint" value="0.0"/>
22-
<joint name="${name}_shoulder_lift_joint" value="-2.356"/>
23-
<joint name="${name}_elbow_joint" value="2.356"/>
24-
<joint name="${name}_wrist_1_joint" value="-1.5707"/>
25-
<joint name="${name}_wrist_2_joint" value="-1.5707"/>
26-
<joint name="${name}_wrist_3_joint" value="0.0"/>
27-
</group_state>
28-
<group_state name="zero" group="${name}">
29-
<joint name="${name}_shoulder_pan_joint" value="0"/>
30-
<joint name="${name}_shoulder_lift_joint" value="0"/>
31-
<joint name="${name}_elbow_joint" value="0"/>
32-
<joint name="${name}_wrist_1_joint" value="0"/>
33-
<joint name="${name}_wrist_2_joint" value="0"/>
34-
<joint name="${name}_wrist_3_joint" value="0"/>
35-
</group_state>
24+
25+
<xacro:universal_robots_group_state
26+
name="${name}"
27+
group_state="home"
28+
joint_positions="${[0.0, -1.5707, 1.5707, -1.5707, -1.5707, 0.0]}"
29+
/>
30+
31+
<xacro:universal_robots_group_state
32+
name="${name}"
33+
group_state="zero"
34+
joint_positions="${[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"
35+
/>
36+
3637
</xacro:macro>
3738
</robot>

‎clearpath_manipulators_description/srdf/gripper/kinova_2f_lite.srdf.xacro

+19-6
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,31 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22
<robot xmlns:xacro="http://ros.org/wiki/xacro">
3+
4+
<xacro:macro name="kinova_2f_lite_group_state" params="name group_state joint_positions">
5+
<group_state name="${group_state}" group="${name}">
6+
<joint name="${name}_right_finger_bottom_joint" value="${joint_positions[0]}"/>
7+
</group_state>
8+
</xacro:macro>
9+
310
<xacro:macro name="kinova_2f_lite" params="name">
411
<group name="${name}">
512
<joint name="${name}_right_finger_bottom_joint"/>
613
<joint name="${name}_right_finger_tip_joint"/>
714
<joint name="${name}_left_finger_bottom_joint"/>
815
<joint name="${name}_left_finger_tip_joint"/>
916
</group>
10-
<group_state name="open" group="${name}">
11-
<joint name="${name}_right_finger_bottom_joint" value="0.0"/>
12-
</group_state>
13-
<group_state name="close" group="${name}">
14-
<joint name="${name}_right_finger_bottom_joint" value="0.85"/>
15-
</group_state>
17+
18+
<xacro:kinova_2f_lite_group_state
19+
name="${name}"
20+
group_state="open"
21+
joint_positions="${[0.0]}"
22+
/>
23+
24+
<xacro:kinova_2f_lite_group_state
25+
name="${name}"
26+
group_state="close"
27+
joint_positions="${[0.85]}"
28+
/>
1629

1730
<disable_collisions link1="${name}_right_finger_dist_link" link2="${name}_left_finger_dist_link" reason="User"/>
1831
</xacro:macro>

‎clearpath_manipulators_description/srdf/gripper/robotiq_2f_140.srdf.xacro

+19-6
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,12 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22
<robot xmlns:xacro="http://ros.org/wiki/xacro">
3+
4+
<xacro:macro name="robotiq_2f_140_group_state" params="name group_state joint_positions">
5+
<group_state name="${group_state}" group="${name}">
6+
<joint name="${name}_finger_joint" value="${joint_positions[0]}"/>
7+
</group_state>
8+
</xacro:macro>
9+
310
<xacro:macro name="robotiq_2f_140" params="name">
411
<group name="${name}">
512
<joint name="${name}_finger_joint"/>
@@ -9,12 +16,18 @@
916
<joint name="${name}_right_inner_knuckle_joint"/>
1017
<joint name="${name}_right_inner_finger_joint"/>
1118
</group>
12-
<group_state name="open" group="${name}">
13-
<joint name="${name}_finger_joint" value="0.0"/>
14-
</group_state>
15-
<group_state name="close" group="${name}">
16-
<joint name="${name}_finger_joint" value="0.698"/>
17-
</group_state>
19+
20+
<xacro:robotiq_2f_140_group_state
21+
name="${name}"
22+
group_state="open"
23+
joint_positions="${[0.0]}"
24+
/>
25+
26+
<xacro:robotiq_2f_140_group_state
27+
name="${name}"
28+
group_state="close"
29+
joint_positions="${[0.7]}"
30+
/>
1831

1932
<disable_collisions link1="${name}_left_inner_knuckle" link2="${name}_left_inner_finger" reason="User"/>
2033
<disable_collisions link1="${name}_right_inner_knuckle" link2="${name}_right_inner_finger" reason="User"/>

‎clearpath_manipulators_description/srdf/gripper/robotiq_2f_85.srdf.xacro

+19-7
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,12 @@
11
<?xml version="1.0" encoding="UTF-8"?>
22
<robot xmlns:xacro="http://ros.org/wiki/xacro">
3+
4+
<xacro:macro name="robotiq_2f_85_group_state" params="name group_state joint_positions">
5+
<group_state name="${group_state}" group="${name}">
6+
<joint name="${name}_robotiq_85_left_knuckle_joint" value="${joint_positions[0]}"/>
7+
</group_state>
8+
</xacro:macro>
9+
310
<xacro:macro name="robotiq_2f_85" params="name">
411
<group name="${name}">
512
<joint name="${name}_robotiq_85_base_joint"/>
@@ -12,16 +19,21 @@
1219
<joint name="${name}_robotiq_85_right_finger_joint"/>
1320
<joint name="${name}_robotiq_85_right_finger_tip_joint"/>
1421
</group>
15-
<group_state name="open" group="${name}">
16-
<joint name="${name}_robotiq_85_left_knuckle_joint" value="0.0"/>
17-
</group_state>
18-
<group_state name="close" group="${name}">
19-
<joint name="${name}_robotiq_85_left_knuckle_joint" value="0.8"/>
20-
</group_state>
22+
23+
<xacro:robotiq_2f_85_group_state
24+
name="${name}"
25+
group_state="open"
26+
joint_positions="${[0.0]}"
27+
/>
28+
29+
<xacro:robotiq_2f_85_group_state
30+
name="${name}"
31+
group_state="close"
32+
joint_positions="${[0.8]}"
33+
/>
2134

2235
<disable_collisions link1="${name}_robotiq_85_left_inner_knuckle_link" link2="${name}_robotiq_85_left_finger_tip_link" reason="User"/>
2336
<disable_collisions link1="${name}_robotiq_85_right_inner_knuckle_link" link2="${name}_robotiq_85_right_finger_tip_link" reason="User"/>
24-
2537
</xacro:macro>
2638

2739
</robot>

0 commit comments

Comments
 (0)