Skip to content

Commit 821cd6a

Browse files
jordan-palaciosjordan-palacios
committed
Add torque and absolute position to differential transmission
1 parent 10c86da commit 821cd6a

2 files changed

Lines changed: 171 additions & 8 deletions

File tree

‎transmission_interface/include/transmission_interface/differential_transmission.hpp‎

Lines changed: 67 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -108,6 +108,9 @@ namespace transmission_interface
108108
*
109109
* \ingroup transmission_types
110110
*/
111+
112+
constexpr auto HW_IF_ABSOLUTE_POSITION = "absolute_position";
113+
111114
class DifferentialTransmission : public Transmission
112115
{
113116
public:
@@ -164,10 +167,14 @@ class DifferentialTransmission : public Transmission
164167
std::vector<JointHandle> joint_position_;
165168
std::vector<JointHandle> joint_velocity_;
166169
std::vector<JointHandle> joint_effort_;
170+
std::vector<JointHandle> joint_torque_;
171+
std::vector<JointHandle> joint_absolute_position_;
167172

168173
std::vector<ActuatorHandle> actuator_position_;
169174
std::vector<ActuatorHandle> actuator_velocity_;
170175
std::vector<ActuatorHandle> actuator_effort_;
176+
std::vector<ActuatorHandle> actuator_torque_;
177+
std::vector<ActuatorHandle> actuator_absolute_position_;
171178
};
172179

173180
inline DifferentialTransmission::DifferentialTransmission(
@@ -228,8 +235,13 @@ void DifferentialTransmission::configure(
228235
joint_velocity_ =
229236
get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_VELOCITY);
230237
joint_effort_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_EFFORT);
238+
joint_torque_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_TORQUE);
239+
joint_absolute_position_ =
240+
get_ordered_handles(joint_handles, joint_names, HW_IF_ABSOLUTE_POSITION);
231241

232-
if (joint_position_.size() != 2 && joint_velocity_.size() != 2 && joint_effort_.size() != 2)
242+
if (
243+
joint_position_.size() != 2 && joint_velocity_.size() != 2 && joint_effort_.size() != 2 &&
244+
joint_torque_.size() != 2 && joint_absolute_position_.size() != 2)
233245
{
234246
throw Exception("Not enough valid or required joint handles were presented.");
235247
}
@@ -240,10 +252,15 @@ void DifferentialTransmission::configure(
240252
get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_VELOCITY);
241253
actuator_effort_ =
242254
get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_EFFORT);
255+
actuator_torque_ =
256+
get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_TORQUE);
257+
actuator_absolute_position_ =
258+
get_ordered_handles(actuator_handles, actuator_names, HW_IF_ABSOLUTE_POSITION);
243259

244260
if (
245261
actuator_position_.size() != 2 && actuator_velocity_.size() != 2 &&
246-
actuator_effort_.size() != 2)
262+
actuator_effort_.size() != 2 && actuator_torque_.size() != 2 &&
263+
actuator_absolute_position_.size() != 2)
247264
{
248265
throw Exception(
249266
fmt::format(
@@ -254,7 +271,9 @@ void DifferentialTransmission::configure(
254271
if (
255272
joint_position_.size() != actuator_position_.size() &&
256273
joint_velocity_.size() != actuator_velocity_.size() &&
257-
joint_effort_.size() != actuator_effort_.size())
274+
joint_effort_.size() != actuator_effort_.size() &&
275+
joint_torque_.size() != actuator_torque_.size() &&
276+
joint_absolute_position_.size() != actuator_absolute_position_.size())
258277
{
259278
throw Exception(
260279
fmt::format(FMT_COMPILE("Pair-wise mismatch on interfaces. \n{}"), get_handles_info()));
@@ -303,6 +322,32 @@ inline void DifferentialTransmission::actuator_to_joint()
303322
joint_eff[1].set_value(
304323
jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1]));
305324
}
325+
326+
auto & act_tor = actuator_torque_;
327+
auto & joint_tor = joint_torque_;
328+
if (act_tor.size() == num_actuators() && joint_tor.size() == num_joints())
329+
{
330+
assert(act_tor[0] && act_tor[1] && joint_tor[0] && joint_tor[1]);
331+
332+
joint_tor[0].set_value(
333+
jr[0] * (act_tor[0].get_value() * ar[0] + act_tor[1].get_value() * ar[1]));
334+
joint_tor[1].set_value(
335+
jr[1] * (act_tor[0].get_value() * ar[0] - act_tor[1].get_value() * ar[1]));
336+
}
337+
338+
auto & act_abs_pos = actuator_absolute_position_;
339+
auto & joint_abs_pos = joint_absolute_position_;
340+
if (act_abs_pos.size() == num_actuators() && joint_abs_pos.size() == num_joints())
341+
{
342+
assert(act_abs_pos[0] && act_abs_pos[1] && joint_abs_pos[0] && joint_abs_pos[1]);
343+
344+
joint_abs_pos[0].set_value(
345+
(act_abs_pos[0].get_value() / ar[0] + act_abs_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) +
346+
joint_offset_[0]);
347+
joint_abs_pos[1].set_value(
348+
(act_abs_pos[0].get_value() / ar[0] - act_abs_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) +
349+
joint_offset_[1]);
350+
}
306351
}
307352

308353
inline void DifferentialTransmission::joint_to_actuator()
@@ -347,6 +392,18 @@ inline void DifferentialTransmission::joint_to_actuator()
347392
act_eff[1].set_value(
348393
(joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / (2.0 * ar[1]));
349394
}
395+
396+
auto & act_tor = actuator_torque_;
397+
auto & joint_tor = joint_torque_;
398+
if (act_tor.size() == num_actuators() && joint_tor.size() == num_joints())
399+
{
400+
assert(act_tor[0] && act_tor[1] && joint_tor[0] && joint_tor[1]);
401+
402+
act_tor[0].set_value(
403+
(joint_tor[0].get_value() / jr[0] + joint_tor[1].get_value() / jr[1]) / (2.0 * ar[0]));
404+
act_tor[1].set_value(
405+
(joint_tor[0].get_value() / jr[0] - joint_tor[1].get_value() / jr[1]) / (2.0 * ar[1]));
406+
}
350407
}
351408

352409
std::string DifferentialTransmission::get_handles_info() const
@@ -356,10 +413,15 @@ std::string DifferentialTransmission::get_handles_info() const
356413
"Got the following handles:\n"
357414
"Joint position: {}, Actuator position: {}\n"
358415
"Joint velocity: {}, Actuator velocity: {}\n"
359-
"Joint effort: {}, Actuator effort: {}"),
416+
"Joint effort: {}, Actuator effort: {}\n"
417+
"Joint torque: {}, Actuator torque: {}\n"
418+
"Joint absolute position: {}, Actuator absolute position: {}"),
360419
to_string(get_names(joint_position_)), to_string(get_names(actuator_position_)),
361420
to_string(get_names(joint_velocity_)), to_string(get_names(actuator_velocity_)),
362-
to_string(get_names(joint_effort_)), to_string(get_names(actuator_effort_)));
421+
to_string(get_names(joint_effort_)), to_string(get_names(actuator_effort_)),
422+
to_string(get_names(joint_torque_)), to_string(get_names(actuator_torque_)),
423+
to_string(get_names(joint_absolute_position_)),
424+
to_string(get_names(actuator_absolute_position_)));
363425
}
364426

365427
} // namespace transmission_interface

‎transmission_interface/test/differential_transmission_test.cpp‎

Lines changed: 104 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -22,11 +22,13 @@
2222

2323
using hardware_interface::HW_IF_EFFORT;
2424
using hardware_interface::HW_IF_POSITION;
25+
using hardware_interface::HW_IF_TORQUE;
2526
using hardware_interface::HW_IF_VELOCITY;
2627
using testing::DoubleNear;
2728
using transmission_interface::ActuatorHandle;
2829
using transmission_interface::DifferentialTransmission;
2930
using transmission_interface::Exception;
31+
using transmission_interface::HW_IF_ABSOLUTE_POSITION;
3032
using transmission_interface::JointHandle;
3133
// Floating-point value comparison threshold
3234
const double EPS = 1e-5;
@@ -125,6 +127,8 @@ TEST(ConfigureTest, FailsWithBadHandles)
125127
testConfigureWithBadHandles(HW_IF_POSITION);
126128
testConfigureWithBadHandles(HW_IF_VELOCITY);
127129
testConfigureWithBadHandles(HW_IF_EFFORT);
130+
testConfigureWithBadHandles(HW_IF_TORQUE);
131+
testConfigureWithBadHandles(HW_IF_ABSOLUTE_POSITION);
128132
}
129133

130134
class TransmissionSetup : public ::testing::Test
@@ -220,6 +224,7 @@ TEST_F(BlackBoxTest, IdentityMap)
220224
testIdentityMap(transmission, input_value, HW_IF_POSITION);
221225
testIdentityMap(transmission, input_value, HW_IF_VELOCITY);
222226
testIdentityMap(transmission, input_value, HW_IF_EFFORT);
227+
testIdentityMap(transmission, input_value, HW_IF_TORQUE);
223228
}
224229
}
225230
}
@@ -236,7 +241,7 @@ TEST_F(WhiteBoxTest, DontMoveJoints)
236241

237242
DifferentialTransmission trans(actuator_reduction, joint_reduction, joint_offset);
238243

239-
// Actuator input (used for effort, velocity and position)
244+
// Actuator input (used for effort, velocity, position, torque and absolute position)
240245
*a_vec[0] = 0.0;
241246
*a_vec[1] = 0.0;
242247

@@ -275,6 +280,30 @@ TEST_F(WhiteBoxTest, DontMoveJoints)
275280
EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS));
276281
EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS));
277282
}
283+
284+
// Torque interface
285+
{
286+
auto a1_handle = ActuatorHandle("act1", HW_IF_TORQUE, a_vec[0]);
287+
auto a2_handle = ActuatorHandle("act2", HW_IF_TORQUE, a_vec[1]);
288+
auto joint1_handle = JointHandle("joint1", HW_IF_TORQUE, j_vec[0]);
289+
auto joint2_handle = JointHandle("joint2", HW_IF_TORQUE, j_vec[1]);
290+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
291+
trans.actuator_to_joint();
292+
EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS));
293+
EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
294+
}
295+
296+
// Absolute position interface
297+
{
298+
auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]);
299+
auto a2_handle = ActuatorHandle("act2", HW_IF_ABSOLUTE_POSITION, a_vec[1]);
300+
auto joint1_handle = JointHandle("joint1", HW_IF_ABSOLUTE_POSITION, j_vec[0]);
301+
auto joint2_handle = JointHandle("joint2", HW_IF_ABSOLUTE_POSITION, j_vec[1]);
302+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
303+
trans.actuator_to_joint();
304+
EXPECT_THAT(joint_offset[0], DoubleNear(j_val[0], EPS));
305+
EXPECT_THAT(joint_offset[1], DoubleNear(j_val[1], EPS));
306+
}
278307
}
279308

280309
TEST_F(WhiteBoxTest, MoveFirstJointOnly)
@@ -284,7 +313,7 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly)
284313

285314
DifferentialTransmission trans(actuator_reduction, joint_reduction);
286315

287-
// Actuator input (used for effort, velocity and position)
316+
// Actuator input (used for effort, velocity, position, torque and absolute position)
288317
*a_vec[0] = 10.0;
289318
*a_vec[1] = 10.0;
290319

@@ -323,6 +352,30 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly)
323352
EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS));
324353
EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
325354
}
355+
356+
// Torque interface
357+
{
358+
auto a1_handle = ActuatorHandle("act1", HW_IF_TORQUE, a_vec[0]);
359+
auto a2_handle = ActuatorHandle("act2", HW_IF_TORQUE, a_vec[1]);
360+
auto joint1_handle = JointHandle("joint1", HW_IF_TORQUE, j_vec[0]);
361+
auto joint2_handle = JointHandle("joint2", HW_IF_TORQUE, j_vec[1]);
362+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
363+
trans.actuator_to_joint();
364+
EXPECT_THAT(400.0, DoubleNear(j_val[0], EPS));
365+
EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
366+
}
367+
368+
// Absolute position interface
369+
{
370+
auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]);
371+
auto a2_handle = ActuatorHandle("act2", HW_IF_ABSOLUTE_POSITION, a_vec[1]);
372+
auto joint1_handle = JointHandle("joint1", HW_IF_ABSOLUTE_POSITION, j_vec[0]);
373+
auto joint2_handle = JointHandle("joint2", HW_IF_ABSOLUTE_POSITION, j_vec[1]);
374+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
375+
trans.actuator_to_joint();
376+
EXPECT_THAT(0.5, DoubleNear(j_val[0], EPS));
377+
EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
378+
}
326379
}
327380

328381
TEST_F(WhiteBoxTest, MoveSecondJointOnly)
@@ -332,7 +385,7 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly)
332385

333386
DifferentialTransmission trans(actuator_reduction, joint_reduction);
334387

335-
// Actuator input (used for effort, velocity and position)
388+
// Actuator input (used for effort, velocity, position, torque and absolute position)
336389
*a_vec[0] = 10.0;
337390
*a_vec[1] = -10.0;
338391

@@ -371,6 +424,30 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly)
371424
EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS));
372425
EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS));
373426
}
427+
428+
// Torque interface
429+
{
430+
auto a1_handle = ActuatorHandle("act1", HW_IF_TORQUE, a_vec[0]);
431+
auto a2_handle = ActuatorHandle("act2", HW_IF_TORQUE, a_vec[1]);
432+
auto joint1_handle = JointHandle("joint1", HW_IF_TORQUE, j_vec[0]);
433+
auto joint2_handle = JointHandle("joint2", HW_IF_TORQUE, j_vec[1]);
434+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
435+
trans.actuator_to_joint();
436+
EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS));
437+
EXPECT_THAT(400.0, DoubleNear(j_val[1], EPS));
438+
}
439+
440+
// Absolute position interface
441+
{
442+
auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]);
443+
auto a2_handle = ActuatorHandle("act2", HW_IF_ABSOLUTE_POSITION, a_vec[1]);
444+
auto joint1_handle = JointHandle("joint1", HW_IF_ABSOLUTE_POSITION, j_vec[0]);
445+
auto joint2_handle = JointHandle("joint2", HW_IF_ABSOLUTE_POSITION, j_vec[1]);
446+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
447+
trans.actuator_to_joint();
448+
EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS));
449+
EXPECT_THAT(0.5, DoubleNear(j_val[1], EPS));
450+
}
374451
}
375452

376453
TEST_F(WhiteBoxTest, MoveBothJoints)
@@ -424,4 +501,28 @@ TEST_F(WhiteBoxTest, MoveBothJoints)
424501
EXPECT_THAT(-2.01250, DoubleNear(j_val[0], EPS));
425502
EXPECT_THAT(4.06875, DoubleNear(j_val[1], EPS));
426503
}
504+
505+
// Torque interface
506+
{
507+
auto a1_handle = ActuatorHandle("act1", HW_IF_TORQUE, a_vec[0]);
508+
auto a2_handle = ActuatorHandle("act2", HW_IF_TORQUE, a_vec[1]);
509+
auto joint1_handle = JointHandle("joint1", HW_IF_TORQUE, j_vec[0]);
510+
auto joint2_handle = JointHandle("joint2", HW_IF_TORQUE, j_vec[1]);
511+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
512+
trans.actuator_to_joint();
513+
EXPECT_THAT(140.0, DoubleNear(j_val[0], EPS));
514+
EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS));
515+
}
516+
517+
// Absolute position interface
518+
{
519+
auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]);
520+
auto a2_handle = ActuatorHandle("act2", HW_IF_ABSOLUTE_POSITION, a_vec[1]);
521+
auto joint1_handle = JointHandle("joint1", HW_IF_ABSOLUTE_POSITION, j_vec[0]);
522+
auto joint2_handle = JointHandle("joint2", HW_IF_ABSOLUTE_POSITION, j_vec[1]);
523+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
524+
trans.actuator_to_joint();
525+
EXPECT_THAT(-2.01250, DoubleNear(j_val[0], EPS));
526+
EXPECT_THAT(4.06875, DoubleNear(j_val[1], EPS));
527+
}
427528
}

0 commit comments

Comments
 (0)