Realtime Interface (Joint and Cartesian)

[ted-miller]

This is just a draft. Both @gavanderhoorn and @jimmy-mcelwain have made some feedback already. I'd like to have a centralized place for that feedback.

This PR adds a real-time control interface, intended to be used in a closed loop system. The idea is to minimize as much communication latency as possible. This will take the user data and pump it directly into the motion API with only the bare minimal processing.

There are currently two motion modes, which require a planned trajectory. This adds a new motion mode, which is activated by the StartRtMode service.
(Note: I will be getting rid of this argument in the service.)

Once that is active, the client must open a UDP connection to port 8889 (*configurable). Once that is open, the client must send the first command packet, with an ID of 0. Then it should wait for a reply from the robot, which contains the current feedback position of the robot. (To be expanded with additional status info...) Upon receiving that feedback packet, the client must immediately send the next command packet.

Please note that each command packet is an incremental motion relative to the current position. The return value of the StartRtMode service call will indicate the time period for each increment. (Default 4 ms for a single robot arm.)

For this interface, the StartRtMode service invocation will indicate whether the increments will be in joint space (radians) or cartesian (currently mm and deg; this will probably change).

There is a test app which uses a USB joystick to move the arm in real time. This one is joint space. This is cartesian.

All testing has been done with Jazzy and YRC1000.

[jimmy-mcelwain]

I keep on getting the message Micro-ROS PC Agent Disconnected ~5 seconds after Starting UserLan link state monitor (port: 1). But then it automatically reconnects, and then it disconnects again 5 seconds later. This is after a lot of messing around with combinations of /start_traj_mode, /start_rt_mode, and stop_traj_mode. I may have gotten into some sort of weird state. I've attached a large debug log here, it was happening near the end (ignore the reset alarm messages, those are unrelated). Since it happens almost exactly 5 seconds after the link state monitor begins, it seems like it's just failing to ping my PC. I don't know why, but I'll look into it more.

I'm also seeing a memory leak. Probably related, but maybe not.

20250829T130735_motoros2_debug_log.txt

[jimmy-mcelwain]

I'm not sure what the best thing to do here is, but I have some thoughts on the reply/feedback message:

Right now we are using the robot feedback position (Ros_CtrlGroup_GetFBPulsePos) to populate the feedback message. This makes sense, but the feedback lags behind the command position pretty significantly (hundreds of milliseconds). This will make it difficult for the client application to adjust its trajectory in real time. So, I think that may be worthwhile to also include the command position in the reply message.

The problem with including the command position, though, is that if you submit an increment and then within the same interpolation cycle read the command position (which is what we are doing with the reply), it does not change. You don't know the updated command position until the next interpolation cycle. You could assume that the updated command position will be the reported command position + the increment that you just submitted, but of course the FSU can mess with your motion so that is not a safe assumption.

The client application could compare the "expected" command position (command position at T1 + new increment submitted at T1) with the newly reported command position (command position at T2) to determine if the FSU has modified the motion and react accordingly. Or to make it more obvious, we could include another field that is either:

1. A boolean indicating that the previously increment was modified by the FSU
2. Another 64 element array showing the amount that was truncated from the previously submitted increment.

This last element is not strictly necessary since a client app could track that/figure it out. But I would want to make it so client app programmers can't easily overlook it

[ted-miller]

@jimmy-mcelwain

I'm also seeing a memory leak.

I'm not explicitly allocating any new memory on the heap. And AFAICT, I'm de-init'ing everything properly.

I have reason to suspect that my libmicroros is outdated. For your testing, did you get that library from me? Or did you build yourself?

[ted-miller]

Are we OK with mandating little-endianness?

[gavanderhoorn]

Are we OK with mandating little-endianness?

it would be against best practices for network protocols (I'm old-fashioned when it comes to network protocols), but I guess with 'everything' being and/or defaulting to little-endian, it'd probably be OK. Would also make (de)serialisation easier, as clients could just use std::memcpy(..) from internal variables for fields/packets.

[ted-miller]

Note to self:
The reason that the FSU detection isn't working properly is the order of operations. The command position needs to be checked before calling mpExRcsIncrementMove.

So now the question is whether I should read feedback position before or after...

[jimmy-mcelwain]

I'm also seeing a memory leak.

I'm not explicitly allocating any new memory on the heap. And AFAICT, I'm de-init'ing everything properly.

I have reason to suspect that my libmicroros is outdated. For your testing, did you get that library from me? Or did you build yourself?

Whoops I missed this. I got the libmicroros from you. That's probably the problem.

[ted-miller]

@jimmy-mcelwain and @gavanderhoorn
I believe this is now ready for proper testing/review. I know I still need to document the interface and protocol.

@EricMarcil
Could you look at this section? I took it from your FSU-speed-limit logic.
https://github.com/ted-miller/motoros2/blob/00e8f80133e9f76e8fa7e1b441ed061144ff1a48/src/RealTimeMotionControl.c#L431

[ted-miller]

I have been testing with Jazzy on YRC1000, using this client app.
https://github.com/ted-miller/rt_control_tester/tree/cartesian

It just reads a USB joystick.

[ted-miller]

Should more of the robot status information be included with the feedback position? My assumption is that a user will maintain a separate subscription to robot_Status

[ted-miller]

What if the user ignores my sequence timing and sends multiple commands in a single cycle. The OS will buffer the data and feed it to me on the next cycle.

Should I purge the buffer after each cycle before sending the reply packet?

[jimmy-mcelwain]

I think that it is a little bit strange that timeout_for_rt_msg and max_sequence_diff_for_rt_msg need to be known separately by both the controller and the PC. I think that these values should either be set via the /start_rt_mode service call instead of the config file, or still set int the config file but returned to the client upon calling /start_rt_mode. This way they would only need to be changed in one place.

[jimmy-mcelwain]

How is the client application supposed to know if the RT session has ended? It looks like the controller exits the increment loop and doesn't send anything to the client no matter what the problem is. A client-side timeout in case of e-stop, timeout, missed packets, etc and tracking /robot_status for errors/alarms may be sufficient. But maybe there could be an "end session" packet. It's probably okay as-is but I just want to check.

[ted-miller]

How is the client application supposed to know if the RT session has ended? It looks like the controller exits the increment loop and doesn't send anything to the client no matter what the problem is. A client-side timeout in case of e-stop, timeout, missed packets, etc and tracking /robot_status for errors/alarms may be sufficient. But maybe there could be an "end session" packet. It's probably okay as-is but I just want to check.

I think that if we had an end-session packet, that we would want to include some reason code. But IMO, that information could just as easily be obtained from robot_status. So, I'm inclined to just let the client time out.

I have added some text to the docs/comments that the client should use the same timeout as the server.

[jimmy-mcelwain]

Right now, the controller doesn't "do" anything if it misses some packets, but not enough to drop the connection. Like if 8 packets are dropped but it takes 10 to disconnect, but it doesn't notify the client or log anything. Now the client may notice that the controller is not responding or once it does respond it may notice the discrepancy in expected command vs actual command position. So I think that is probably acceptable, but just wanted to mention that we don't "do" anything like we do if the FSU is modifying the command position.

[jimmy-mcelwain]

After a near-crash while testing this, I think that it would be good to allow the user to configure optional speed limits in the config file. I confirmed that the problem that resulted in the near-crash wasn't code related on either the controller or client side, it was just the joystick that I was using failed. For now I'm going to lower the speed limit on my client app.

I'm also okay with not adding any speed limit config option since:

• users can set up FSU rules
• it is not clear how to implement a speed limit with both cartesian and joint on the wide range of arms that we have. E.g. would we specify absolute speed for both? per-axis/joint? Or would we say reduce speed by 50% for all axes.

Just a thought

As far as reliability, it seems quite good. It very rarely (something like 1/20000) I think "misses" an interpolation cycle that it may perform one cycle late. That could possibly be improved, but it is infrequent and small enough that I don't think it matters too much, especially considering users are intended to monitor feedback.

Also, do we want to "force" users to read the feedback messages? Technically they could just have a 4ms (or whatever the interpolation period is) timer on their client PC and fire packets without reading feedback. That's obviously a bad idea, but do we want to allow it?

[ted-miller]

speed limit

Since this can be configured on the client app and also through the FSU, I don't think it's necessary.

reliability

I think that's due to ubuntu and not the robot controller.

Also, do we want to "force" users to read the feedback messages? Technically they could just have a 4ms (or whatever the interpolation period is) timer on their client PC and fire packets without reading feedback.

The latest commit ensures that the two systems stay in sync. If the client attempts to send anything before the robot's "trigger", then the data gets discarded.

[jimmy-mcelwain]

A couple more notes:

There is a bug where the rotational portions of the cartesian feedback messages are being truncated to the nearest degree. This isn't very obvious because it is converted to radians before being sent to the client, but if you convert it to degrees it is easy to see. So the cartesian feedback messages can be off by up to 1 degree.

If I end a session with an e-stop and then I start a new session, then I get the "you are being slowed down" message as a response to the first packet. It seems like it "remembers" from the previous session that it failed to complete a movement (because of the e-stop). I think something isn't being cleared properly between sessions.

I am able to control the robot for several seconds past when the agent disconnects. I can kill the micro-ros agent on my PC and still control via a joystick until a few seconds later the cleanup happens. This is because we chose to use our own UDP message rather than ROS2. This means you can't call /stop_traj_mode or monitor the /robot_status topic during these few seconds. I don't know if we want to do something to avoid this behavior.

Unlike other services, you must do /stop_traj_mode and then /start_rt_mode to restart again. I don't have a problem with this, but with say trajectory mode, I can just call /start_traj_mode after an estop and it works. But with the RT interfaces, if I e-stop, then I I need to call /stop_traj_mode and then /start_traj_mode to start it up again.

[jimmy-mcelwain]

going to maintenance mode, the ROBOT --> CURRENT POSITION then use the PAGE key to switch to the S1 group

Just a quick note, I'm guessing that @EricMarcil intended to say management mode. Not 100% sure.

I would also like to say @cadkin that I'm guessing your problem is caused by a bug in this PR that I just left another review for.

I do not have access to any external axes at my setup right now. I could try to provide a new build for you if you would like, @cadkin

[cadkin]

@cadkin I've looked closer at the parameters and compared the conversion to pulses with our simulator. The parameters normally used are matching the conversion in our simulator. So, the controller settings are correct.

Would you be able to get me another set of values like the ones above, but also incude the controller pulse positions. You can display the controller pulse positions on the pendant by going to management mode, then ROBOT --> CURRENT POSITION then use the PAGE key to switch to the S1 group. I also want the feedback pulses under ROBOT --> SERVO MONITOR (You need to be in management security to access that menu). You can also check the ROBOT --> COMMAND POSITION to see if you have values displaying (I'm not sure if motion through MotoRos populates those or not)

That should help us understand if the issue is on the command side or the current position side.

Sorry for the delayed reply, but here's what I collected for this:

RtReply packet

RtReply: {
  seq: 0
  fsu_interference: false
  target:
    - group 0
    -- current: s -0.9837605863538408
    -- current: l -0.5938710746500018
    -- current: u -0.7816552502750073
    -- current: r -1.002710608855453
    -- current: b -1.0126313942343401
    -- current: t 4.211933166636486
    -- current: e 0
    -- current: 8 0
    -- commanded: s -0.9837605863538408
    -- commanded: l -0.5938710746500018
    -- commanded: u -0.7816675221213104
    -- commanded: r -1.0027286027649882
    -- commanded: b -1.0126313942343401
    -- commanded: t 4.211971546257847
    -- commanded: e 0
    -- commanded: 8 0
    - group 1
    -- current: s 2.6253949914913175e-05
    -- current: l -0.2804117625629485
    -- current: u 0
    -- current: r 0
    -- current: b 0
    -- current: t 0
    -- current: e 0
    -- current: 8 0
    -- commanded: s -0.7080165213053785
    -- commanded: l -0.47819689476190613
    -- commanded: u 0
    -- commanded: r 0
    -- commanded: b 0
    -- commanded: t 0
    -- commanded: e 0
    -- commanded: 8 0
}

Pulse Values GP12

Pulse Values D500F

Servo Monitor

Let me know if you need any extra info from our pendant. Thanks again for the help!

going to maintenance mode, the ROBOT --> CURRENT POSITION then use the PAGE key to switch to the S1 group

Just a quick note, I'm guessing that @EricMarcil intended to say management mode. Not 100% sure.

I would also like to say @cadkin that I'm guessing your problem is caused by a bug in this PR that I just left another review for.

I do not have access to any external axes at my setup right now. I could try to provide a new build for you if you would like, @cadkin

If you could provide another build that would be wonderful. I've worked around it for the time being (deadlines are looming), but I definitely want to circle back around to this.

On a separate note, we've also been getting intermittent issues that require us to restart the entire controller:

Pendant Disconnection Error

This seems to happen somewhat infrequently, but I've noticed that when it does it's usually when we first start the micro-ros bridge and only when using the preview build.

I'd be happy to try a new build and see if some of the tweaks have caused this issue to clear up.

[EricMarcil]

The values supplied confirm that the reported current position is correct.

So, the issue would with the command position of the 2nd group. The values don't even make sense. It doesn't look like ratio error. I don't see how you could go from an almost zero value (2.6253949914913175e-05) to -0.7080165213053785. My guess is that the commanded values are garbage (not initialized or overwritten by some other data).

You say that this is at initialization, so you haven't sent any motion yet?

[cadkin]

Yes, that is correct - this is the very first packet I get back after initializing the UDP connection. I should note that if I just ignore the commanded value, this offset doesn't seem to go away. E.g. here's the start packet vs a random packet from later in the lifecycle:

Packet Seq 0

RtReply: {
  seq: 0
  fsu_interference: false
  target:
    - group 0
    -- current: s -0.9748962351789675
    -- current: l -0.6016828718123094
    -- current: u -0.7087604832346817
    -- current: r -1.0200027559188911
    -- current: b -0.9091657597171571
    -- current: t 4.265511118056377
    -- current: e 0
    -- current: 8 0
    -- commanded: s -0.9748962351789675
    -- commanded: l -0.6016828718123094
    -- commanded: u -0.7087604832346817
    -- commanded: r -1.0200207498284264
    -- commanded: b -0.9091657597171571
    -- commanded: t 4.265511118056377
    -- commanded: e 0
    -- commanded: 8 0
    - group 1
    -- current: s 0.04084239475096659
    -- current: l -0.0594544268976211
    -- current: u 0
    -- current: r 0
    -- current: b 0
    -- current: t 0
    -- current: e 0
    -- current: 8 0
    -- commanded: s -0.7016368114760546
    -- commanded: l -0.48448711044167625
    -- commanded: u 0
    -- commanded: r 0
    -- commanded: b 0
    -- commanded: t 0
    -- commanded: e 0
    -- commanded: 8 0
}

Packet Seq 1405

read: RtReply: {
  seq: 1405
  fsu_interference: false
  target:
    - group 0
    -- current: s -0.9748962351789675
    -- current: l -0.6016828718123094
    -- current: u -0.7087604832346817
    -- current: r -1.0200207498284264
    -- current: b -0.9091657597171571
    -- current: t 4.265511118056377
    -- current: e 0
    -- current: 8 0
    -- commanded: s -0.9748962351789675
    -- commanded: l -0.6016828718123094
    -- commanded: u -0.7087604832346817
    -- commanded: r -1.0200207498284264
    -- commanded: b -0.9091657597171571
    -- commanded: t 4.265511118056377
    -- commanded: e 0
    -- commanded: 8 0
    - group 1
    -- current: s 0.04084239475096659
    -- current: l -0.0594544268976211
    -- current: u 0
    -- current: r 0
    -- current: b 0
    -- current: t 0
    -- current: e 0
    -- current: 8 0
    -- commanded: s -0.7016368114760546
    -- commanded: l -0.48448711044167625
    -- commanded: u 0
    -- commanded: r 0
    -- commanded: b 0
    -- commanded: t 0
    -- commanded: e 0
    -- commanded: 8 0
}

If I then rotate our positioner:

Packet Seq 5615

read: RtReply: {
  seq: 5615
  fsu_interference: false
  target:
    - group 0
    -- current: s -0.9748962351789675
    -- current: l -0.6016828718123094
    -- current: u -0.7087604832346817
    -- current: r -1.0200207498284264
    -- current: b -0.9091657597171571
    -- current: t 4.265511118056377
    -- current: e 0
    -- current: 8 0
    -- commanded: s -0.9748962351789675
    -- commanded: l -0.6016828718123094
    -- commanded: u -0.7087604832346817
    -- commanded: r -1.0200207498284264
    -- commanded: b -0.9091657597171571
    -- commanded: t 4.265511118056377
    -- commanded: e 0
    -- commanded: 8 0
    - group 1
    -- current: s 0.04084239475096659
    -- current: l -1.0574371338368207
    -- current: u 0
    -- current: r 0
    -- current: b 0
    -- current: t 0
    -- current: e 0
    -- current: 8 0
    -- commanded: s -0.7016368114760546
    -- commanded: l -0.48448711044167625
    -- commanded: u 0
    -- commanded: r 0
    -- commanded: b 0
    -- commanded: t 0
    -- commanded: e 0
    -- commanded: 8 0
}

It seems like the commanded value just never changes.

[jimmy-mcelwain]

I have a lot more comments to make, but just as a start here are a few things.

[jimmy-mcelwain]

This is not sufficient, because it only really works on the first connection. After the agent disconnects, msgRobotStatus is destroyed, but this is already in the while (TRUE) loop below and continues referencing the freed memory even through connect/disconnect cycles.

[jimmy-mcelwain]

I generally do not like the way that this Ros_RtMotionControl_SendRobotStatus is handled. It is always alive, sending messages once every 10 ms even if there is no client connected. The fact that it is reading from msgRobotStatus means that it only gives useful information when connected to the micro-ros agent and the ControllerStatusIO code is running. I mentioned this elsewhere, but I still feel that it would be better if this was a bitfield included in the main feedback message.

[jimmy-mcelwain]

msgRobotStatus should probably be set to NULL upon being destroyed, but that only solves one part of the problem.