Skip to content

Commit ce3a0e9

Browse files
mcbedmcbed
committed
added motor drive example package
1 parent 9e081b9 commit ce3a0e9

File tree

9 files changed

+294
-0
lines changed

9 files changed

+294
-0
lines changed

‎README.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
Example repository for the [EtherCAT Driver ROS2 stack](https://github.com/ICube-Robotics/ethercat_driver_ros2).
66

77
This repository contains the following example packages :
8+
- EtherCAT compatible CIA402 motor drive (example using Maxon EPOS3)
89
- Custom Force Sensor EtherCAT slave using strain gauges and EasyCAT
910

1011
## Contacts ##

‎ethercat_motor_drive/CMakeLists.txt‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(ethercat_motor_drive)
3+
find_package(ament_cmake REQUIRED)
4+
find_package(rclcpp REQUIRED)
5+
find_package(ethercat_driver REQUIRED)
6+
find_package(ethercat_interface REQUIRED)
7+
find_package(ethercat_generic_cia402_drive REQUIRED)
8+
9+
install(
10+
DIRECTORY config description launch
11+
DESTINATION share/${PROJECT_NAME}
12+
)
13+
14+
ament_package()

‎ethercat_motor_drive/README.md‎

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
# ethercat_motor_drive
2+
3+
This repository contains the instructions for running an EtherCAT compatible motor drive and use it in ROS2 with [EtherCAT Driver ROS2 stack](https://github.com/ICube-Robotics/ethercat_driver_ros2).
4+
5+
## Required Hardware ##
6+
- Maxon EPOS3 motor drive
7+
- Compatible DC motor with encoder
8+
9+
## Usage ##
10+
***Required setup : Ubuntu 22.04 LTS***
11+
12+
1. Install `ros2` packages. The current development is based of `ros2 humble`. Installation steps are described [here](https://docs.ros.org/en/humble/Installation.html).
13+
2. Source your `ros2` environment:
14+
```shell
15+
source /opt/ros/humble/setup.bash
16+
```
17+
**NOTE**: The ros2 environment needs to be sources in every used terminal. If only one distribution of ros2 is used, it can be added to the `~/.bashrc` file.
18+
3. Install `colcon` and its extensions :
19+
```shell
20+
sudo apt install python3-colcon-common-extensions
21+
```
22+
3. Create a new ros2 workspace:
23+
```shell
24+
mkdir ~/ros2_ws/src
25+
```
26+
4. Pull relevant packages, install dependencies, compile, and source the workspace by using:
27+
```shell
28+
cd ~/ros2_ws
29+
git clone https://github.com/ICube-Robotics/ethercat_driver_ros2_exapmles.git src/ethercat_driver_ros2_examples
30+
vcs import src < ethercat_driver_ros2_examples.repos
31+
rosdep install --ignore-src --from-paths . -y -r
32+
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install --packages-select ethercat_motor_drive
33+
source install/setup.bash
34+
```
35+
5. Run the system:
36+
```shell
37+
ros2 launch ethercat_motor_drive motor_drive.launch.py
38+
```
39+
40+
## Contacts ##
41+
![icube](https://icube.unistra.fr/fileadmin/templates/DUN/icube/images/logo.png)
42+
43+
[ICube Laboratory](https://icube.unistra.fr), [University of Strasbourg](https://www.unistra.fr/), France
44+
45+
__Maciej Bednarczyk:__ [[email protected]](mailto:[email protected]), @github: [mcbed](https://github.com/mcbed)

‎ethercat_motor_drive/config/controllers.yaml‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
controller_manager:
2+
ros__parameters:
3+
update_rate: 100 # Hz
4+
5+
joint_state_broadcaster:
6+
type: joint_state_broadcaster/JointStateBroadcaster
7+
8+
trajectory_controller:
9+
type: joint_trajectory_controller/JointTrajectoryController
10+
11+
velocity_controller:
12+
type: velocity_controllers/JointGroupVelocityController
13+
14+
effort_controller:
15+
type: effort_controllers/JointGroupEffortController
16+
17+
trajectory_controller:
18+
ros__parameters:
19+
command_interfaces:
20+
- position
21+
state_interfaces:
22+
- position
23+
joints:
24+
- joint_1
25+
26+
velocity_controller:
27+
ros__parameters:
28+
joints:
29+
- joint_1
30+
31+
effort_controller:
32+
ros__parameters:
33+
joints:
34+
- joint_1

‎ethercat_motor_drive/config/maxon_epos3_config.yaml‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
# Configuration file for Maxon EPOS3 drive
2+
vendor_id: 0x000000fb
3+
product_id: 0x64400000
4+
assign_activate: 0x0300 # DC Synch register
5+
auto_fault_reset: false # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
6+
sdo: # sdo data to be transferred at drive startup
7+
- {index: 0x60C2, sub_index: 1, type: int8, value: 10} # Set interpolation time for cyclic modes to 10 ms
8+
- {index: 0x60C2, sub_index: 2, type: int8, value: -3} # Set base 10-3s
9+
rpdo: # RxPDO = receive PDO Mapping
10+
- index: 0x1603
11+
channels:
12+
- {index: 0x6040, sub_index: 0, type: uint16, default: 0} # Control word
13+
- {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan} # Target position
14+
- {index: 0x60ff, sub_index: 0, type: int32, default: 0} # Target velocity
15+
- {index: 0x6071, sub_index: 0, type: int16, default: 0} # Target torque
16+
- {index: 0x60b0, sub_index: 0, type: int32, default: 0} # Offset position
17+
- {index: 0x60b1, sub_index: 0, type: int32, default: 0} # Offset velocity
18+
- {index: 0x60b2, sub_index: 0, type: int16, default: 0} # Offset torque
19+
- {index: 0x6060, sub_index: 0, type: int8, default: 8} # Mode of operation
20+
- {index: 0x2078, sub_index: 1, type: uint16, default: 0} # Digital Output Functionalities
21+
- {index: 0x60b8, sub_index: 0, type: uint16, default: 0} # Touch Probe Function
22+
tpdo: # TxPDO = transmit PDO Mapping
23+
- index: 0x1a03
24+
channels:
25+
- {index: 0x6041, sub_index: 0, type: uint16} # Status word
26+
- {index: 0x6064, sub_index: 0, type: int32, state_interface: position} # Position actual value
27+
- {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity} # Velocity actual value
28+
- {index: 0x6077, sub_index: 0, type: int16, state_interface: effort} # Torque actual value
29+
- {index: 0x6061, sub_index: 0, type: int8} # Mode of operation display
30+
- {index: 0x2071, sub_index: 1, type: int16} # Digital Input Functionalities State
31+
- {index: 0x60b9, sub_index: 0, type: int16} # Touch Probe Status
32+
- {index: 0x60ba, sub_index: 0, type: int32} # Touch Probe Position 1 Positive Value
33+
- {index: 0x60bb, sub_index: 0, type: int32} # Touch Probe Position 1 Negative Value

‎ethercat_motor_drive/description/config/motor_drive.config.xacro‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<?xml version="1.0"?>
2+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="motor_drive">
3+
4+
<link name="world"/>
5+
6+
<xacro:include filename="$(find ethercat_motor_drive)/description/ros2_control/motor_drive.ros2_control.xacro" />
7+
8+
<xacro:motor_drive/>
9+
</robot>

‎ethercat_motor_drive/description/ros2_control/motor_drive.ros2_control.xacro‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
<?xml version="1.0"?>
2+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3+
4+
<xacro:macro name="motor_drive">
5+
6+
<ros2_control name="motor_drive" type="system">
7+
<hardware>
8+
<plugin>ethercat_driver/EthercatDriver</plugin>
9+
<param name="master_id">0</param>
10+
<param name="control_frequency">100</param>
11+
</hardware>
12+
13+
<joint name="joint_1">
14+
<state_interface name="position"/>
15+
<state_interface name="velocity"/>
16+
<state_interface name="effort"/>
17+
<command_interface name="position"/>
18+
<command_interface name="reset_fault"/>
19+
<ec_module name="Maxon">
20+
<plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
21+
<param name="alias">0</param>
22+
<param name="position">0</param>
23+
<param name="mode_of_operation">8</param>
24+
<param name="slave_config">$(find ethercat_motor_drive)/config/maxon_epos3_config.yaml</param>
25+
</ec_module>
26+
</joint>
27+
</ros2_control>
28+
29+
</xacro:macro>
30+
31+
</robot>

‎ethercat_motor_drive/launch/motor_drive.launch.py‎

Lines changed: 109 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,109 @@
1+
# Copyright 2023 ICube Laboratory, University of Strasbourg
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
from launch import LaunchDescription
16+
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
17+
from launch.actions import DeclareLaunchArgument
18+
19+
from launch_ros.actions import Node
20+
from launch_ros.substitutions import FindPackageShare
21+
22+
23+
def generate_launch_description():
24+
25+
# Declare arguments
26+
declared_arguments = []
27+
declared_arguments.append(
28+
DeclareLaunchArgument(
29+
'description_file',
30+
default_value='motor_drive.config.xacro',
31+
description='URDF/XACRO description file with the axis.',
32+
)
33+
)
34+
35+
description_file = LaunchConfiguration('description_file')
36+
37+
# Get URDF via xacro
38+
robot_description_content = Command(
39+
[
40+
PathJoinSubstitution([FindExecutable(name="xacro")]),
41+
" ",
42+
PathJoinSubstitution(
43+
[
44+
FindPackageShare("ethercat_motor_drive"),
45+
"description/config",
46+
description_file,
47+
]
48+
),
49+
]
50+
)
51+
robot_description = {"robot_description": robot_description_content}
52+
53+
robot_controllers = PathJoinSubstitution(
54+
[
55+
FindPackageShare("ethercat_motor_drive"),
56+
"config",
57+
"controllers.yaml",
58+
]
59+
)
60+
61+
control_node = Node(
62+
package="controller_manager",
63+
executable="ros2_control_node",
64+
parameters=[robot_description, robot_controllers],
65+
output="both",
66+
)
67+
robot_state_pub_node = Node(
68+
package="robot_state_publisher",
69+
executable="robot_state_publisher",
70+
output="both",
71+
parameters=[robot_description],
72+
)
73+
74+
joint_state_broadcaster_spawner = Node(
75+
package="controller_manager",
76+
executable="spawner",
77+
arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
78+
)
79+
80+
trajectory_controller_spawner = Node(
81+
package="controller_manager",
82+
executable="spawner",
83+
arguments=["trajectory_controller", "-c", "/controller_manager"],
84+
)
85+
86+
velocity_controller_spawner = Node(
87+
package="controller_manager",
88+
executable="spawner",
89+
arguments=["velocity_controller", "-c", "/controller_manager"],
90+
)
91+
92+
effort_controller_spawner = Node(
93+
package="controller_manager",
94+
executable="spawner",
95+
arguments=["effort_controller", "-c", "/controller_manager"],
96+
)
97+
98+
nodes = [
99+
control_node,
100+
robot_state_pub_node,
101+
joint_state_broadcaster_spawner,
102+
trajectory_controller_spawner,
103+
# velocity_controller_spawner,
104+
# effort_controller_spawner,
105+
]
106+
107+
return LaunchDescription(
108+
declared_arguments +
109+
nodes)

‎ethercat_motor_drive/package.xml‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>ethercat_motor_drive</name>
5+
<version>0.0.1</version>
6+
<description> Package with instructions for running an EtherCAT compatible motor drive and use it in ROS2</description>
7+
<maintainer email="[email protected]">Maciej Bednarczyk</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
<exec_depend>ethercat_driver</exec_depend>
12+
<exec_depend>ethercat_interface</exec_depend>
13+
<exec_depend>ethercat_generic_cia402_drive</exec_depend>
14+
15+
<export>
16+
<build_type>ament_cmake</build_type>
17+
</export>
18+
</package>

0 commit comments

Comments
 (0)