Skip to content

Commit

Permalink
Move r6bot into description package
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich committed Aug 13, 2023
1 parent 57a561e commit 77256b5
Show file tree
Hide file tree
Showing 25 changed files with 179 additions and 133 deletions.
2 changes: 1 addition & 1 deletion example_7/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ install(
DESTINATION include/ros2_control_demo_example_7
)
install(
DIRECTORY description/launch description/ros2_control description/urdf description/rviz description/srdf description/meshes
DIRECTORY description/launch description/ros2_control description/urdf
DESTINATION share/ros2_control_demo_example_7
)
install(
Expand Down
2 changes: 1 addition & 1 deletion example_7/bringup/launch/r6bot_controller.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ def generate_launch_description():
]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("ros2_control_demo_example_7"), "rviz", "view_robot.rviz"]
[FindPackageShare("ros2_control_demo_description"), "r6bot/rviz", "view_robot.rviz"]
)

control_node = Node(
Expand Down
2 changes: 1 addition & 1 deletion example_7/description/launch/view_r6bot.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ def generate_launch_description():
robot_description = {"robot_description": robot_description_content}

rviz_config_file = PathJoinSubstitution(
[FindPackageShare("ros2_control_demo_example_7"), "rviz", "view_robot.rviz"]
[FindPackageShare("ros2_control_demo_description"), "r6bot/rviz", "view_robot.rviz"]
)

joint_state_publisher_node = Node(
Expand Down
134 changes: 4 additions & 130 deletions example_7/description/urdf/r6bot.urdf.xacro
Original file line number Diff line number Diff line change
@@ -1,140 +1,14 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="r6bot">
<!-- helper macro -->
<xacro:include filename="$(find ros2_control_demo_example_7)/urdf/inc/create_link.xacro"/>

<!-- Import r6bot macro -->
<xacro:include filename="$(find ros2_control_demo_description)/r6bot/urdf/r6bot_description.urdf.xacro"/>

<!-- create link fixed to the "world" -->
<link name="world"/>

<xacro:create_link
link_name="base_link"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_example_7"
mesh_name="link_0"
/>

<xacro:create_link
link_name="link_1"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_example_7"
mesh_name="link_1"
/>

<xacro:create_link
link_name="link_2"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_example_7"
mesh_name="link_2"
/>

<xacro:create_link
link_name="link_3"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_example_7"
mesh_name="link_3"
/>

<xacro:create_link
link_name="link_4"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_example_7"
mesh_name="link_4"
/>

<xacro:create_link
link_name="link_5"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_example_7"
mesh_name="link_5"
/>

<xacro:create_link
link_name="link_6"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_example_7"
mesh_name="link_6"
/>

<link name="ft_frame"/>
<link name="tool0"/>

<!-- base_joint fixes base_link to the environment -->
<joint name="base_joint" type="fixed">
<parent link="world"/>
<child link="base_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>

<!-- joints - main serial chain -->
<joint name="joint_1" type="revolute">
<parent link="base_link"/>
<child link="link_1"/>
<origin xyz="0 0 0.061584" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint_2" type="revolute">
<parent link="link_1"/>
<child link="link_2"/>
<origin xyz="-0.101717 0 0.182284" rpy="${-pi/2} -${pi/3} ${pi/2}"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint_3" type="revolute">
<parent link="link_2"/>
<child link="link_3"/>
<origin xyz="0.685682 0 0.041861" rpy="0 ${pi} ${pi+pi/2}"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint_4" type="revolute">
<parent link="link_3"/>
<child link="link_4"/>
<origin xyz="0.518777 0 0.067458" rpy="0 ${pi} ${pi+pi/6}"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint_5" type="revolute">
<parent link="link_4"/>
<child link="link_5"/>
<origin xyz="0.112654 0 0.110903" rpy="${pi/2} ${pi} ${pi/2}"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint_6" type="revolute">
<parent link="link_5"/>
<child link="link_6"/>
<origin xyz="-0.085976 0 0.133436" rpy="0 ${-pi/2} 0"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint-ft_frame" type="fixed">
<parent link="link_6"/>
<child link="ft_frame"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<joint name="joint-tool" type="fixed">
<parent link="ft_frame"/>
<child link="tool0"/>
<origin xyz="0 0 .185" rpy="0 0 0"/>
</joint>
<xacro:r6bot_description/>

<!-- ros2 control tag -->
<!-- Import r6bot ros2_control description -->
<xacro:include filename="$(find ros2_control_demo_example_7)/ros2_control/r6bot.ros2_control.xacro" />
<xacro:r6bot_ros2_control name="r6bot"/>
Expand Down
1 change: 1 addition & 0 deletions example_7/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>ros2controlcli</exec_depend>
<exec_depend>ros2launch</exec_depend>
<exec_depend>ros2_control_demo_description</exec_depend>
<exec_depend>rviz2</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
Expand Down
11 changes: 11 additions & 0 deletions ros2_control_demo_description/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
cmake_minimum_required(VERSION 3.5)
project(ros2_control_demo_description)

find_package(ament_cmake REQUIRED)

install(
DIRECTORY r6bot/launch r6bot/meshes r6bot/srdf r6bot/urdf r6bot/rviz
DESTINATION share/${PROJECT_NAME}/r6bot
)

ament_package()
21 changes: 21 additions & 0 deletions ros2_control_demo_description/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ros2_control_demo_description</name>
<version>0.0.0</version>
<description>Package with URDF and description files of test robots.</description>

<maintainer email="denis@stogl.de">Denis Štogl</maintainer>

<license>Apache-2.0</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<exec_depend>joint_state_publisher_gui</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>rviz2</exec_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
File renamed without changes.
File renamed without changes.
139 changes: 139 additions & 0 deletions ros2_control_demo_description/r6bot/urdf/r6bot_description.urdf.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,139 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

<xacro:macro name="r6bot_description">

<!-- helper macro -->
<xacro:include filename="$(find ros2_control_demo_description)/r6bot/urdf/inc/create_link.xacro"/>

<xacro:create_link
link_name="base_link"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_description/r6bot"
mesh_name="link_0"
/>

<xacro:create_link
link_name="link_1"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_description/r6bot"
mesh_name="link_1"
/>

<xacro:create_link
link_name="link_2"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_description/r6bot"
mesh_name="link_2"
/>

<xacro:create_link
link_name="link_3"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_description/r6bot"
mesh_name="link_3"
/>

<xacro:create_link
link_name="link_4"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_description/r6bot"
mesh_name="link_4"
/>

<xacro:create_link
link_name="link_5"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_description/r6bot"
mesh_name="link_5"
/>

<xacro:create_link
link_name="link_6"
xyz_offset="0 0 0"
rpy_offset="0 0 0"
mesh_package="ros2_control_demo_description/r6bot"
mesh_name="link_6"
/>

<link name="ft_frame"/>
<link name="tool0"/>

<!-- base_joint fixes base_link to the environment -->
<joint name="base_joint" type="fixed">
<parent link="world"/>
<child link="base_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>

<!-- joints - main serial chain -->
<joint name="joint_1" type="revolute">
<parent link="base_link"/>
<child link="link_1"/>
<origin xyz="0 0 0.061584" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint_2" type="revolute">
<parent link="link_1"/>
<child link="link_2"/>
<origin xyz="-0.101717 0 0.182284" rpy="${-pi/2} -${pi/3} ${pi/2}"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint_3" type="revolute">
<parent link="link_2"/>
<child link="link_3"/>
<origin xyz="0.685682 0 0.041861" rpy="0 ${pi} ${pi+pi/2}"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint_4" type="revolute">
<parent link="link_3"/>
<child link="link_4"/>
<origin xyz="0.518777 0 0.067458" rpy="0 ${pi} ${pi+pi/6}"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint_5" type="revolute">
<parent link="link_4"/>
<child link="link_5"/>
<origin xyz="0.112654 0 0.110903" rpy="${pi/2} ${pi} ${pi/2}"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint_6" type="revolute">
<parent link="link_5"/>
<child link="link_6"/>
<origin xyz="-0.085976 0 0.133436" rpy="0 ${-pi/2} 0"/>
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.5"/>
</joint>

<joint name="joint-ft_frame" type="fixed">
<parent link="link_6"/>
<child link="ft_frame"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

<joint name="joint-tool" type="fixed">
<parent link="ft_frame"/>
<child link="tool0"/>
<origin xyz="0 0 .185" rpy="0 0 0"/>
</joint>

</xacro:macro>

</robot>

0 comments on commit 77256b5

Please sign in to comment.