Velocity control mode & new robot_state/system/io interfaces

[enginkarlidag]

VAL3

• Velocity control implemented with functions for velocity commands -> requires add-on (Velocity or Motion)!
• Velocity control possible in joint space (JOINT) or cartesian space (w.r.t. BASE/TOOL frame)
• Velocity control mode is started with a VelocityConfig message with various parameters (e.g. vel/accel limits)
• Added state machine to switch between motion control modes (JOINT_TRAJECTORY_STREAMING, VELOCITY_CONTROL) -> the default mode is JOINT_TRAJECTORY_STREAMING and works as before
• Increased feedback task frequency (synced with interpolation clock)
• SYSTEM socket interface (receiver task, port 11001) to handle system commands (e.g. set drive power)
• IO socket interface (receiver task, port 11003) to handle IO commands (e.g. read/write IO) -> only writing single IO is fully implemented

Other changes/improvements:

• Sending JointFeedback message including joint velocities -> requires add-on (Motion)! Note: The generic industrial_robot_client doesn't support JointFeedback messages
• Renamed socket name "Feedback" -> "State" (socket configuration must be adapted on the controller!)
• Used VAL3 libraries and custom types for better modularization
• Used VAL3 custom types for "enums"
• Added debug screen for debug messages (switching to the debug screen activates the "debug mode")

NOTE: Developed and tested on a CS8 Controller only!

ROS-Industrial clients

• robot_state node with additional support for JointFeedback messages
• IO interface for reading/writing IO (only "write single IO" fully implemented)
• system interface for system commands ("set drive power" implemented)
• added staubli_msgs package (with IO related messages/services)

[enginkarlidag]

@gavanderhoorn Could you have a look at this? You said that you would consider merging our extensions in #32 (Comment).

I would like to help as much as I can before I leave the university. Now I still have the opportunity to test this on a real robot (at least for CS8 and maybe also for CS9).

Just tell me how I can help:

• Would it help if I merge the master branch into our faps-devel branch, resolve the conflicts locally and push it again?
• Or should I try to rebase it on top of the master branch?

[gavanderhoorn]

Hi, thanks for the PR.

Could you have a look at this? You said that you would consider merging our extensions in #32 (Comment).

Yes, I did.

I do have to say we're always going to be looking to maintain backwards compatibility as much as possible. That doesn't mean we can never introduce breaking changes, but it would be great if we could try to avoid introducing them unless necessary.

Especially changing default behaviour should not be weight against the disadvantages of doing that.

Just tell me how I can help:

• Would it help if I merge the master branch into our faps-devel branch, resolve the conflicts locally and push it again?

• Or should I try to rebase it on top of the master branch?

It doesn't appear as-if there would be an advantage to maintaining the history of parallel development in the repository here and your fork, so to avoid adding (more) merge commits, a rebase would be greatly appreciated, yes.

I expect most conflicts to be caused by the magic nr removal and some of the network parsing code.

[gavanderhoorn]

And I forgot to mention: it would be great if we could try to avoid introducing additional, required dependencies on the VAL 3 side.

The velocity control infrastructure you added introduces one. If we could somehow make it possible for the code to auto-detect whether that addon is present, or in some other way make it possible to configure its use, that would be something we should do.

We can't make it a hard requirement for the "default" driver, as that would make it unusable for many (current) users.

[gavanderhoorn]

Just a comment to let you know this PR is not forgotten.

It's a big PR with many changes and therefore difficult to review. It also requires quite some setup to review, which doesn't make it any easier.

It would be great if people who have looked at this code already (such as @IvoD1998 in #41) could comment on what their experiences are.

[IvoD1998]

Due to other tasks and projects, we haven't had the time to review this code yet. We have tried to translate their IO functionality manually which we got to work on the CS9, but haven't looked into this further.

My apologies.

[IvoD1998]

We adjusted the FAU-FAPS driver ourselves to be functional on CS9 and came to the following conclusions 1) In order for it to work, you first need to acquire the motion/velocity add-ons from Staubli as stated in the readme. 2) The VAL3 code has to be adjusted by removing the screen-related commands as they are no longer present in CS9. (This implies removing the debug-sections of the VAL3 code) 3) The IO of the robot has to be linked before IO functionality is usable 4) The CMake of the VAL3 driver and the and the robot_middleware (velocity control). the CMake of the middleware requires additional dependencies of industrial_msgs and industrial_robot_client. Both require an updated target_compile_definition for industrial_robot_client:

target_compile_definitions(
  robot_middleware
  PRIVATE
  ${industrial_robot_client_DEFINITIONS})

After preliminary testing of the jog functionality everything appears to function on the CS9 controller now, but ofcourse we have only done quick and simple testing now.

I hope this is of help! Kind regards

[leo002]

Hi, I just wanted to say a big thanks for everybody working on this project. I'm quite new to ROS and VAL3 and it was not easy to make this PR working. Eventually I can confirm this PR#36 works on a CS8 controller. I might have run into some minor problems though, of which I'm not too sure about at this moment.

One thing I encountered is when you use the Message Publisher in rqt and set a refresh rate of below <4/sec on the topic /jog_interface/vel_cmd, the motion stutters and the actual moving speed seems to depend on the publishing refresh rate. For publishing refresh rates above and including 4/sec, the actual motion speed doesn't seem to change, which is a good thing.

What's the reason for this behavior and what's the recommended publishing rate?

[enginkarlidag]

Hi, I just wanted to say a big thanks for everybody working on this project. I'm quite new to ROS and VAL3 and it was not easy to make this PR working. Eventually I can confirm this PR#36 works on a CS8 controller. I might have run into some minor problems though, of which I'm not too sure about at this moment.

One thing I encountered is when you use the Message Publisher in rqt and set a refresh rate of below <4/sec on the topic /jog_interface/vel_cmd, the motion stutters and the actual moving speed seems to depend on the publishing refresh rate. For publishing refresh rates above and including 4/sec, the actual motion speed doesn't seem to change, which is a good thing.

What's the reason for this behavior and what's the recommended publishing rate?

Hi, this is actually a "safety feature" 😁

There is a timeout of 250 ms (in the example configuraton file) for the jog command so it will stop after this timeout if no message was received. Which makes sense with your description when you say it's ok for 4 published messages per second. Otherwise the robot would keep moving even if nothing is published at all after just one message. In an error scenario where someone forgets to publish the stop command this could be dangerous. The stuttering you saw was caused by the timeout and thus the stop commands in between.

There is no timeout / publishing rate I can recommend but I chose 250 ms for the timeout so the required minimum publisher rate is not too high with 4/sec and the robot will stop not too late if anything goes wrong.

Feel free to use your own configuration. You can set the timeout here.