Add support for custom modes

[mathias-luedtke]

For kinetic:

• fail gracefully if a standard mode does not work
• auto-support all <0 modes
• find a solution for automatic hardware_inteface mapping

For melodic:

• support modes with more than one command
• Add support for pos_vel and pos_vel_acc

[BenArtes]

I'm working on supporting a custom motor controller with a Master / Follower mode similar to the Maxon EPOS MasterEncoder mode, so I'd like to start implementing this. My focus will be on what you have listed as changes for kinetic, custom modes with one command.

Many manufacturer specific modes are single command modes with different behavior so we really just need to find a way to dynamically create new modes based on ModeForwardHelper from configuration settings which would be a run time version of what is done for the currently implemented modes (minus Homing).

For my application I actually want to create a mode that changes the mode of the controller but doesn't forward commands as the motor is configured as a follower.

As you pointed out pos_vel and pos_vel_acc should also eventually be implemented.

Finally for complex custom changes, users can derive from Motor / MotorBase and create a new Motor402 Allocator; I haven't looked into this much and think there might be some trickiness to the interface mapping issues.

The issues that I have identified for one command custom modes are:

1. Definition of Custom Mode, Mappings, Etc
   • Loaded via Params from .yaml config file
2. Addition of Custom Mode Functionality.
   • The default operation modes are implemented as templates, how to create run-time equivalents
3. Mapping of Custom Operation Mode (0x6060 / 0x6061) and Supported drive modes (0x6502)
   • Need to create a mapping between Manufacturer Specific Operation Modes and Supported Drive Mode bits
4. Mapping of Custom Operation Mode and hardware_interfaces
   • Needs to be added to interface_mapping.h which will be tricky as we can't extend canopen::MotorBaseOperationMode enum.

For configuration I would imagine something along the lines of:

custom_operation_mode:
  name: 
  id: -1
  suppored_drive_bit: 16
  interface: "hardware_interface::VelocityJointInterface"
    od_entry: "obj60FF"
      data_type: "int32_t" 

Have I summarized the above well? Any thoughts?

[mathias-luedtke]

Thanks for the detailed summary!

Loaded via Params from .yaml config file

I would say that this step is optional. This could be solved with another plugin system once the other issues are solved.

The default operation modes are implemented as templates, how to create run-time equivalents

Again, more or less optional.

Mapping of Custom Operation Mode (0x6060 / 0x6061) and Supported drive modes (0x6502)
Need to create a mapping between Manufacturer Specific Operation Modes and Supported Drive Mode bits

I am not sure if vendor-specific modes get reflected in 0x6502.
That's why I added MotorBase::isModeSupported.

Mapping of Custom Operation Mode and hardware_interfaces

That's the tricky one.
I guess, the mechanism needs to get changed.
InterfaceMapping is only meant for the standard modes, that's why it is using the enum.

[BenArtes]

I am not sure if vendor-specific modes get reflected in 0x6502.
That's why I added MotorBase::isModeSupported.

Hmm, it looks like I misunderstood. Bits 16-31 are listed as Manufacturer Specific in 0x6502 so I had assumed they corresponded to the Manufacturer Specific Operation Modes, but after reviewing implementations from a number of companies it doesn't appear that they are used that way.

I quickly looked at (Drives with Vendor Specific Modes marked as VSM):

• Beckhoff
• Chieftek
• Elmo
• Faulhaber
• Hiwin
• Kollmorgen - VSM
• Lenze
• Maxon - VSM
• Moog - VSM
• Nanotec
• Parker - VSM
• RoboTeq - VSM
• Trinamic
• TR-Electronic

and the only Vendor who implemented Vendor Specific Modes AND used the Supported Drive Mode bits was Moog (PDF). While not exhaustive (and I may have missed some VSMs in the above list), it certainly looks like there is another issue: Drives have VSMs without any indication.

So we'd either have to automatically look up supported modes from a hard-coded table or include it in the config yaml somehow.

I think creating another Plugin is a good idea, but would we want to create a number of included plugins for each Vendor that users can switch between using an allocator string? That definitely seems better than trying to automatically look-up during run-time from ID Entries, but duplication would certainly be a concern.

That's the tricky one.
I guess, the mechanism needs to get changed.
InterfaceMapping is only meant for the standard modes, that's why it is using the enum.

I'm going to think about this part a bit more and look into where / what the plugin would consist of.

[mathias-luedtke]

So we'd either have to automatically look up supported modes from a hard-coded table or include it in the config yaml somehow.

I would query it from the canopen::Mode instance.

I think creating another Plugin is a good idea, but would we want to create a number of included plugins for each Vendor that users can switch between using an allocator string? That definitely seems better than trying to automatically look-up during run-time from ID Entries, but duplication would certainly be a concern.

The modes could be registered from the vendor-specific MotorBase plugin (currently supported).
Or a Mode plugin system could get implemented.
The latter would allow vendor-specifc modes with hard-coded settings and in addtion a flexible plugin with run-time settings.

We could even mix both ;)
(A MotorBase could just specifies the default modes, which might get overwritten at run-time).

[BenArtes]

Questions:

I would query it from the canopen::Mode instance.

Does this mean you'd add some kind of isModeSupported(&storage) virtual function to canopen::Mode that could query whatever it needs to determine support? This would make the Mode responsible for determining if it is supported, rather than having the MotorBase determine if the Mode is supported?

On first thought0, this feels a little backwards and risks having isModeSupported functions that end up listing out all of the motor drives that support them:

virtual bool Mode::isModeSupported(&storage) {
return isMaxonEPOS() || isBeckHoff() || ... ;
}

Though, admittedly, if many vendors supported the same modes they wouldn't be vendor specific so this problem seems unlikely.

Implementation Plan

My first pass at this is going to try to change things so that a new Motor can be derived from MotorBase / Motor and add new vendor specific modes that get registered with interface_mapping.h.

This partial support would solve my immediate need and get you some code to check that we're actually on the same page. After that as time permits I could implement the plugin system.

interface_mapping

Some issues with the current interface_mapping:

• interface_mapping treats all motors as having the same modes; with vendor specific modes on a system with multiple vendor's drives this might not be true. There needs to be provision for extending interface_mapping on a per motor basis.
• While keeping the hardware_interface::__ strings out of the mode keeps ROS details out of CANopen code, in the extended / plugin use case there is no way to know what string the Vendor Specifc Mode is going to need.

So my proposal would be to create an interface mapping per Motor, add the hardware_interface string to Mode, then build the interface_mapping during initialization. This way the Default Modes will be provided by Motor402::registerDefaultModes but they could be overridden.

For this implementation I'd have to add an interface_mapping to HandleLayerBase, a conflict check function, and switch all canopen::MotorBase::OperationMode types to std::uint16_t potentially with a value check function.

Thoughts?

[mikaelarguedas]

Hi @BenArtes!
How far did you manage to go regarding custom modes support ? Did you succeed to achieve it with ros_canopen ? or did you move to another solution ?
I'm looking at managing motors in a leader / follower pattern that's handled by a manufacturer's custom mode but requires me to support a negative mode and a specific sequence on the control word.
Happy to chat via another channel if your solution moved away from ros_canopen to not derail this thread.

Cheers!

[BenArtes]

I did succeed in adding custom mode support but haven't had the time to clean up the various ros_canopen additions to create a PR.

Essentially I replaced the interface_mapping with a dynamic mapping; the mappings get registered via a call to interface_mapping from the registerMode function (which was modified to accept the necessary arguments).

Then I used the canopen402 plugin to create a custom plugin with a custom ModeTargetHelper; implementing a custom registerDefaultModes that also called the base class registerDefaultModes.

[mikaelarguedas]

That's great to know!
Do you happen to have a public branch somewhere with the various changes you had to make allowing me (and hopefully others) to build support for specific drives on top of it ?

[BenArtes]

Unfortunately not, it's on an internal repository waiting to be cleaned up and pushed to a public server at which point I'll create a PR and try to get the changes merged into ros_canopen.

I'd been hoping to do that sooner, but we're still deep in R&D phase and I am unlikely to get the time to cleanup and publicly publish the changes until we get closer to release and delivering to customers.

[mikaelarguedas]

Thanks for the prompt answer, I'll look into replicating your work in the meantime then. Good luck in your endeavors!