Skip to content

Commit c2546cd

Browse files
jordan-palaciosjordan-palacios
committed
Add force and current interfaces to transmissions
1 parent bb87986 commit c2546cd

File tree

4 files changed

+233
-6
lines changed

4 files changed

+233
-6
lines changed

‎transmission_interface/include/transmission_interface/differential_transmission.hpp‎

Lines changed: 65 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -168,12 +168,16 @@ class DifferentialTransmission : public Transmission
168168
std::vector<JointHandle> joint_velocity_;
169169
std::vector<JointHandle> joint_effort_;
170170
std::vector<JointHandle> joint_torque_;
171+
std::vector<JointHandle> joint_force_;
172+
std::vector<JointHandle> joint_curret_;
171173
std::vector<JointHandle> joint_absolute_position_;
172174

173175
std::vector<ActuatorHandle> actuator_position_;
174176
std::vector<ActuatorHandle> actuator_velocity_;
175177
std::vector<ActuatorHandle> actuator_effort_;
176178
std::vector<ActuatorHandle> actuator_torque_;
179+
std::vector<ActuatorHandle> actuator_force_;
180+
std::vector<ActuatorHandle> actuator_current_;
177181
std::vector<ActuatorHandle> actuator_absolute_position_;
178182
};
179183

@@ -236,12 +240,16 @@ void DifferentialTransmission::configure(
236240
get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_VELOCITY);
237241
joint_effort_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_EFFORT);
238242
joint_torque_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_TORQUE);
243+
joint_force_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_FORCE);
244+
joint_curret_ =
245+
get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_CURRENT);
239246
joint_absolute_position_ =
240247
get_ordered_handles(joint_handles, joint_names, HW_IF_ABSOLUTE_POSITION);
241248

242249
if (
243250
joint_position_.size() != 2 && joint_velocity_.size() != 2 && joint_effort_.size() != 2 &&
244-
joint_torque_.size() != 2 && joint_absolute_position_.size() != 2)
251+
joint_torque_.size() != 2 && joint_force_.size() != 2 && joint_curret_.size() != 2 &&
252+
joint_absolute_position_.size() != 2)
245253
{
246254
throw Exception("Not enough valid or required joint handles were presented.");
247255
}
@@ -254,13 +262,17 @@ void DifferentialTransmission::configure(
254262
get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_EFFORT);
255263
actuator_torque_ =
256264
get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_TORQUE);
265+
actuator_force_ =
266+
get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_FORCE);
267+
actuator_current_ =
268+
get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_CURRENT);
257269
actuator_absolute_position_ =
258270
get_ordered_handles(actuator_handles, actuator_names, HW_IF_ABSOLUTE_POSITION);
259271

260272
if (
261273
actuator_position_.size() != 2 && actuator_velocity_.size() != 2 &&
262-
actuator_effort_.size() != 2 && actuator_torque_.size() != 2 &&
263-
actuator_absolute_position_.size() != 2)
274+
actuator_effort_.size() != 2 && actuator_torque_.size() != 2 && actuator_force_.size() != 2 &&
275+
actuator_current_.size() != 2 && actuator_absolute_position_.size() != 2)
264276
{
265277
throw Exception(
266278
fmt::format(
@@ -273,6 +285,8 @@ void DifferentialTransmission::configure(
273285
joint_velocity_.size() != actuator_velocity_.size() &&
274286
joint_effort_.size() != actuator_effort_.size() &&
275287
joint_torque_.size() != actuator_torque_.size() &&
288+
joint_force_.size() != actuator_force_.size() &&
289+
joint_curret_.size() != actuator_current_.size() &&
276290
joint_absolute_position_.size() != actuator_absolute_position_.size())
277291
{
278292
throw Exception(
@@ -335,6 +349,28 @@ inline void DifferentialTransmission::actuator_to_joint()
335349
jr[1] * (act_tor[0].get_value() * ar[0] - act_tor[1].get_value() * ar[1]));
336350
}
337351

352+
auto & act_for = actuator_force_;
353+
auto & joint_for = joint_force_;
354+
if (act_for.size() == num_actuators() && joint_for.size() == num_joints())
355+
{
356+
assert(act_for[0] && act_for[1] && joint_for[0] && joint_for[1]);
357+
358+
joint_for[0].set_value(
359+
jr[0] * (act_for[0].get_value() * ar[0] + act_for[1].get_value() * ar[1]));
360+
joint_for[1].set_value(
361+
jr[1] * (act_for[0].get_value() * ar[0] - act_for[1].get_value() * ar[1]));
362+
}
363+
364+
auto & act_cur = actuator_current_;
365+
auto & joint_cur = joint_curret_;
366+
if (act_cur.size() == num_actuators() && joint_cur.size() == num_joints())
367+
{
368+
assert(act_cur[0] && act_cur[1] && joint_cur[0] && joint_cur[1]);
369+
370+
joint_cur[0].set_value(act_cur[0].get_value() + act_cur[1].get_value());
371+
joint_cur[1].set_value(act_cur[0].get_value() - act_cur[1].get_value());
372+
}
373+
338374
auto & act_abs_pos = actuator_absolute_position_;
339375
auto & joint_abs_pos = joint_absolute_position_;
340376
if (act_abs_pos.size() == num_actuators() && joint_abs_pos.size() == num_joints())
@@ -404,6 +440,28 @@ inline void DifferentialTransmission::joint_to_actuator()
404440
act_tor[1].set_value(
405441
(joint_tor[0].get_value() / jr[0] - joint_tor[1].get_value() / jr[1]) / (2.0 * ar[1]));
406442
}
443+
444+
auto & act_for = actuator_force_;
445+
auto & joint_for = joint_force_;
446+
if (act_for.size() == num_actuators() && joint_for.size() == num_joints())
447+
{
448+
assert(act_for[0] && act_for[1] && joint_for[0] && joint_for[1]);
449+
450+
act_for[0].set_value(
451+
(joint_for[0].get_value() / jr[0] + joint_for[1].get_value() / jr[1]) / (2.0 * ar[0]));
452+
act_for[1].set_value(
453+
(joint_for[0].get_value() / jr[0] - joint_for[1].get_value() / jr[1]) / (2.0 * ar[1]));
454+
}
455+
456+
auto & act_cur = actuator_current_;
457+
auto & joint_cur = joint_curret_;
458+
if (act_cur.size() == num_actuators() && joint_cur.size() == num_joints())
459+
{
460+
assert(act_cur[0] && act_cur[1] && joint_cur[0] && joint_cur[1]);
461+
462+
act_cur[0].set_value((joint_cur[0].get_value() + joint_cur[1].get_value()) / 2.0);
463+
act_cur[1].set_value((joint_cur[0].get_value() - joint_cur[1].get_value()) / 2.0);
464+
}
407465
}
408466

409467
std::string DifferentialTransmission::get_handles_info() const
@@ -415,11 +473,15 @@ std::string DifferentialTransmission::get_handles_info() const
415473
"Joint velocity: {}, Actuator velocity: {}\n"
416474
"Joint effort: {}, Actuator effort: {}\n"
417475
"Joint torque: {}, Actuator torque: {}\n"
476+
"Joint force: {}, Actuator force: {}\n"
477+
"Joint current: {}, Actuator current: {}\n"
418478
"Joint absolute position: {}, Actuator absolute position: {}"),
419479
to_string(get_names(joint_position_)), to_string(get_names(actuator_position_)),
420480
to_string(get_names(joint_velocity_)), to_string(get_names(actuator_velocity_)),
421481
to_string(get_names(joint_effort_)), to_string(get_names(actuator_effort_)),
422482
to_string(get_names(joint_torque_)), to_string(get_names(actuator_torque_)),
483+
to_string(get_names(joint_force_)), to_string(get_names(actuator_force_)),
484+
to_string(get_names(joint_curret_)), to_string(get_names(actuator_current_)),
423485
to_string(get_names(joint_absolute_position_)),
424486
to_string(get_names(actuator_absolute_position_)));
425487
}

‎transmission_interface/include/transmission_interface/simple_transmission.hpp‎

Lines changed: 31 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -135,12 +135,16 @@ class SimpleTransmission : public Transmission
135135
JointHandle joint_velocity_ = {"", "", nullptr};
136136
JointHandle joint_effort_ = {"", "", nullptr};
137137
JointHandle joint_torque_ = {"", "", nullptr};
138+
JointHandle joint_force_ = {"", "", nullptr};
139+
JointHandle joint_current_ = {"", "", nullptr};
138140
JointHandle joint_absolute_position_ = {"", "", nullptr};
139141

140142
ActuatorHandle actuator_position_ = {"", "", nullptr};
141143
ActuatorHandle actuator_velocity_ = {"", "", nullptr};
142144
ActuatorHandle actuator_effort_ = {"", "", nullptr};
143145
ActuatorHandle actuator_torque_ = {"", "", nullptr};
146+
ActuatorHandle actuator_force_ = {"", "", nullptr};
147+
ActuatorHandle actuator_current_ = {"", "", nullptr};
144148
ActuatorHandle actuator_absolute_position_ = {"", "", nullptr};
145149
};
146150

@@ -206,11 +210,13 @@ inline void SimpleTransmission::configure(
206210
joint_velocity_ = get_by_interface(joint_handles, hardware_interface::HW_IF_VELOCITY);
207211
joint_effort_ = get_by_interface(joint_handles, hardware_interface::HW_IF_EFFORT);
208212
joint_torque_ = get_by_interface(joint_handles, hardware_interface::HW_IF_TORQUE);
213+
joint_force_ = get_by_interface(joint_handles, hardware_interface::HW_IF_FORCE);
214+
joint_current_ = get_by_interface(joint_handles, hardware_interface::HW_IF_CURRENT);
209215
joint_absolute_position_ = get_by_interface(joint_handles, HW_IF_ABSOLUTE_POSITION);
210216

211217
if (
212-
!joint_position_ && !joint_velocity_ && !joint_effort_ && !joint_torque_ &&
213-
!joint_absolute_position_)
218+
!joint_position_ && !joint_velocity_ && !joint_effort_ && !joint_torque_ && !joint_force_ &&
219+
!joint_current_ && !joint_absolute_position_)
214220
{
215221
throw Exception("None of the provided joint handles are valid or from the required interfaces");
216222
}
@@ -219,11 +225,13 @@ inline void SimpleTransmission::configure(
219225
actuator_velocity_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_VELOCITY);
220226
actuator_effort_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_EFFORT);
221227
actuator_torque_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_TORQUE);
228+
actuator_force_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_FORCE);
229+
actuator_current_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_CURRENT);
222230
actuator_absolute_position_ = get_by_interface(actuator_handles, HW_IF_ABSOLUTE_POSITION);
223231

224232
if (
225233
!actuator_position_ && !actuator_velocity_ && !actuator_effort_ && !actuator_torque_ &&
226-
!actuator_absolute_position_)
234+
!actuator_force_ && !actuator_current_ && !actuator_absolute_position_)
227235
{
228236
throw Exception("None of the provided joint handles are valid or from the required interfaces");
229237
}
@@ -251,6 +259,16 @@ inline void SimpleTransmission::actuator_to_joint()
251259
joint_torque_.set_value(actuator_torque_.get_value() * reduction_);
252260
}
253261

262+
if (joint_force_ && actuator_force_)
263+
{
264+
joint_force_.set_value(actuator_force_.get_value() * reduction_);
265+
}
266+
267+
if (joint_current_ && actuator_current_)
268+
{
269+
joint_current_.set_value(actuator_current_.get_value());
270+
}
271+
254272
if (joint_absolute_position_ && actuator_absolute_position_)
255273
{
256274
joint_absolute_position_.set_value(
@@ -279,6 +297,16 @@ inline void SimpleTransmission::joint_to_actuator()
279297
{
280298
actuator_torque_.set_value(joint_torque_.get_value() / reduction_);
281299
}
300+
301+
if (joint_force_ && actuator_force_)
302+
{
303+
actuator_force_.set_value(joint_force_.get_value() / reduction_);
304+
}
305+
306+
if (joint_current_ && actuator_current_)
307+
{
308+
actuator_current_.set_value(joint_current_.get_value());
309+
}
282310
}
283311

284312
} // namespace transmission_interface

‎transmission_interface/test/differential_transmission_test.cpp‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,9 @@
2020
#include "random_generator_utils.hpp"
2121
#include "transmission_interface/differential_transmission.hpp"
2222

23+
using hardware_interface::HW_IF_CURRENT;
2324
using hardware_interface::HW_IF_EFFORT;
25+
using hardware_interface::HW_IF_FORCE;
2426
using hardware_interface::HW_IF_POSITION;
2527
using hardware_interface::HW_IF_TORQUE;
2628
using hardware_interface::HW_IF_VELOCITY;
@@ -128,6 +130,8 @@ TEST(ConfigureTest, FailsWithBadHandles)
128130
testConfigureWithBadHandles(HW_IF_VELOCITY);
129131
testConfigureWithBadHandles(HW_IF_EFFORT);
130132
testConfigureWithBadHandles(HW_IF_TORQUE);
133+
testConfigureWithBadHandles(HW_IF_FORCE);
134+
testConfigureWithBadHandles(HW_IF_CURRENT);
131135
testConfigureWithBadHandles(HW_IF_ABSOLUTE_POSITION);
132136
}
133137

@@ -225,6 +229,9 @@ TEST_F(BlackBoxTest, IdentityMap)
225229
testIdentityMap(transmission, input_value, HW_IF_VELOCITY);
226230
testIdentityMap(transmission, input_value, HW_IF_EFFORT);
227231
testIdentityMap(transmission, input_value, HW_IF_TORQUE);
232+
testIdentityMap(transmission, input_value, HW_IF_FORCE);
233+
// testIdentityMap(transmission, input_value, HW_IF_CURRENT);
234+
// testIdentityMap(transmission, input_value, HW_IF_ABSOLUTE_POSITION);
228235
}
229236
}
230237
}
@@ -293,6 +300,30 @@ TEST_F(WhiteBoxTest, DontMoveJoints)
293300
EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
294301
}
295302

303+
// Force interface
304+
{
305+
auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]);
306+
auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]);
307+
auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]);
308+
auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, j_vec[1]);
309+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
310+
trans.actuator_to_joint();
311+
EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS));
312+
EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
313+
}
314+
315+
// Current interface
316+
{
317+
auto a1_handle = ActuatorHandle("act1", HW_IF_CURRENT, a_vec[0]);
318+
auto a2_handle = ActuatorHandle("act2", HW_IF_CURRENT, a_vec[1]);
319+
auto joint1_handle = JointHandle("joint1", HW_IF_CURRENT, j_vec[0]);
320+
auto joint2_handle = JointHandle("joint2", HW_IF_CURRENT, j_vec[1]);
321+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
322+
trans.actuator_to_joint();
323+
EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS));
324+
EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
325+
}
326+
296327
// Absolute position interface
297328
{
298329
auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]);
@@ -365,6 +396,30 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly)
365396
EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
366397
}
367398

399+
// Force interface
400+
{
401+
auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]);
402+
auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]);
403+
auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]);
404+
auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, j_vec[1]);
405+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
406+
trans.actuator_to_joint();
407+
EXPECT_THAT(400.0, DoubleNear(j_val[0], EPS));
408+
EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
409+
}
410+
411+
// Current interface
412+
{
413+
auto a1_handle = ActuatorHandle("act1", HW_IF_CURRENT, a_vec[0]);
414+
auto a2_handle = ActuatorHandle("act2", HW_IF_CURRENT, a_vec[1]);
415+
auto joint1_handle = JointHandle("joint1", HW_IF_CURRENT, j_vec[0]);
416+
auto joint2_handle = JointHandle("joint2", HW_IF_CURRENT, j_vec[1]);
417+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
418+
trans.actuator_to_joint();
419+
EXPECT_THAT(20.0, DoubleNear(j_val[0], EPS));
420+
EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
421+
}
422+
368423
// Absolute position interface
369424
{
370425
auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]);
@@ -437,6 +492,30 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly)
437492
EXPECT_THAT(400.0, DoubleNear(j_val[1], EPS));
438493
}
439494

495+
// Force interface
496+
{
497+
auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]);
498+
auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]);
499+
auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]);
500+
auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, j_vec[1]);
501+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
502+
trans.actuator_to_joint();
503+
EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS));
504+
EXPECT_THAT(400.0, DoubleNear(j_val[1], EPS));
505+
}
506+
507+
// Current interface
508+
{
509+
auto a1_handle = ActuatorHandle("act1", HW_IF_CURRENT, a_vec[0]);
510+
auto a2_handle = ActuatorHandle("act2", HW_IF_CURRENT, a_vec[1]);
511+
auto joint1_handle = JointHandle("joint1", HW_IF_CURRENT, j_vec[0]);
512+
auto joint2_handle = JointHandle("joint2", HW_IF_CURRENT, j_vec[1]);
513+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
514+
trans.actuator_to_joint();
515+
EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS));
516+
EXPECT_THAT(20.0, DoubleNear(j_val[1], EPS));
517+
}
518+
440519
// Absolute position interface
441520
{
442521
auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]);
@@ -514,6 +593,30 @@ TEST_F(WhiteBoxTest, MoveBothJoints)
514593
EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS));
515594
}
516595

596+
// Force interface
597+
{
598+
auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]);
599+
auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]);
600+
auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]);
601+
auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, j_vec[1]);
602+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
603+
trans.actuator_to_joint();
604+
EXPECT_THAT(140.0, DoubleNear(j_val[0], EPS));
605+
EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS));
606+
}
607+
608+
// Current interface
609+
{
610+
auto a1_handle = ActuatorHandle("act1", HW_IF_CURRENT, a_vec[0]);
611+
auto a2_handle = ActuatorHandle("act2", HW_IF_CURRENT, a_vec[1]);
612+
auto joint1_handle = JointHandle("joint1", HW_IF_CURRENT, j_vec[0]);
613+
auto joint2_handle = JointHandle("joint2", HW_IF_CURRENT, j_vec[1]);
614+
trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
615+
trans.actuator_to_joint();
616+
EXPECT_THAT(8.0, DoubleNear(j_val[0], EPS));
617+
EXPECT_THAT(-2.0, DoubleNear(j_val[1], EPS));
618+
}
619+
517620
// Absolute position interface
518621
{
519622
auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]);

0 commit comments

Comments
 (0)