SRC-4962 Add command type to JointState & ActuatorCommand

[mykolasjuraitis]

Proposed changes

Add command type to JointState & ActuatorCommand so that controllers can indicate what commanded effort means.

Checklist

If the task is applicable to this pull request (see applicability criteria in brackets), make sure it is completed before checking the corresponding box. Otherwise, tick the box right away. Make sure that ALL boxes are checked BEFORE the PR is merged.

• Manually tested that added code works as intended (if any functional/runnable code is added).
• Added automated tests (if a new class is added (Python or C++), interface of that class must be unit tested).
• Tested on real hardware (if the changed or added code is used with the real hardware).
• Added documentation (For any new feature, explain what it does and how to use it. Write the documentation in a relevant space, e.g. Github, Confluence, etc.)

[guihomework]

Hi @mykolasjuraitis , I might be wrong, but I thought ros_ethercat was a robot agnostic package. Making it depend on Shadow messages for a Shadow specific setting (PWM or TORQUE) is a bit strange and not generic anymore.

Just to be a bit more helpful, what about introducing this new variable to the sr_hardware_interface like the valve is introduced in the muscle command here

https://github.com/shadow-robot/sr_core/blob/070d22f20c66dda1734643c6a54c26081c6cab6d/sr_hardware_interface/include/sr_hardware_interface/sr_actuator.hpp#L199

[toliver]

I kind of dislike making the field a ROS msg when none of the others are...
I can't give a good reason for it though.
I guess storing the type could be done with an int enum, while a ROS msg class actually involves a much bigger object, with serialization methods and so on.

[guihomework]

@toliver so you concur with introducing a enum for PWM/TORQUE within the generic ros_ethercat ? What if some other systems want CURRENT (in case they control their efforts in current or so) ?

[mykolasjuraitis]

_control_type_type is actually an alias for int enum. But I agree ideally we should get rid of existing dependency to sr_robot_msgs not add more usages.

Which one would you prefer locally declared enum with values: unspecified, torque, pwm or std::string with suggested values: "", "torque", "pwm" but allowing anything controller wants to pass to the driver?

[mykolasjuraitis]

@guihomework thanks for suggestion at top level comment. We should be able to add desired field in child classes at sr_core repository instead of here.

[toliver]

@guihomework what we are currently trying to modify is the mechanism to let the driver know if it should send a PWM or torque command to the hand.
Currently that is determined by a variable in the driver that can be set via an ENV variable at startup or via a ROS service during runtime.

But in order to avoid issues with the running controllers (that are tuned either to produce PWM or torque cmds) we reload the controller parameters and then call the service "reset_gains" for all running controllers.
This operation involves launching a launchfile, and it takes several seconds, interrupting the realtime loop when that happens.

That's why we want to remove the need for it.
We've thought that a way to achieve that is to add a parameter to each contoller's yaml file to specify if it uses PWM or torque (meaning tendon tension commands). Then when we switch controllers, if all the new controllers use the same type of command, the driver will know that by checking their respective actuator structures and will send the commanded_effort flagged as PWM or torque.

The controller will set the type in the jointState structure (or a specialised child of that, as @mykolasjuraitis is proposing), then the transmissions will propagate that to the actuator structure and the driver will read it from there.

How does that sound? Do you see any conflicts with the way you work or with your custom controllers?
It is certainly going to break compatibility, but hopefully it will be a very easy update.

[toliver]

We have considered doing it in a more "proper" ros_control way, by defining a new hardware_interface for the RosEthercat class (additionally to having joint_effort_command_interface_ we could have a joint_PWM_command_interface_.
Then every controller class would have a specialization for one or the other hardware interface (as it is the case with the standard ros_control controllers).

Then the prepareSwitch and doSwitch methods would take care of switching the command type variable in the hand driver, based on the interfaces used by the running controllers.

The reason why we haven't taken this route that is probably the right way to do it in ros_control is that we think it will involve a lot more changes.

This is something we plan to address when we migrate the driver to ROS2.

[guihomework]

@toliver I completely understand your goal, find it a good idea, and the chosen solution seems ok (I did not think of all corner cases), but my early intuition made me believe it can all be done in the child class defined in sr_hardware_interface not in the parent hardware_interface in ros_ethercat. This way ros_ethercat remains agnostic to the particularity of the hand having 2 effort commands.

most controllers used for the hand are in sr_mechanism_controllers and you will necessarily handle the existing controllers there (we still use mixed-position-velocity controllers, no other special one) so that they work with your new plan. I was mainly "complaining" about changing ros_ethercat in general when fixing internal problems of a particular robot, but we would not be affected by the change as we do not use ros_ethercat out of the Shadow hand. I wonder if the pr2 (if still updated) switched to ros_ethercat instead of pr2_ethercat... They might be affected with this PR.

Regarding "breaking" compatibility, up to now, if one loaded a controller tuned for TORQUE (a 100-unit command would make the joint start to move) on a driver loaded for PWM control this could have consequences. I don't remember which PWM value starts to make the joint move, do you ?. The opposite (driver loaded for TORQUE control and PWM controller) could also happen, but was less frequent, since PWM control was default on hand shipped to customers I believe.

With the plan to use the command_type in the parameters of the controller, a default behaviour (in case this parameter was omitted due to not updated yet) should be chosen that is conservative. Which default you will choose could result in potentially high torques. It all depends what is safer, typical TORQUE tuning output or typical PWM tuning output.
Maybe you could also refuse to start a controller if the parameter is not set, with a warning mentioning this is mandatory. Now. maybe that is even safer.

[toliver]

That's a good point. We were thinking that the driver would throw an error to the user and send PWM=0 to the hand if it detects that the controllers are not setting a valid command_type.

[mykolasjuraitis]

@guihomework I have created similar PR without referencing any Shadow specific data structures: #115 Could you please have a look?

[guihomework]

Hi, @mykolasjuraitis I have been looking for several minutes already and what you seem to have fixed is the non-usage of a sr_robot_msg, which is good, but the PR still changes the ros_ethercat_model.
There is an addition of a field in Commands and in Joint, only for Shadow purposes. I believe it is not generic enough yet to be required to stay at this level of the model.
Otherwise you could argue you also need a valve state there as well.

I had pointed to sr_hardware_interface, as a potential place to add this field but did not look in detail yesterday.
I miss the full picture to see if my intuition is possible or not, and I think you probably looked it up.

Should I still look deeper and maybe suggest a more prepared alternative ?

[mykolasjuraitis]

@guihomework problem is that we can add new fields in actuator data structures in sr_core repository, but I see no way to augment JointState class with additional information about command type. JointState creation is deep in ros_ethercat repo and controllers to use it to send commands via transmission classes. Suggested alternatives are always welcome. As @toliver explained in previous comments what we want to achieve is to propagate type of effort command from controller (for example here https://github.com/shadow-robot/sr_core/blob/noetic-devel/sr_mechanism_controllers/src/srh_joint_effort_controller.cpp#L193) to the driver (to be used here https://github.com/shadow-robot/sr-ros-interface-ethercat/blob/noetic-devel/sr_robot_lib/src/sr_motor_robot_lib.cpp#L267).

[guihomework]

@mykolasjuraitis while you were typing you answer I was looking and EXACTLY found the same line you refer to
https://github.com/shadow-robot/sr-ros-interface-ethercat/blob/noetic-devel/sr_robot_lib/src/sr_motor_robot_lib.cpp#L267
this means I get what you want to do.

@mykolasjuraitis and @toliver I see the problem is on the propagation, not on the internal usage/storage of the type variable which could be in the derived Actuator something like SrMotorActuatorCommand motor_command_ the same as SrMotorActuatorState motor_state_

In summary, the controller loads and reads parameters, and this way knows its effort type and should transmit it to driver. However, this happens only at switch of controllers, you do not expect a controller to live update its type on the fly, do you ?

I see there is no existing mechanism to synchronize the driver and the controller, even at start, except nullifying the command. The joint_state is shared between the controller being stopped and the one starting, and the value effort could change type and the driver needs to know.
So your idea is to use the joint_state, that stores the effort, but augment it with the type, that would then be transmitted for each joint (so multiple times per step), and for each time step It seems overkill to me, and requires this deep change that I don't find adequate. But I do not have good alternatives yet.
I still expose some :

• One could tell to the driver via a single RPC from the controller during start that the command effort will be of "this type". But it is risky as one needs to be sure the joint_state is not used during the switch and also there are as many controllers as there are joints, so 20 RPC would be needed, and the storage of the up-coming type could be in the Actuator I would say. Maybe a stopping controller could set NaN as the commanded_effort, to invalidate the effort, and the new controller could change the effort to zero only in its first update, after the start has done the RPC for announcing the next effort type. Saves lot of parameter reloading if the RPC requests the same type as currently running, and avoid changing a lot of things everywhere, and is internal to shadow (between your drivers and your controllers)
• Use hacky hidden flags in the effort, the same way as for muscle hands. the 2 valve command uses a custom "packing" of the 2 ints into the effort as a double. I believe that the PWM or TORQUE also could live with a INT16 or INT32 even, and the rest of the bits of the double could store extra flags for describing the type of effort you exchange between your controllers and your drivers. If it works for the valves, it should work for the motors as well.

If you still want to go on changing the joint_state I suggest then to make it generic.

• effort and commanded_effort should both have a type,
• with the same generic ENUM
• default type should be NEWTON as it is the current default, and maybe add a RAW too.

currently the hand does not use Newtons and new Shadow hand users are confused. Maybe a joint_state_publisher or the transmission could even read a calibration yaml in the future and publish real newtons from the read TORQUE (divide by 10 roughly).

Shadow packages could then augmented the 2 generic types with PWM, GAUGE_DIFF (because this is actually what the TORQUE is), VALVE_CMD
etc... and then indeed, this would be an enum sent every time but the driver would totally know how to interpret the effort everytime, for every joint.