Added calculation of joint velocities.

[hartmanndennis]

With this PR computed joint velocities are published by JointStatePublisher and can be viewed with rqt plot for example.

[gavanderhoorn]

Thanks for the PR.

Can you say anything about the (perceived) 'quality' of the velocities that come out of this?

In my experience dx/dt is really not a very good way to derive joint velocities and can result in noisy data.

---

I'm not opposed in general to adding this, but I think we need to be very careful with adding derived data to JointState messages as it implies that the robot has sensors / capabilities that it really does not. This makes me hesitant to merge something like this.

[hartmanndennis]

There is of course some noise but I assume velocities directly from robots which have that capability would have some noise as well. It was usefull for debugging for me.
I'll see if I can add some example plots next week.

[BrettHemes-3M]

You could accomplish the same with a separate debugging node outside of the hardware interface and then you could choose the derivative approximation that best suits your needs/application. I agree with @gavanderhoorn that this could easily cause confusion if published directly.

[ivareri]

To add my two cents:
As a relatively new user, I would expect any published data to come from the KRC. If merged, I think the readme\documentation should be updated to clarify that velocities are not from the KRC, and how they are calculated.

[gavanderhoorn]

I agree.

We could make this configurable by the user, with a default off.

[BrettHemes-3M]

The actual joint velocities are available within the KRC via a combination of $vel_axis_act (current velocity as percentage of maximum velocity) and $vel_axis_ma (controller-configured maximum allowed joint velocity). I would suggest us investigating returning these measured values from the controller itself.

[gavanderhoorn]

It would indeed make sense to use values calculated/derived by the controller if possible. That would be preferable over dx/dt on the ROS side.

[gavanderhoorn]

@hartmanndennis: would you be interested in making the changes to read the joint velocity from the RSI datastream?

[hartmanndennis]

These are KRL variables and not RSI elements like DEF_RIst. I don't know if it's easily or at all possible to send KRL variables with RSI while doing motion with RSI_MOVECORR. But I'm not an RSI expert.

[gavanderhoorn]

I seem to remember joint velocities being available through the RSI function blocks as well. But I'm not sure, would have to check the manual again.

[gavanderhoorn]

Hm, no: POSACT, AXISACT and GEARTORQUE. No velocities.

Thought: what about making RSI do the dx/dt for us? There is a D object.

[BrettHemes-3M]

The differentiation object should work here

[hartmanndennis]

dx/dt velocities with current indigo-devel:

dx/dt velocities with fixed duration from robot controller:

[gavanderhoorn]

Is this with the D RSI block, or calculated on the ROS side?

[hartmanndennis]

Sorry for being unclear, this is using this PR, so calculated on ROS side by simple dx/dt.

[gavanderhoorn]

Ok.

would you be up for trying the D component? I'm rather curious to see whether there would be any difference.

[hartmanndennis]

I'm not currently interested to dive into RSI stuff, which seems to be not that trivial.

[gavanderhoorn]

Ok, fair enough.

It would be good if we could use IPOC_(t_n+1) - IPOC_(t_n) for these dx/dts as well. So we can avoid having to configure that as a parameter.

We'd just have to be careful with IPOC overflows.

[gavanderhoorn]

Btw: I believe what you're seeing in those plots is exactly why using a real-time OS with deterministic scheduling is so important for RSI (and a host of other tasks).

[Ghassan-Nafash]

Hello @hartmanndennis and @gavanderhoorn. I am reading this chat of the Pull Request. I want to ask if it is possible to appy velocity control commands with KR C4, KSS v8.3.43 and RSI v3.2.0. I am controlling this robot and i am asking what are the possible commands that the RSI can receive? Velocity Control, position Control, Cartesian velocity Control?

Thanks in Advance