--- /dev/null
+<!--******************************************************************************
+ Barret Hand Bringup
+ Ignition Launch File
+ Copyright (C) 2023 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Geneal Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************-->
+
+<launch>
+ <arg name="pause" default="true"/>
+ <arg name="gui" default="true"/>
+ <arg name="use_sim_time" default="true"/>
+
+ <arg name="controller" default="bhand_controller"/>
+ <arg name="config" default="$(find-pkg-share bhand_bringup)/config/$(var controller).yaml"/>
+
+ <include file="$(find-pkg-share bhand_description)/launch/ignition.launch.xml" >
+ <arg name="pause" value="$(var pause)"/>
+ <arg name="gui" value="$(var gui)"/>
+ <arg name="use_sim_time" value="$(var use_sim_time)"/>
+ </include>
+
+ <include file="$(find-pkg-share bhand_bringup)/launch/$(var controller).launch.xml" >
+ <arg name="config" value="$(var config)"/>
+ <arg name="use_sim_time" value="$(var use_sim_time)"/>
+ </include>
+</launch>
<launch>
<arg name="world" default="false"/>
<arg name="use_sim_time" default="false"/>
+ <arg name="ignition" default="false"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
- <param name="robot_description" value="$(command 'xacro $(find-pkg-share bhand_description)/xacro/bhand.urdf.xacro world:=$(var world)')" type="str"/>
+ <param name="robot_description" value="$(command 'xacro $(find-pkg-share bhand_description)/xacro/bhand.urdf.xacro world:=$(var world) ignition:=$(var ignition)')" type="str"/>
<param name="use_sim_time" value="$(var use_sim_time)"/>
</node>
</launch>
<!--******************************************************************************
Bhand Description
- Gazebo Ignitio Launch File
- Copyright (C) 2022 Walter Fetter Lages <w.fetter@ieee.org>
+ Gazebo Ignition Launch File
+ Copyright (C) 2022, 2023 Walter Fetter Lages <w.fetter@ieee.org>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
<arg name="use_sim_time" default="true"/>
<arg name="world" default="true"/>
- <arg unless="$(var pause)" name="ign_pause" default="-r"/>
- <arg if="$(var pause)" name="ign_pause" default=""/>
+ <arg unless="$(var pause)" name="gz_pause" default="-r"/>
+ <arg if="$(var pause)" name="gz_pause" default=""/>
- <arg unless="$(var gui)" name="ign_gui" default="-s"/>
- <arg if="$(var gui)" name="ign_gui" default=""/>
+ <arg unless="$(var gui)" name="gz_gui" default="-s"/>
+ <arg if="$(var gui)" name="gz_gui" default=""/>
- <include file="$(find-pkg-share ros_ign_gazebo)/launch/ign_gazebo.launch.py">
- <arg name="ign_args" value="$(var ign_pause) $(var ign_gui) empty.sdf"/>
+ <include file="$(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py">
+ <arg name="gz_args" value="$(var gz_pause) $(var gz_gui) empty.sdf"/>
</include>
<include file="$(find-pkg-share bhand_description)/launch/bhand.launch.xml">
<arg name="world" value="$(var world)"/>
<arg name="use_sim_time" value="$(var use_sim_time)"/>
+ <arg name="ignition" value="true"/>
</include>
- <node name="bhand_spawner" pkg="ros_ign_gazebo" exec="create" args="-topic robot_description -name bhand"/>
+ <node name="bhand_spawner" pkg="ros_gz_sim" exec="create" args="-topic robot_description -name bhand"/>
- <node name="clock_bridge" pkg="ros_ign_bridge" exec="parameter_bridge" args="/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock">
+ <node name="clock_bridge" pkg="ros_gz_bridge" exec="parameter_bridge" args="/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock">
<param name="use_sim_time" value="$(var use_sim_time)"/>
</node>
</launch>
<?xml version="1.0"?>
<robot name="bhand" xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:arg name="world" default="false" />
+ <xacro:arg name="world" default="false"/>
+ <xacro:arg name="ignition" default="false"/>
- <xacro:property name="M_PI" value="3.1415926535897931" />
+ <xacro:property name="M_PI" value="3.1415926535897931"/>
- <xacro:include filename="$(find bhand_description)/xacro/bhand_base.urdf.xacro" />
- <xacro:include filename="$(find bhand_description)/xacro/bhand_link1.urdf.xacro" />
- <xacro:include filename="$(find bhand_description)/xacro/bhand_link2.urdf.xacro" />
- <xacro:include filename="$(find bhand_description)/xacro/bhand_link3.urdf.xacro" />
+ <xacro:include filename="$(find bhand_description)/xacro/bhand_base.urdf.xacro"/>
+ <xacro:include filename="$(find bhand_description)/xacro/bhand_link1.urdf.xacro"/>
+ <xacro:include filename="$(find bhand_description)/xacro/bhand_link2.urdf.xacro"/>
+ <xacro:include filename="$(find bhand_description)/xacro/bhand_link3.urdf.xacro"/>
<link name="bhand_origin"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
</inertial_attr>
- <origin xyz="-0.02475 0.0 ${0.1325-0.06}" rpy="0.0 0.0 0.0" />
- <limit effort="30.0" velocity="1.5" lower="0" upper="${M_PI}" />
+ <origin xyz="-0.02475 0.0 ${0.1325-0.06}" rpy="0.0 0.0 0.0"/>
+ <limit effort="30.0" velocity="1.5" lower="0" upper="${M_PI}"/>
<mimic_attr/>
</xacro:bhand_link1>
- <xacro:bhand_transmission joint="spread" />
+ <xacro:bhand_transmission joint="spread"/>
<xacro:bhand_link1 parent="base_link" link="finger1_link_1" joint="spread_mimic" type="revolute">
<inertial_attr>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
</inertial_attr>
- <origin xyz="0.02475 0.0 ${0.1325-0.06}" rpy="0.0 0.0 0.0" />
- <limit effort="30" velocity="1.5" lower="-${M_PI}" upper="0" />
+ <origin xyz="0.02475 0.0 ${0.1325-0.06}" rpy="0.0 0.0 0.0"/>
+ <limit effort="30" velocity="1.5" lower="-${M_PI}" upper="0"/>
<mimic_attr>
- <mimic joint="bhand_spread" multiplier="-1.0" offset="0.0" />
+ <mimic joint="bhand_spread" multiplier="-1.0" offset="0.0"/>
</mimic_attr>
</xacro:bhand_link1>
- <xacro:bhand_transmission joint="spread_mimic" />
+ <xacro:bhand_transmission joint="spread_mimic"/>
<xacro:bhand_link1 parent="base_link" link="finger3_link_1" joint="finger3_joint_1" type="fixed">
<inertial_attr>
</inertial>
</inertial_attr>
<!--inertial_attr/-->
- <origin xyz="0.0 0.0 ${0.1325-0.06}" rpy="0.0 0.0 ${M_PI}" />
- <limit effort="0.0" velocity="0.0" lower="0.0" upper="0.0" />
+ <origin xyz="0.0 0.0 ${0.1325-0.06}" rpy="0.0 0.0 ${M_PI}"/>
+ <limit effort="0.0" velocity="0.0" lower="0.0" upper="0.0"/>
<mimic_attr/>
</xacro:bhand_link1>
- <xacro:bhand_link2 parent="finger1_link_1" link="finger1_link_2" joint="finger1_joint_2" />
- <xacro:bhand_link2 parent="finger2_link_1" link="finger2_link_2" joint="finger2_joint_2" />
- <xacro:bhand_link2 parent="finger3_link_1" link="finger3_link_2" joint="finger3_joint_2" />
+ <xacro:bhand_link2 parent="finger1_link_1" link="finger1_link_2" joint="finger1_joint_2"/>
+ <xacro:bhand_link2 parent="finger2_link_1" link="finger2_link_2" joint="finger2_joint_2"/>
+ <xacro:bhand_link2 parent="finger3_link_1" link="finger3_link_2" joint="finger3_joint_2"/>
- <xacro:bhand_link3 parent="finger1_link_2" link="finger1_link_3" joint="finger1_joint_3" mimic_joint="finger1_joint_2" />
- <xacro:bhand_link3 parent="finger2_link_2" link="finger2_link_3" joint="finger2_joint_3" mimic_joint="finger2_joint_2" />
- <xacro:bhand_link3 parent="finger3_link_2" link="finger3_link_3" joint="finger3_joint_3" mimic_joint="finger3_joint_2" />
+ <xacro:bhand_link3 parent="finger1_link_2" link="finger1_link_3" joint="finger1_joint_3" mimic_joint="finger1_joint_2"/>
+ <xacro:bhand_link3 parent="finger2_link_2" link="finger2_link_3" joint="finger2_joint_3" mimic_joint="finger2_joint_2"/>
+ <xacro:bhand_link3 parent="finger3_link_2" link="finger3_link_3" joint="finger3_joint_3" mimic_joint="finger3_joint_2"/>
- <xacro:if value="$(arg world)" >
+ <xacro:if value="$(arg world)">
- <link name="world" />
+ <link name="world"/>
<joint name="bhand_origin_joint" type="fixed">
<parent link="world"/>
- <child link="bhand_origin" />
- <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+ <child link="bhand_origin"/>
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
</joint>
</xacro:if>
- <ros2_control name="BhandSystem" type="system">
- <hardware>
- <plugin>gazebo_ros2_control/GazeboSystem</plugin>
- </hardware>
-
- <joint name="bhand_finger1_joint_2">
- <command_interface name="position">
- <param name="min">0.0</param>
- <param name="max">${140 * M_PI /180}</param>
- </command_interface>
- <state_interface name="position"/>
- </joint>
-
- <joint name="bhand_finger1_joint_3">
- <param name="mimic">bhand_finger1_joint_2</param>
- <param name="multiplier">${48 / 140}</param>
- <command_interface name="position"/>
- <state_interface name="position"/>
- </joint>
-
- <joint name="bhand_finger2_joint_2">
- <command_interface name="position">
- <param name="min">0,9</param>
- <param name="max">${140 * M_PI /180}</param>
- </command_interface>
- <state_interface name="position"/>
- </joint>
-
- <joint name="bhand_finger2_joint_3">
- <param name="mimic">bhand_finger2_joint_2</param>
- <param name="multiplier">${48 / 140}</param>
- <command_interface name="position"/>
- <state_interface name="position"/>
- </joint>
-
- <joint name="bhand_finger3_joint_2">
- <command_interface name="position">
- <param name="min">0.0</param>
- <param name="max">${140 * M_PI /180}</param>
- </command_interface>
- <state_interface name="position"/>
- </joint>
-
- <joint name="bhand_finger3_joint_3">
- <param name="mimic">bhand_finger3_joint_2</param>
- <param name="multiplier">${48 / 140}</param>
- <command_interface name="position"/>
- <state_interface name="position"/>
- </joint>
-
- <joint name="bhand_spread">
- <command_interface name="position">
- <param name="min">0.0</param>
- <param name="max">${M_PI}</param>
- </command_interface>
- <state_interface name="position"/>
- </joint>
-
- <joint name="bhand_spread_mimic">
- <param name="mimic">bhand_spread</param>
- <param name="multiplier">-1.0</param>
- <command_interface name="position"/>
- <state_interface name="position"/>
- </joint>
- </ros2_control>
-
+ <xacro:include filename="ros2_control.urdf.xacro"/>
+
<xacro:if value="$(arg world)">
- <gazebo>
- <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
- <robot_param>robot_description</robot_param>
- <robot_param_node>robot_state_publisher</robot_param_node>
- <parameters>$(find bhand_description)/config/controller_manager.yaml</parameters>
- </plugin>
- </gazebo>
+ <xacro:include filename="gazebo.urdf.xacro"/>
</xacro:if>
</robot>
--- /dev/null
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:arg name="ignition" default="false"/>
+
+ <!-- Gazebo Classic -->
+ <xacro:unless value="$(arg ignition)">
+ <gazebo>
+ <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+ <robot_param>robot_description</robot_param>
+ <robot_param_node>robot_state_publisher</robot_param_node>
+ <parameters>$(find bhand_description)/config/controller_manager.yaml</parameters>
+ </plugin>
+ </gazebo>
+ </xacro:unless>
+
+ <!-- Gazebo Ignition -->
+ <xacro:if value="$(arg ignition)">
+ <gazebo>
+ <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+ <robot_param>robot_description</robot_param>
+ <robot_param_node>robot_state_publisher</robot_param_node>
+ <parameters>$(find bhand_description)/config/controller_manager.yaml</parameters>
+ </plugin>
+ </gazebo>
+ </xacro:if>
+</robot>
--- /dev/null
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <ros2_control name="BhandSystem" type="system">
+ <!-- Gazebo Classic -->
+ <xacro:unless value="$(arg ignition)">
+ <hardware>
+ <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+ </hardware>
+ </xacro:unless>
+ <xacro:if value="$(arg ignition)">
+ <!-- Gazebo Ignition -->
+ <hardware>
+ <plugin>ign_ros2_control/IgnitionSystem</plugin>
+ </hardware>
+ </xacro:if>
+
+ <joint name="bhand_finger1_joint_2">
+ <command_interface name="position">
+ <param name="min">0.0</param>
+ <param name="max">${140 * M_PI /180}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ </joint>
+
+ <joint name="bhand_finger1_joint_3">
+ <param name="mimic">bhand_finger1_joint_2</param>
+ <param name="multiplier">${48 / 140}</param>
+ <command_interface name="position"/>
+ <state_interface name="position"/>
+ </joint>
+
+ <joint name="bhand_finger2_joint_2">
+ <command_interface name="position">
+ <param name="min">0,9</param>
+ <param name="max">${140 * M_PI /180}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ </joint>
+
+ <joint name="bhand_finger2_joint_3">
+ <param name="mimic">bhand_finger2_joint_2</param>
+ <param name="multiplier">${48 / 140}</param>
+ <command_interface name="position"/>
+ <state_interface name="position"/>
+ </joint>
+
+ <joint name="bhand_finger3_joint_2">
+ <command_interface name="position">
+ <param name="min">0.0</param>
+ <param name="max">${140 * M_PI /180}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ </joint>
+
+ <joint name="bhand_finger3_joint_3">
+ <param name="mimic">bhand_finger3_joint_2</param>
+ <param name="multiplier">${48 / 140}</param>
+ <command_interface name="position"/>
+ <state_interface name="position"/>
+ </joint>
+
+ <joint name="bhand_spread">
+ <command_interface name="position">
+ <param name="min">0.0</param>
+ <param name="max">${M_PI}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ </joint>
+
+ <joint name="bhand_spread_mimic">
+ <param name="mimic">bhand_spread</param>
+ <param name="multiplier">-1.0</param>
+ <command_interface name="position"/>
+ <state_interface name="position"/>
+ </joint>
+ </ros2_control>
+</robot>
-/computed_torque_controller:
+computed_torque_controller:
ros__parameters:
joints:
- wam_joint_1
-/pid_independent_joint_controller:
+pid_independent_joint_controller:
ros__parameters:
joints:
- wam_joint_1
-/pid_plus_gravity_controller:
+pid_plus_gravity_controller:
ros__parameters:
joints:
- wam_joint_1
--- /dev/null
+<!--******************************************************************************
+ Barrett WAM Bringup
+ Ignition Launch File
+ Copyright (C) 2023 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Geneal Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************-->
+
+<launch>
+ <arg name="pause" default="true"/>
+ <arg name="gui" default="true"/>
+ <arg name="use_sim_time" default="true"/>
+ <arg name="table" default="true"/>
+ <arg name="bhand" default="true"/>
+
+ <!-- This is the default Barrett WAM Controller, used by libbarrett -->
+ <arg name="controller" default="pid_plus_gravity_controller"/>
+ <arg name="config" default="$(find-pkg-share wam_bringup)/config/$(var controller).yaml"/>
+
+ <include file="$(find-pkg-share wam_description)/launch/ignition.launch.xml" >
+ <arg name="pause" value="$(var pause)"/>
+ <arg name="gui" value="$(var gui)"/>
+ <arg name="use_sim_time" value="$(var use_sim_time)"/>
+ <arg name="table" value="$(var table)"/>
+ <arg name="bhand" value="$(var bhand)"/>
+ </include>
+
+ <include file="$(find-pkg-share wam_bringup)/launch/$(var controller).launch.xml" >
+ <arg name="config" value="$(var config)"/>
+ <arg name="use_sim_time" value="$(var use_sim_time)"/>
+ </include>
+
+ <group if="$(var bhand)">
+ <arg name="bhand_config" default="$(find-pkg-share bhand_bringup)/config/bhand_controller.yaml"/>
+
+ <node name="bhand_controller_spawner" pkg="controller_manager" exec="spawner"
+ args="-t position_controllers/JointGroupPositionController -p $(var bhand_config) bhand_controller"/>
+ </group>
+</launch>
<!--******************************************************************************
Barrett WAM Description
Gazebo Ignitio Launch File
- Copyright (C) 2022 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2022, 2023 Walter Fetter Lages <w.fetter@ieee.org>
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
<arg name="table" default="true"/>
<arg name="bhand" default="true"/>
- <arg unless="$(var pause)" name="ign_pause" default="-r"/>
- <arg if="$(var pause)" name="ign_pause" default=""/>
+ <arg unless="$(var pause)" name="gz_pause" default="-r"/>
+ <arg if="$(var pause)" name="gz_pause" default=""/>
- <arg unless="$(var gui)" name="ign_gui" default="-s"/>
- <arg if="$(var gui)" name="ign_gui" default=""/>
+ <arg unless="$(var gui)" name="gz_gui" default="-s"/>
+ <arg if="$(var gui)" name="gz_gui" default=""/>
- <include file="$(find-pkg-share ros_ign_gazebo)/launch/ign_gazebo.launch.py">
- <arg name="ign_args" value="$(var ign_pause) $(var ign_gui) empty.sdf"/>
+ <include file="$(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py">
+ <arg name="gz_args" value="$(var gz_pause) $(var gz_gui) empty.sdf"/>
</include>
<include file="$(find-pkg-share wam_description)/launch/wam.launch.xml">
<arg name="table" value="$(var table)"/>
<arg name="bhand" value="$(var bhand)"/>
<arg name="use_sim_time" value="$(var use_sim_time)"/>
+ <arg name="ignition" value="true"/>
</include>
<node name="wam_spawner" pkg="ros_ign_gazebo" exec="create" args="-topic robot_description -name wam"/>
- <node name="clock_bridge" pkg="ros_ign_bridge" exec="parameter_bridge" args="/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock">
+ <node name="clock_bridge" pkg="ros_gz_bridge" exec="parameter_bridge" args="/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock">
<param name="use_sim_time" value="$(var use_sim_time)"/>
</node>
</launch>
<arg name="bhand" default="true"/>
<arg name="world" default="false"/>
<arg name="use_sim_time" default="false"/>
+ <arg name="ignition" default="false"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
- <param name="robot_description" value="$(command 'xacro $(find-pkg-share wam_description)/xacro/wam.urdf.xacro world:=$(var world) table:=$(var table) bhand:=$(var bhand)')" type="str"/>
+ <param name="robot_description" value="$(command 'xacro $(find-pkg-share wam_description)/xacro/wam.urdf.xacro world:=$(var world) table:=$(var table) bhand:=$(var bhand) ignition:=$(var ignition)')" type="str"/>
<param name="use_sim_time" value="$(var use_sim_time)"/>
</node>
</launch>
--- /dev/null
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:arg name="ignition" default="false"/>
+
+ <!-- Gazebo Classic -->
+ <xacro:unless value="$(arg ignition)">
+ <gazebo>
+ <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+ <robot_param>robot_description</robot_param>
+ <robot_param_node>robot_state_publisher</robot_param_node>
+ <parameters>$(find wam_description)/config/controller_manager.yaml</parameters>
+ </plugin>
+ </gazebo>
+ </xacro:unless>
+
+ <!-- Gazebo Ignition -->
+ <xacro:if value="$(arg ignition)">
+ <gazebo>
+ <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+ <robot_param>robot_description</robot_param>
+ <robot_param_node>robot_state_publisher</robot_param_node>
+ <parameters>$(find wam_description)/config/controller_manager.yaml</parameters>
+ </plugin>
+ </gazebo>
+ </xacro:if>
+</robot>
--- /dev/null
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <ros2_control name="GazeboSystem" type="system">
+ <!-- Gazebo Classic -->
+ <xacro:unless value="$(arg ignition)">
+ <hardware>
+ <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+ </hardware>
+ </xacro:unless>
+ <xacro:if value="$(arg ignition)">
+ <!-- Gazebo Ignition -->
+ <hardware>
+ <plugin>ign_ros2_control/IgnitionSystem</plugin>
+ </hardware>
+ </xacro:if>
+
+ <joint name="wam_joint_1">
+ <command_interface name="effort">
+ <param name="min">${-1.8*42.00}</param>
+ <param name="max">${1.8*42.00}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ <state_interface name="velocity"/>
+ <state_interface name="effort"/>
+ </joint>
+
+ <joint name="wam_joint_2">
+ <command_interface name="effort">
+ <param name="min">${-1.8*28.25}</param>
+ <param name="max">${1.8*28.25}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ <state_interface name="velocity"/>
+ <state_interface name="effort"/>
+ </joint>
+
+ <joint name="wam_joint_3">
+ <command_interface name="effort">
+ <param name="min">${-1.8*28.25}</param>
+ <param name="max">${1.8*28.25}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ <state_interface name="velocity"/>
+ <state_interface name="effort"/>
+ </joint>
+
+ <joint name="wam_joint_4">
+ <command_interface name="effort">
+ <param name="min">${-1.6*18.00}</param>
+ <param name="max">${1.6*18.00}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ <state_interface name="velocity"/>
+ <state_interface name="effort"/>
+ </joint>
+
+ <joint name="wam_joint_5">
+ <command_interface name="effort">
+ <param name="min">${-0.6*9.48}</param>
+ <param name="max">${0.6*9.48}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ <state_interface name="velocity"/>
+ <state_interface name="effort"/>
+ </joint>
+
+ <joint name="wam_joint_6">
+ <command_interface name="effort">
+ <param name="min">${-0.6*9.48}</param>
+ <param name="max">${0.6*9.48}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ <state_interface name="velocity"/>
+ <state_interface name="effort"/>
+ </joint>
+
+ <joint name="wam_joint_7">
+ <command_interface name="effort">
+ <param name="min">${-0.613*14.93}</param>
+ <param name="max">${0.613*14.93}</param>
+ </command_interface>
+ <state_interface name="position"/>
+ <state_interface name="velocity"/>
+ <state_interface name="effort"/>
+ </joint>
+ </ros2_control>
+</robot>
<xacro:wam_j7 parent="wam_link_6"/>
<xacro:wam_tool_plate parent="wam_link_7"/>
- <ros2_control name="GazeboSystem" type="system">
- <hardware>
- <plugin>gazebo_ros2_control/GazeboSystem</plugin>
- </hardware>
-
- <joint name="wam_joint_1">
- <command_interface name="effort">
- <param name="min">${-1.8*42.00}</param>
- <param name="max">${1.8*42.00}</param>
- </command_interface>
- <state_interface name="position"/>
- <state_interface name="velocity"/>
- <state_interface name="effort"/>
- </joint>
-
- <joint name="wam_joint_2">
- <command_interface name="effort">
- <param name="min">${-1.8*28.25}</param>
- <param name="max">${1.8*28.25}</param>
- </command_interface>
- <state_interface name="position"/>
- <state_interface name="velocity"/>
- <state_interface name="effort"/>
- </joint>
-
- <joint name="wam_joint_3">
- <command_interface name="effort">
- <param name="min">${-1.8*28.25}</param>
- <param name="max">${1.8*28.25}</param>
- </command_interface>
- <state_interface name="position"/>
- <state_interface name="velocity"/>
- <state_interface name="effort"/>
- </joint>
-
- <joint name="wam_joint_4">
- <command_interface name="effort">
- <param name="min">${-1.6*18.00}</param>
- <param name="max">${1.6*18.00}</param>
- </command_interface>
- <state_interface name="position"/>
- <state_interface name="velocity"/>
- <state_interface name="effort"/>
- </joint>
-
- <joint name="wam_joint_5">
- <command_interface name="effort">
- <param name="min">${-0.6*9.48}</param>
- <param name="max">${0.6*9.48}</param>
- </command_interface>
- <state_interface name="position"/>
- <state_interface name="velocity"/>
- <state_interface name="effort"/>
- </joint>
-
- <joint name="wam_joint_6">
- <command_interface name="effort">
- <param name="min">${-0.6*9.48}</param>
- <param name="max">${0.6*9.48}</param>
- </command_interface>
- <state_interface name="position"/>
- <state_interface name="velocity"/>
- <state_interface name="effort"/>
- </joint>
-
- <joint name="wam_joint_7">
- <command_interface name="effort">
- <param name="min">${-0.613*14.93}</param>
- <param name="max">${0.613*14.93}</param>
- </command_interface>
- <state_interface name="position"/>
- <state_interface name="velocity"/>
- <state_interface name="effort"/>
- </joint>
- </ros2_control>
+ <xacro:include filename="ros2_control.urdf.xacro"/>
+ <xacro:include filename="gazebo.urdf.xacro"/>
- <gazebo>
- <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
- <robot_param>robot_description</robot_param>
- <robot_param_node>robot_state_publisher</robot_param_node>
- <parameters>$(find wam_description)/config/controller_manager.yaml</parameters>
- </plugin>
- </gazebo>
-
<xacro:if value="$(arg world)">
<xacro:unless value="$(arg table)">