File tree Expand file tree Collapse file tree 1 file changed +10
-3
lines changed Expand file tree Collapse file tree 1 file changed +10
-3
lines changed Original file line number Diff line number Diff line change @@ -398,12 +398,19 @@ hardware_interface::return_type FrankaHardwareInterface::perform_command_mode_sw
398
398
hardware_interface::return_type FrankaHardwareInterface::prepare_command_mode_switch (
399
399
const std::vector<std::string>& start_interfaces,
400
400
const std::vector<std::string>& stop_interfaces) {
401
- auto contains_interface_type = [](const std::string& interface,
402
- const std::string& interface_type) {
401
+ auto contains_interface_type = [this ](const std::string& interface,
402
+ const std::string& interface_type) {
403
403
size_t slash_position = interface.find (' /' );
404
404
if (slash_position != std::string::npos && slash_position + 1 < interface.size ()) {
405
405
std::string after_slash = interface.substr (slash_position + 1 );
406
- return after_slash == interface_type;
406
+ std::string before_slash = interface.substr (0 , slash_position - 1 );
407
+ if (after_slash == interface_type) {
408
+ for (const auto & joint : this ->info_ .joints ) {
409
+ if (before_slash == joint.name ) {
410
+ return true ;
411
+ }
412
+ }
413
+ }
407
414
}
408
415
return false ;
409
416
};
You can’t perform that action at this time.
0 commit comments