Skip to content

Commit acaed57

Browse files
mhubiimhubii
authored
Merge pull request #290 from lbr-stack/dev-rolling-initial-positions
Dev rolling initial positions
2 parents 5df4103 + b3dd7da commit acaed57

File tree

14 files changed

+97
-32
lines changed

14 files changed

+97
-32
lines changed

‎lbr_bringup/launch/gazebo.launch.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ def generate_launch_description() -> LaunchDescription:
1111
# launch arguments
1212
ld.add_action(LBRDescriptionMixin.arg_model())
1313
ld.add_action(LBRDescriptionMixin.arg_robot_name())
14+
ld.add_action(LBRROS2ControlMixin.arg_init_jnt_pos())
1415
ld.add_action(
1516
LBRROS2ControlMixin.arg_ctrl()
1617
) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/ros2_control/lbr_controllers.yaml

‎lbr_bringup/launch/mock.launch.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ def generate_launch_description() -> LaunchDescription:
1515
ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg())
1616
ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
1717
ld.add_action(LBRROS2ControlMixin.arg_ctrl())
18+
ld.add_action(LBRROS2ControlMixin.arg_init_jnt_pos())
1819

1920
# robot description
2021
robot_description = LBRDescriptionMixin.param_robot_description(mode="mock")

‎lbr_bringup/lbr_bringup/description.py

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,18 @@ def param_robot_description(
3535
),
3636
]
3737
),
38+
initial_joint_positions_path: Optional[
39+
Union[LaunchConfiguration, str]
40+
] = PathJoinSubstitution(
41+
[
42+
FindPackageShare(
43+
LaunchConfiguration("sys_cfg_pkg", default="lbr_description")
44+
),
45+
LaunchConfiguration(
46+
"init_jnt_pos", default="ros2_control/initial_joint_positions.yaml"
47+
),
48+
]
49+
),
3850
) -> Dict[str, str]:
3951
robot_description = {
4052
"robot_description": Command(
@@ -56,6 +68,8 @@ def param_robot_description(
5668
mode,
5769
" system_config_path:=",
5870
system_config_path,
71+
" initial_joint_positions_path:=",
72+
initial_joint_positions_path,
5973
]
6074
)
6175
}

‎lbr_bringup/lbr_bringup/ros2_control.py

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,14 @@ def arg_sys_cfg() -> DeclareLaunchArgument:
5656
description="The relative path from sys_cfg_pkg to the lbr_system_config.yaml file.",
5757
)
5858

59+
@staticmethod
60+
def arg_init_jnt_pos() -> DeclareLaunchArgument:
61+
return DeclareLaunchArgument(
62+
name="init_jnt_pos",
63+
default_value="ros2_control/initial_joint_positions.yaml",
64+
description="The relative path from sys_cfg_pkg to the initial_joint_positions.yaml file.",
65+
)
66+
5967
@staticmethod
6068
def arg_use_sim_time() -> DeclareLaunchArgument:
6169
return DeclareLaunchArgument(

‎lbr_description/ros2_control/initial_joint_positions.yaml

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# initial joint positions [degrees]
2+
A1 : 0.0
3+
A2 : 25.0
4+
A3 : 0.0
5+
A4 : 90.0
6+
A5 : 0.0
7+
A6 : 0.0
8+
A7 : 0.0

‎lbr_description/ros2_control/lbr_system_interface.xacro

Lines changed: 25 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,15 @@
11
<?xml version="1.0"?>
22
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
33
<xacro:macro name="lbr_system_interface"
4-
params="robot_name mode joint_limits system_config_path">
4+
params="robot_name mode joint_limits system_config_path initial_joint_positions_path">
55
<!-- load system parameters via yaml -->
66
<xacro:property name="system_config"
77
value="${xacro.load_yaml(system_config_path)}" />
8-
8+
9+
<!-- load initial positions via yaml for mock/gazebo simulation -->
10+
<xacro:property name="initial_joint_positions"
11+
value="${xacro.load_yaml(initial_joint_positions_path)}" />
12+
913
<ros2_control name="${robot_name}_system_interface" type="system">
1014
<!-- load plugin depending on mode -->
1115
<xacro:if value="${mode == 'mock'}">
@@ -92,7 +96,7 @@
9296

9397
<!-- define joints and command/state interfaces for each joint -->
9498
<xacro:macro name="joint_interface"
95-
params="name min_position max_position max_velocity max_torque mode">
99+
params="name min_position max_position max_velocity max_torque mode initial_position">
96100
<joint name="${name}">
97101
<command_interface name="position">
98102
<param name="min">${min_position}</param>
@@ -106,7 +110,9 @@
106110
<param name="max"> ${max_torque}</param>
107111
</command_interface>
108112
</xacro:unless>
109-
<state_interface name="position" />
113+
<state_interface name="position">
114+
<param name="initial_value">${initial_position}</param>
115+
</state_interface>
110116
<state_interface name="velocity" />
111117
<state_interface name="effort" />
112118
<xacro:if value="${mode == 'hardware'}">
@@ -130,43 +136,50 @@
130136
max_position="${joint_limits['A1']['upper'] * PI / 180}"
131137
max_velocity="${joint_limits['A1']['velocity'] * PI / 180}"
132138
max_torque="${joint_limits['A1']['effort']}"
133-
mode="${mode}" />
139+
mode="${mode}"
140+
initial_position="${initial_joint_positions['A1'] * PI / 180}" />
134141
<xacro:joint_interface name="${robot_name}_A2"
135142
min_position="${joint_limits['A2']['lower'] * PI / 180}"
136143
max_position="${joint_limits['A2']['upper'] * PI / 180}"
137144
max_velocity="${joint_limits['A2']['velocity'] * PI / 180}"
138145
max_torque="${joint_limits['A2']['effort']}"
139-
mode="${mode}" />
146+
mode="${mode}"
147+
initial_position="${initial_joint_positions['A2'] * PI / 180}" />
140148
<xacro:joint_interface name="${robot_name}_A3"
141149
min_position="${joint_limits['A3']['lower'] * PI / 180}"
142150
max_position="${joint_limits['A3']['upper'] * PI / 180}"
143151
max_velocity="${joint_limits['A3']['velocity'] * PI / 180}"
144152
max_torque="${joint_limits['A3']['effort']}"
145-
mode="${mode}" />
153+
mode="${mode}"
154+
initial_position="${initial_joint_positions['A3'] * PI / 180}" />
146155
<xacro:joint_interface name="${robot_name}_A4"
147156
min_position="${joint_limits['A4']['lower'] * PI / 180}"
148157
max_position="${joint_limits['A4']['upper'] * PI / 180}"
149158
max_velocity="${joint_limits['A4']['velocity'] * PI / 180}"
150159
max_torque="${joint_limits['A4']['effort']}"
151-
mode="${mode}" />
160+
mode="${mode}"
161+
initial_position="${initial_joint_positions['A4'] * PI / 180}"/>
152162
<xacro:joint_interface name="${robot_name}_A5"
153163
min_position="${joint_limits['A5']['lower'] * PI / 180}"
154164
max_position="${joint_limits['A5']['upper'] * PI / 180}"
155165
max_velocity="${joint_limits['A5']['velocity'] * PI / 180}"
156166
max_torque="${joint_limits['A5']['effort']}"
157-
mode="${mode}" />
167+
mode="${mode}"
168+
initial_position="${initial_joint_positions['A5'] * PI / 180}"/>
158169
<xacro:joint_interface name="${robot_name}_A6"
159170
min_position="${joint_limits['A6']['lower'] * PI / 180}"
160171
max_position="${joint_limits['A6']['upper'] * PI / 180}"
161172
max_velocity="${joint_limits['A6']['velocity'] * PI / 180}"
162173
max_torque="${joint_limits['A6']['effort']}"
163-
mode="${mode}" />
174+
mode="${mode}"
175+
initial_position="${initial_joint_positions['A6'] * PI / 180}" />
164176
<xacro:joint_interface name="${robot_name}_A7"
165177
min_position="${joint_limits['A7']['lower'] * PI / 180}"
166178
max_position="${joint_limits['A7']['upper'] * PI / 180}"
167179
max_velocity="${joint_limits['A7']['velocity'] * PI / 180}"
168180
max_torque="${joint_limits['A7']['effort']}"
169-
mode="${mode}" />
181+
mode="${mode}"
182+
initial_position="${initial_joint_positions['A7'] * PI / 180}" />
170183
</ros2_control>
171184
</xacro:macro>
172-
</robot>
185+
</robot>

‎lbr_description/urdf/iiwa14/iiwa14.xacro

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,9 @@
1111
<xacro:arg
1212
name="system_config_path"
1313
default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" />
14+
<xacro:arg
15+
name="initial_joint_positions_path"
16+
default="$(find lbr_description)/ros2_control/initial_joint_positions.yaml" />
1417

1518
<!-- KDL requires a link without inertia / Gazebo requires a connection to world link or robot
1619
will tipple https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/230 -->
@@ -27,5 +30,6 @@
2730
<xacro:iiwa14
2831
robot_name="$(arg robot_name)"
2932
mode="$(arg mode)"
30-
system_config_path="$(arg system_config_path)" />
31-
</robot>
33+
system_config_path="$(arg system_config_path)"
34+
initial_joint_positions_path="$(arg initial_joint_positions_path)" />
35+
</robot>

‎lbr_description/urdf/iiwa14/iiwa14_description.xacro

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
44
<!-- robot as extracted via https://github.com/syuntoku14/fusion2urdf -->
55
<xacro:macro name="iiwa14"
6-
params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml'">
6+
params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml' initial_joint_positions_path:=^|'$(find lbr_description)/ros2_control/initial_joint_positions.yaml'">
77

88
<!-- includes -->
99
<xacro:include filename="$(find lbr_description)/gazebo/lbr_gazebo.xacro" />
@@ -25,7 +25,8 @@
2525
robot_name="${robot_name}"
2626
mode="${mode}"
2727
joint_limits="${joint_limits}"
28-
system_config_path="${system_config_path}" />
28+
system_config_path="${system_config_path}"
29+
initial_joint_positions_path="${initial_joint_positions_path}" />
2930

3031
<link name="${robot_name}_link_0">
3132
<inertial>
@@ -294,4 +295,4 @@
294295
<link name="${robot_name}_link_ee">
295296
</link>
296297
</xacro:macro>
297-
</robot>
298+
</robot>

‎lbr_description/urdf/iiwa7/iiwa7.xacro

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,9 @@
1111
<xacro:arg
1212
name="system_config_path"
1313
default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" />
14+
<xacro:arg
15+
name="initial_joint_positions_path"
16+
default="$(find lbr_description)/ros2_control/initial_joint_positions.yaml" />
1417

1518
<!-- KDL requires a link without inertia / Gazebo requires a connection to world link or robot
1619
will tipple https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/230 -->
@@ -27,5 +30,6 @@
2730
<xacro:iiwa7
2831
robot_name="$(arg robot_name)"
2932
mode="$(arg mode)"
30-
system_config_path="$(arg system_config_path)" />
31-
</robot>
33+
system_config_path="$(arg system_config_path)"
34+
initial_joint_positions_path="$(arg initial_joint_positions_path)" />
35+
</robot>

‎lbr_description/urdf/iiwa7/iiwa7_description.xacro

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
44
<!-- robot as extracted via https://github.com/syuntoku14/fusion2urdf -->
55
<xacro:macro name="iiwa7"
6-
params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml'">
6+
params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml' initial_joint_positions_path:=^|'$(find lbr_description)/ros2_control/initial_joint_positions.yaml'">
77

88
<!-- includes -->
99
<xacro:include filename="$(find lbr_description)/gazebo/lbr_gazebo.xacro" />
@@ -25,7 +25,8 @@
2525
robot_name="${robot_name}"
2626
mode="${mode}"
2727
joint_limits="${joint_limits}"
28-
system_config_path="${system_config_path}" />
28+
system_config_path="${system_config_path}"
29+
initial_joint_positions_path="${initial_joint_positions_path}" />
2930

3031
<link name="${robot_name}_link_0">
3132
<inertial>
@@ -295,4 +296,4 @@
295296
<link name="${robot_name}_link_ee">
296297
</link>
297298
</xacro:macro>
298-
</robot>
299+
</robot>

0 commit comments

Comments
 (0)