Case study: motor coupled to a gearbox and a transmission screw with an encoder
This case study is a simple example to show how to set up the config of the EcCiA402Drive
generic plugin to take into account transmission parameters and encoder parameters.
System configuration
The system is composed of a motor coupled to a transmission line (a gearbox and a transmission screw) whose rotation is measured by an encoder.
Transmission configuration
The transmission is composed of a motor, a gearbox, and a transmission screw (lead screw with ball bearing for zero friction) with the following characteristics:
gearbox with gear ratio, reduction absolute value: 57/13 (\(r \triangleq \frac{13}{57}\))
gearbox maximum efficiency: 0.84 (\(\eta_g \triangleq 0.84\))
lead screw with lead: 1 mm (\(l \triangleq 1 mm\))
Encoder configuration
The encoder has the following characteristics:
Resolution (number of pulses/revolution): 500 CPR
Encoder type: quadrature encoder
Digital resolution (number of counts/revolution): \(\text{C} \triangleq 500\times 4 = 2000\)
Convert state values
The drive will transmit the following state values:
position
in \(\text{counts}\) or \(c\)velocity
in \(\theta\) (revolutions per minute)torque
(\(T_i\)) in \(\text{mNm}\) (torque of the motor at the input of the gearbox)
We would like to have the following state values:
position
(\(d\)) in \(\text{m}\)velocity
(\(v\)) in \(\text{m/s}\)force
(\(F_o\)) in \(\text{mN}\) (Force of the lead screw)
This is the configuration part of the tpdo of the EcCiA402Drive
plugin and corresponding to the state_interface
in ROS2 control terminology.
Linear displacement per count of the encoder
The motor has a reductor so each revolution of the motor does not correspond to a revolution of the system. It is necessary to first compute the rotation of the shaft at the output of the gearbox.
reduction absolute value is \(r \triangleq \frac{13}{57}\)
So for 57 revolutions of the motor, the shaft will only turn 13 times. So the motor has to turn approximately 4.4 times to make the system turn once.
The lead of the screw is defined as the linear distance traveled per rotation of the screw and in this case is \(1mm\).
The encoder reports \(C \triangleq 2000\) counts per revolution (CPR) so 2000 counts per revolution of the shaft, so an angular resolution of \(\frac{360}{2000} = 0.18^{\circ}\).
Let \(d\) be the linear displacement in meters, \(r\) the reduction absolute value, \(l\) the lead of the screw in meters and \(C\) the counts per revolution of the encoder and \(c\) the value of the encoder. Then, a count \(c\) of the encoder corresponds to a linear displacement \(d\) of
The conversion factor is more precisely \(0.11403508771929824 \times 10^{-6}\). So the displacement resolution, the displacement corresponding to one count of the encoder, is approximately \(0.114 µm\).
The tpdo part of the configuration of the EcCiA402Drive
plugin will then be set up with the following values:
tpdo: # TxPDO = transmit PDO Mapping, slave (out) to master (in) (MISO), state values transmitted by the drive to ROS2 control
- index: 0x1a03
channels:
- {
index: 0x6064,
sub_index: 0,
type: int32,
state_interface: position,
factor: 0.11403508771929824e-6,
offset: 0
} # Position actual value
Convert velocity in \(\text{m/s}\) from \(\text{rpm}\)
If the motor moves at a velocity of \(\theta\) RPM, then the linear velocity \(v\) in \(\text{m/s}\) of the system is given by:
The tpdo part of the configuration of the EcCiA402Drive
plugin will then be set up with the following values:
tpdo: # TxPDO = transmit PDO Mapping, slave (out) to master (in) (MISO), state values transmitted by the drive to ROS2 control
- index: 0x1a03
channels:
- {
index: 0x606c,
sub_index: 0,
type: int32,
state_interface: velocity,
factor: 3.8011695906432747e-6,
offset: 0
} # Velocity actual value
Transform torque
The output torque (\(T_o\)) of the gearbox given the input torque of the motor (\(T_i\)) is given by the formula:
Torque to Force conversion
The force \(F_o\) of the lead screw is given by the formula:
Thus the force \(F_o\) in \(\text{mN}\) given the input torque of the motor (\(T_i\) in \(\text{mNm}\)) is provided by the formula:
The tpdo part of the configuration of the EcCiA402Drive
plugin will then be set up with the following values:
tpdo: # TxPDO = transmit PDO Mapping, slave (out) to master (in) (MISO), state values transmitted by the drive to ROS2 control
- index: 0x1a03
channels:
- {
index: 0x6077,
sub_index: 0,
type: int32,
state_interface: effort,
factor: 23141.45480828912,
offset: 0
} # Force actual value in mN
Convert command values
This is the configuration part of the rpdo of the EcCiA402Drive
plugin and corresponding to the command_interface
in ROS2 control terminology.
The drive will take the following command values:
position
in \(\text{counts}\) or \(c\)velocity
in \(\theta\) (revolutions per minute)torque
(\(T_i\)) in \(\text{mNm}\) (torque of the motor at the input of the gearbox)
We would like to send the following command values:
position
(\(d\)) in \(\text{m}\)velocity
(\(v\)) in \(\text{m/s}\)force
(\(F_o\)) in \(\text{mN}\) (Force of the lead screw)
Command in counts to move by a certain distance in m
To command the motor to move by a certain distance \(d\) in meters, the number of counts \(c\) to send to the motor is given by the formula:
The ‘rpdo’ part of the configuration of the EcCiA402Drive
plugin will then be set up with the following values:
rpdo: # RxPDO = receive PDO Mapping, master (out) to slave (in) (MOSI), command values received by the drive from ROS2 control
- index: 0x1603
channels:
- {
index: 0x607a,
sub_index: 0,
type: int32,
command_interface: position,
factor: 8769.23076923077,
offset: 0
} # Target position
Command in rpm to move at a certain velocity given in m/s
To command the motor to move at a certain velocity in m/s, the number of rpm to send to the motor is given by the formula:
The ‘rpdo’ part of the configuration of the EcCiA402Drive
plugin will then be set up with the following values:
rpdo: # RxPDO = receive PDO Mapping, master (out) to slave (in) (MOSI), command values received by the drive from ROS2 control
- index: 0x1603
channels:
- {
index: 0x60ff,
sub_index: 0,
type: int32,
command_interface: velocity,
factor: 263076.92307692306,
offset: 0
} # Target velocity
Force to torque conversion
The input torque of the motor (\(T_i\) in \(\text{mNm}\)) given the target command force \(F_o\) in \(\text{mN}\) is provided by the formula:
The tpdo part of the configuration of the EcCiA402Drive
plugin will then be set up with the following values:
rpdo: # RxPDO = receive PDO Mapping, master (out) to slave (in) (MOSI), command values received by the drive from ROS2 control
- index: 0x1603
channels:
- {
index: 0x6071,
sub_index: 0,
type: int32,
state_interface: effort,
factor: 4.321249499153383e-05,
offset: 0
} # Target force
Complete configuration example for an EPOS4 drive
Note that we omit the offset
parameter since their default value is equal to zero.
1# Configuration file for Maxon EPOS4 50/5 drive
2vendor_id: 0x000000fb
3product_id: 0x63500000 # Product code
4assign_activate: 0x0300 # DC Synch register
5auto_fault_reset: true # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
6sdo: # 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
9rpdo: # RxPDO = receive PDO 4 Mapping, master (out) to slave (in) (MOSI), command values received by the drive from ROS2 control
10 - index: 0x1603
11 channels:
12 - {
13 index: 0x6040,
14 sub_index: 0,
15 type: uint16,
16 command_interface: control_word,
17 default: 0,
18 } # Control word
19 - {
20 index: 0x607a,
21 sub_index: 0,
22 type: int32,
23 command_interface: position,
24 factor: 8769.23076923077,
25 default: .nan,
26 } # Target position
27 - {
28 index: 0x60ff,
29 sub_index: 0,
30 type: int32,
31 command_interface: velocity,
32 factor: 263076.92307692306,
33 default: 0,
34 } # Target velocity
35 - {
36 index: 0x6071,
37 sub_index: 0,
38 type: int16,
39 command_interface: effort,
40 factor: 4.321249499153383e-05,
41 default: 0,
42 } # Target torque
43 - { index: 0x60b0, sub_index: 0, type: int32, default: 0 } # Offset position
44 - { index: 0x60b1, sub_index: 0, type: int32, default: 0 } # Offset velocity
45 - { index: 0x60b2, sub_index: 0, type: int16, default: 0 } # Offset torque
46 - {
47 index: 0x6060,
48 sub_index: 0,
49 type: int8,
50 command_interface: mode_of_operation,
51 default: 9,
52 } # Mode of operation. Available modes are: position (8), velocity (9), effort (10) and homing (6)
53tpdo: # TxPDO = transmit PDO 4 Mapping, slave (out) to master (in) (MISO), state values transmitted by the drive to ROS2 control
54 - index: 0x1a03
55 channels:
56 - {
57 index: 0x6041,
58 sub_index: 0,
59 type: uint16,
60 state_interface: status_word,
61 } # Status word
62 - {
63 index: 0x6064,
64 sub_index: 0,
65 type: int32,
66 state_interface: position,
67 factor: 0.11403508771929824e-6,
68 } # Position actual value
69 - {
70 index: 0x606c,
71 sub_index: 0,
72 type: int32,
73 state_interface: velocity,
74 factor: 3.8011695906432747e-6,
75 } # Velocity actual value
76 - {
77 index: 0x6077,
78 sub_index: 0,
79 type: int16,
80 state_interface: effort,
81 factor: 23141.45480828912,
82 } # Force actual value
83 - {
84 index: 0x6061,
85 sub_index: 0,
86 type: int8,
87 state_interface: mode_of_operation,
88 } # Mode of operation display
89 - { index: 0x3141, sub_index: 1, type: int16 } # Digital Input Properties
90 - { index: 0x60b9, sub_index: 0, type: int16 } # Touch Probe Status
91 - { index: 0x60ba, sub_index: 0, type: int32 } # Touch Probe Position 1 Positive edge
92 - { index: 0x60bb, sub_index: 0, type: int32 } # Touch Probe Position 1 Negative edge