Realtime arm servoing with Staubli TX2-90

[davidDS96]

Hello community,

In our research group, we would like to perform Realtime arm servoing with a Staubli TX2-90 robot arm with a CS9 controller.
The realtime arm servoing, as mentioned in the tutorial of MoveIt (https://ros-planning.github.io/moveit_tutorials/doc/realtime_servo/realtime_servo_tutorial.html) requires a JointGroupPositionController or a JointGroupVelocityController.
Is it possible to add this controller or to switch to this type of controller with the current release of the Staubli driver?

Kind regards,
David

[gavanderhoorn]

The controller mentioned in the Servo tutorial is a ros_control controller. staubli_val3_driver is not ros_control based, so the answer to your exact question would be: no, that is not possible.

Servo however does not require those controllers. It requires a topic-based interface with similar semantics (ie: accepted message types and behaviour). Andy probably described it as he does, because it would otherwise get too vague for users unfamiliar with how things work (the rest of the tutorial is also heavily ros_control dependent, but that's just because they use Gazebo).

Depending on your familiarity with VAL 3, it could be possible to add such a "streaming" interface to staubli_val3_driver, but it would require more work than simply switching controllers.

An approach similar to ros-industrial/motoman#215 could be used.

#30 describes some experiences by other users attempting to use the existing interface, but that didn't work out too well.

[gavanderhoorn]

Another alternative could be to look at UniVAL. This is a much lower-level interface to Staubli controllers.

It's a paid option though, and would require development in languages other than VAL 3.

Perhaps @marshallpowell97 could add something about that.

[gavanderhoorn]

Having written that, I just noticed a new fork (FAU-FAPS/staubli_val3_driver) which seems to add "velocity control": master...FAU-FAPS:faps-devel, which would make what I wrote in #32 (comment) already quite a bit easier.

Perhaps @enginkarlidag can comment on his work, and say something about latency and whether using MoveIt servo would be possible.

---

Edit: and also @enginkarlidag: your fork appears to miss #8 and #15, which fixed some of the issues you have also addressed (magic nrs and some others).

[enginkarlidag]

Hi,

our fork was noticed faster than I thought ;)

We are currently working on the release of our work which we called an "Adaptive Motion Control Middleware". It is integrated into the existing system architecture of a ROS-I (simple message) based ROS 1 driver as a kind of abstraction layer. The "middleware" adds motion control modes like jogging and pose tracking. For this, we modified the robot-specific drivers for Staubli (VAL3) and Motoman (MotoPlus) to add a velocity control mode. I will tell you when the repo is public. It should be in a few days.

About latency:
The control loop is depending on the interpolation clock of the robot controller (e.g. 4 ms for Staubli CS8).

About MoveIt Servo:
I know the package but I didn't check if it would be "compatible" to our approach tbh.

About #8 and #15:
The VAL3 implementation is actually quite old. We are just releasing it now. Therefore some issues (e.g. magic numbers) are solved differently. Nevertheless, I have tried to "rebase" our state to a relatively current commit.

[gavanderhoorn]

our fork was noticed faster than I thought ;)

I just happened to take a look at the network :)

We are currently working on the release of our work which we called an "Adaptive Motion Control Middleware". It is integrated into the existing system architecture of a ROS-I (simple message) based ROS 1 driver as a kind of abstraction layer. The "middleware" adds motion control modes like jogging and pose tracking. For this, we modified the robot-specific drivers for Staubli (VAL3) and Motoman (MotoPlus) to add a velocity control mode. I will tell you when the repo is public. It should be in a few days.

Sounds interesting.

Are there any plans to contribute these changes upstream? They seem very useful for other users as well.

About latency:
The control loop is depending on the interpolation clock of the robot controller (e.g. 4 ms for Staubli CS8).

Ok.

But what about total end-to-end latency? What is the depth of the buffer on the OEM controller's side for instance? Does that influence performance?

Have you done any performance measurements of your setup?

About MoveIt Servo:
I know the package but I didn't check if it would be "compatible" to our approach tbh.

How do you interact with the extended driver? What sort of messages are exchanged?

In the basis, MoveIt Servo needs a Float64MultiArray or similar topic interface.

About #8 and #15:
The VAL3 implementation is actually quite old. We are just releasing it now. Therefore some issues (e.g. magic numbers) are solved differently. Nevertheless, I have tried to "rebase" our state to a relatively current commit.

Ok.

I'd like to consider merging your extensions into the main repository. That would require some 'conflict resolution' it seems.

[enginkarlidag]

Sounds interesting.

Are there any plans to contribute these changes upstream? They seem very useful for other users as well.

The "middleware" package and the driver forks were developed at our lab while I was working there as a student assistant. The code is released now in parallel to a paper which will be presented soon. I would have to discuss this with the responsible people at the lab.

Ok.

But what about total end-to-end latency? What is the depth of the buffer on the OEM controller's side for instance? Does that influence performance?

Have you done any performance measurements of your setup?

There is no buffering in the driver (server) implementation at least. Only the most current commands will be executed in velocity control. We are using VAL3 add-on functions which apply joint/cartesian velocity commands.

Unfortunately we don't have any performance measurements for Staubli. We have done some measurements for Motoman. Maybe I can do a "quick&dirty" measurement on commanded vs. feedback velocity. At least for joint velocity commands this is done quickly with rqt_plot. Would that be sufficient for a first impression about the expected latencies?

How do you interact with the extended driver? What sort of messages are exchanged?

In the basis, MoveIt Servo needs a Float64MultiArray or similar topic interface.

On the ROS side we use simple topics for velocity commands (TwistStamped, JointJog) and for pose goals (PoseStamped) in the middleware node. The communication with the driver on the robot controller (server) is done via Simple Messages. I've implemented some new messages for this.

Ok.

I'd like to consider merging your extensions into the main repository. That would require some 'conflict resolution' it seems.

That would be cool. If there are any problems with merging, I am available for further questions.

---

Edit: It should be noted that the used velocity functions (and also another function to get the joint speeds) in VAL3 are only available if a certain add-on is installed. When you merge these extensions, maybe the functionality provided by add-ons should be optional. I think there should be a possibility in VAL3 to check the available add-ons...

[gavanderhoorn]

Sounds interesting.
Are there any plans to contribute these changes upstream? They seem very useful for other users as well.

The "middleware" package and the driver forks were developed in a project at our lab while I was working there as a student assistant. The code is released now in parallel to a paper which will be presented soon. I would have to discuss this with the responsible people at the lab.

To clarify: I was referring to the changes to staubli_val3_driver. Not the parts you refer to as "middleware" per se.

Unfortunately we don't have any performance measurements for Staubli. We have done some measurements for Motoman. Maybe I can do a "quick&dirty" measurement on commanded vs. feedback velocity. At least for joint velocity commands this is done quickly with rqt_plot. Would that be sufficient for a first impression about the expected latencies?

that could already provide a nice first insight into what users could expect, yes.

Edit: It should be noted that the used velocity functions (and also another function to get the joint speeds) in VAL3 are only available if a certain add-on is installed. When you merge these extensions, maybe the functionality provided by add-ons should be optional. I think there should be a possibility in VAL3 to check the available add-ons...

yes, I'd understood that from reading the code.

IIRC, that velocity addon is/was a free addon, so it should not be too much of a problem for interested users.

[enginkarlidag]

Sounds interesting.
Are there any plans to contribute these changes upstream? They seem very useful for other users as well.

The "middleware" package and the driver forks were developed in a project at our lab while I was working there as a student assistant. The code is released now in parallel to a paper which will be presented soon. I would have to discuss this with the responsible people at the lab.

To clarify: I was referring to the changes to staubli_val3_driver. Not the parts you refer to as "middleware" per se.

Ok, we would like to contribute our changes upstream. But I am new to GitHub, so I'll certainly need some assistance for the required steps. Should I open a pull request in our faps-devel branch?

BTW, our adaptive_motion_control repo is now public. In the robot_middleware package we implemented the additional motion control modes (jogging, pose tracking) based on velocity commands. The middleware could be used to implement real-time arm servoing as described by @davidDS96. Here is the link for anyone interested:

https://github.com/FAU-FAPS/adaptive_motion_control

Unfortunately we don't have any performance measurements for Staubli. We have done some measurements for Motoman. Maybe I can do a "quick&dirty" measurement on commanded vs. feedback velocity. At least for joint velocity commands this is done quickly with rqt_plot. Would that be sufficient for a first impression about the expected latencies?

that could already provide a nice first insight into what users could expect, yes.

I've made a quick measurement (commanded vs. feedback joint velocity):

I would like to describe some details about the plots in the following notes:

• the robot_middleware was used to control the robot (jogging)
• a motion_control_msgs/VelocityCommand message is published on the /robot_driver/raw_vel_cmd topic. It contains the unfiltered (raw) command as received by the middleware and is published right after the actual velocity command (simple message) was sent to the robot
• the /joint_states topic is affected by a(n additional) delay caused by the relaying of the state messages (simple message) through the middleware
• it is obvious that the raw velocity commands are filtered. This is how the VAL3 add-on Velocity handles this. (I have used another VAL3 add-on Alter in the past, which enables much faster control of the robot)

For a better understanding of how the middleware works you can have a look at the README file. The documentation is not yet complete. For example, the description of the ROS API (params, topics etc.) is still missing, but I plan to add that asap.

[IvoD1998]

hi @enginkarlidag
I have been looking into your middleware software for the purpose discussed above. I want to use it to perform real time servoing of a staubli TX2-90 arm. However, as far as I can tell, the driver alterations you made for the staubli val3 driver only work when using a CS8 controller, not the CS9 model. Is this correct or am I missing something?
In case the current version only works on the CS8 model, do you have any indication if and when a CS9 compatible version will be released?

Kind regards
Ivo

[enginkarlidag]

Hi @IvoD1998,

yes we developed the VAL3 driver on/for a CS8 controller. We provide the results of our work on a "as is" basis and I will most likely not be able to actively work on it anymore. But I just opened a pull request and when it's merged into the main repo it should also support the CS9 controller. Of course, I will help where I can to resolve the merge conflicts.

If you don't want to wait you could try to make it work on the CS9 controller yourself. AFAIK, only the screen related VAL3 functions and data types are not supported in the new version. Well, and maybe also some IO related stuff... You can have a look at the discussions here and here.