Skip to content

Commit

Permalink
GazeboでのMoveIt動作に対応 (#6)
Browse files Browse the repository at this point in the history 
* control_configパス追加

* ign_ros2_control追加

* dummy_mimic joint追加

* test修正

* joint_limit値の調整

* 干渉が生じるモデルを修正

* Gazebo上での初期姿勢設定

* 関節可動範囲の調整
  • Loading branch information
@Kuwamai
Kuwamai authored Oct 26, 2023
1 parent e8e16dc commit 490345a
Show file tree
Hide file tree
Showing 8 changed files with 312 additions and 7 deletions.
Binary file modified meshes/collision/L_Link5.stl
View file
Binary file not shown.
6 changes: 5 additions & 1 deletion sciurus17_description/robot_description_loader.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,14 @@ def __init__(self):
'urdf',
'sciurus17.urdf.xacro')
self.use_gazebo = 'false'
self.gz_control_config_package = ''
self.gz_control_config_file_path = ''

def load(self):
return Command([
'xacro ',
self.robot_description_path,
' use_gazebo:=', self.use_gazebo
' use_gazebo:=', self.use_gazebo,
' gz_control_config_package:=', self.gz_control_config_package,
' gz_control_config_file_path:=', self.gz_control_config_file_path
])
6 changes: 4 additions & 2 deletions test/test_robot_description_loader.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,9 @@ def test_change_description_path():


def test_use_gazebo():
# use_gazeboが変更され、xacroにgazeboタグがセットされることを期待
# use_gazeboが変更され、xacroにign_ros2_controlがセットされることを期待
rdl = RobotDescriptionLoader()
rdl.use_gazebo = 'true'
assert '<gazebo' in exec_load(rdl)
rdl.gz_control_config_package = 'sciurus17_description'
rdl.gz_control_config_file_path = 'config/dummy_controllers.yaml'
assert 'ign_ros2_control/IgnitionSystem' in exec_load(rdl)
259 changes: 259 additions & 0 deletions urdf/sciurus17.gazebo_ros2_control.xacro
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,259 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

<xacro:macro name="sciurus17_gazebo_ros2_control_settings">

<ros2_control name="sciurus17" type="system">
<hardware>
<plugin>ign_ros2_control/IgnitionSystem</plugin>
</hardware>

<joint name="${NAME_JOINT_BODY}">
<command_interface name="position">
<param name="min">${NAME_JOINT_BODY_LOWER_LIMIT}</param>
<param name="max">${NAME_JOINT_BODY_UPPER_LIMIT}</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_NECK_1}">
<command_interface name="position">
<param name="min">${JOINT_NECK_1_LOWER_LIMIT}"</param>
<param name="max">${JOINT_NECK_1_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_NECK_2}">
<command_interface name="position">
<param name="min">${JOINT_NECK_2_LOWER_LIMIT}"</param>
<param name="max">${JOINT_NECK_2_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_R_1}">
<command_interface name="position">
<param name="min">${JOINT_ARM_R_1_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_R_1_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_R_2}">
<command_interface name="position">
<param name="min">${JOINT_ARM_R_2_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_R_2_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">-1.5707</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_R_3}">
<command_interface name="position">
<param name="min">${JOINT_ARM_R_3_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_R_3_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_R_4}">
<command_interface name="position">
<param name="min">${JOINT_ARM_R_4_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_R_4_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">2.7262</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_R_5}">
<command_interface name="position">
<param name="min">${JOINT_ARM_R_5_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_R_5_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_R_6}">
<command_interface name="position">
<param name="min">${JOINT_ARM_R_6_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_R_6_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">-2.0943</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_R_7}">
<command_interface name="position">
<param name="min">${JOINT_ARM_R_7_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_R_7_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_HAND_R_A}">
<command_interface name="position">
<param name="min">${JOINT_HAND_R_LOWER_LIMIT}"</param>
<param name="max">${JOINT_HAND_R_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_HAND_R_B}">
<param name="mimic">${NAME_JOINT_HAND_R_A}</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_L_1}">
<command_interface name="position">
<param name="min">${JOINT_ARM_L_1_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_L_1_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_L_2}">
<command_interface name="position">
<param name="min">${JOINT_ARM_L_2_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_L_2_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">1.5707</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_L_3}">
<command_interface name="position">
<param name="min">${JOINT_ARM_L_3_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_L_3_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_L_4}">
<command_interface name="position">
<param name="min">${JOINT_ARM_L_4_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_L_4_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">-2.7262</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_L_5}">
<command_interface name="position">
<param name="min">${JOINT_ARM_L_5_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_L_5_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_L_6}">
<command_interface name="position">
<param name="min">${JOINT_ARM_L_6_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_L_6_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">2.09439</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_ARM_L_7}">
<command_interface name="position">
<param name="min">${JOINT_ARM_L_7_LOWER_LIMIT}"</param>
<param name="max">${JOINT_ARM_L_7_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_HAND_L_A}">
<command_interface name="position">
<param name="min">${JOINT_HAND_L_LOWER_LIMIT}"</param>
<param name="max">${JOINT_HAND_L_UPPER_LIMIT}"</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${NAME_JOINT_HAND_L_B}">
<param name="mimic">${NAME_JOINT_HAND_L_A}</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

</ros2_control>
</xacro:macro>
</robot>
25 changes: 21 additions & 4 deletions urdf/sciurus17.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,13 @@
<xacro:include filename="$(find sciurus17_description)/urdf/sciurus17_left_arm.xacro"/>
<xacro:include filename="$(find sciurus17_description)/urdf/sciurus17_left_hand.xacro"/>
<xacro:include filename="$(find sciurus17_description)/urdf/sciurus17_gazebo.xacro"/>
<xacro:include filename="$(find sciurus17_description)/urdf/sciurus17.gazebo_ros2_control.xacro"/>

<xacro:arg name="use_gazebo" default="false" />
<xacro:arg name="gz_control_config_package" default="" />
<xacro:arg name="gz_control_config_file_path" default="" />
<xacro:property name="GZ_CONTROL_CONFIG_PACKAGE" value="$(arg gz_control_config_package)"/>
<xacro:property name="GZ_CONTROL_CONFIG_FILE_PATH" value="$(arg gz_control_config_file_path)"/>

<material name="white">
<color rgba="0.9 0.9 0.9 1.0"/>
Expand Down Expand Up @@ -104,15 +109,19 @@
<xacro:property name="JOINT_ARM_R_2_UPPER_LIMIT" value="${radians(0)}"/>
<xacro:property name="JOINT_ARM_R_3_LOWER_LIMIT" value="${radians(-90)}"/>
<xacro:property name="JOINT_ARM_R_3_UPPER_LIMIT" value="${radians(90)}"/>
<xacro:property name="JOINT_ARM_R_4_LOWER_LIMIT" value="${radians(0)}"/>
<!-- Gazeboの挙動がlimit付近で不安定なため、limitをサーボの可動範囲より少し大きくする -->
<!-- 参照: https://github.com/rt-net/sciurus17_description/pull/6 -->
<xacro:property name="JOINT_ARM_R_4_LOWER_LIMIT" value="${radians(-0.01)}"/>
<xacro:property name="JOINT_ARM_R_4_UPPER_LIMIT" value="${radians(157)}"/>
<xacro:property name="JOINT_ARM_R_5_LOWER_LIMIT" value="${radians(-90)}"/>
<xacro:property name="JOINT_ARM_R_5_UPPER_LIMIT" value="${radians(90)}"/>
<xacro:property name="JOINT_ARM_R_6_LOWER_LIMIT" value="${radians(-120)}"/>
<xacro:property name="JOINT_ARM_R_6_UPPER_LIMIT" value="${radians(60)}"/>
<xacro:property name="JOINT_ARM_R_7_LOWER_LIMIT" value="${radians(-170)}"/>
<xacro:property name="JOINT_ARM_R_7_UPPER_LIMIT" value="${radians(170)}"/>
<xacro:property name="JOINT_HAND_R_LOWER_LIMIT" value="${radians(0)}"/>
<!-- Gazeboの挙動がlimit付近で不安定なため、limitをサーボの可動範囲より少し大きくする -->
<!-- 参照: https://github.com/rt-net/sciurus17_description/pull/6 -->
<xacro:property name="JOINT_HAND_R_LOWER_LIMIT" value="${radians(-0.01)}"/>
<xacro:property name="JOINT_HAND_R_UPPER_LIMIT" value="${radians(86)}"/>

<xacro:property name="JOINT_ARM_L_1_LOWER_LIMIT" value="${radians(-90)}"/>
Expand All @@ -122,15 +131,19 @@
<xacro:property name="JOINT_ARM_L_3_LOWER_LIMIT" value="${radians(-90)}"/>
<xacro:property name="JOINT_ARM_L_3_UPPER_LIMIT" value="${radians(90)}"/>
<xacro:property name="JOINT_ARM_L_4_LOWER_LIMIT" value="${radians(-157)}"/>
<xacro:property name="JOINT_ARM_L_4_UPPER_LIMIT" value="${radians(0)}"/>
<!-- Gazeboの挙動がlimit付近で不安定なため、limitをサーボの可動範囲より少し大きくする -->
<!-- 参照: https://github.com/rt-net/sciurus17_description/pull/6 -->
<xacro:property name="JOINT_ARM_L_4_UPPER_LIMIT" value="${radians(0.01)}"/>
<xacro:property name="JOINT_ARM_L_5_LOWER_LIMIT" value="${radians(-90)}"/>
<xacro:property name="JOINT_ARM_L_5_UPPER_LIMIT" value="${radians(90)}"/>
<xacro:property name="JOINT_ARM_L_6_LOWER_LIMIT" value="${radians(-60)}"/>
<xacro:property name="JOINT_ARM_L_6_UPPER_LIMIT" value="${radians(120)}"/>
<xacro:property name="JOINT_ARM_L_7_LOWER_LIMIT" value="${radians(-170)}"/>
<xacro:property name="JOINT_ARM_L_7_UPPER_LIMIT" value="${radians(170)}"/>
<xacro:property name="JOINT_HAND_L_LOWER_LIMIT" value="${radians(-86)}"/>
<xacro:property name="JOINT_HAND_L_UPPER_LIMIT" value="${radians(0)}"/>
<!-- Gazeboの挙動がlimit付近で不安定なため、limitをサーボの可動範囲より少し大きくする -->
<!-- 参照: https://github.com/rt-net/sciurus17_description/pull/6 -->
<xacro:property name="JOINT_HAND_L_UPPER_LIMIT" value="${radians(0.01)}"/>

<xacro:property name="COLOR_LINK_BASE" value="white"/>
<xacro:property name="COLOR_LINK_BODY" value="white"/>
Expand Down Expand Up @@ -160,6 +173,10 @@
<xacro:sciurus17_left_hand/>

<xacro:if value="$(arg use_gazebo)">

<xacro:gazebo_robot_settings/>

<xacro:sciurus17_gazebo_ros2_control_settings/>

</xacro:if>
</robot>
5 changes: 5 additions & 0 deletions urdf/sciurus17_gazebo.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,11 @@
</xacro:macro>

<xacro:macro name="gazebo_robot_settings">
<gazebo>
<plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
<parameters>$(find ${GZ_CONTROL_CONFIG_PACKAGE})/${GZ_CONTROL_CONFIG_FILE_PATH}</parameters>
</plugin>
</gazebo>

<gazebo reference="${NAME_LINK_BASE}">
<xacro:material_gazebo_white/>
Expand Down
9 changes: 9 additions & 0 deletions urdf/sciurus17_left_hand.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,15 @@
</inertial>
</link>

<!--Dummy joint/link to prevent error with mimic joint
See https://github.com/ros-controls/gazebo_ros2_control/issues/173
-->
<joint name="${NAME_JOINT_HAND_L_B}_mimic" type="fixed">
<parent link="world" />
<child link="dummy_mimic_fix_l" />
</joint>
<link name="dummy_mimic_fix_l"/>

<joint name="${NAME_JOINT_HAND_L_B}" type="revolute">
<parent link="${NAME_LINK_ARM_L_7}" />
<child link="${NAME_LINK_HAND_L_B}" />
Expand Down
Loading

0 comments on commit 490345a

Please sign in to comment.