Realtime on Moveit ROS1

[tianshiz]

Hi, I recently saw this article on piknik: https://picknik.ai/control/realtime/moveit/2020/05/18/jogarm-realtime-cartesian-motion-with-moveit.html I also saw the presentation about Moveit2's plans for Real Time on ROS2. Very interesting projects and looking forward to them.

However, in the interim, I am looking at potential replacements for my current commercial realtime planner and I was wondering if the new JogArm(now named moveit_servo) package would fit the bill, or if there are some practical limitations compared to other planners?

Some specs on my robot:

• melodic on 18.04 lowlatency kernel
• wheeled and multiple arms
• require RT for active compliance as the robot drives over rough terrain
• require RT for gripper usage on arm
• teleop control
• active obstacle avoidance, basically need to stop the wheels before we hit an obstacle
• already works with moveit
• need to run at 100Hz

The robot isn't going to moving that fast so I'm not looking for extreme realtime performance. But is the above achievable with the JogArm package? Or perhaps I need to wait for ROS2?

I was hoping to move to JogArm now on ROS1, and then get a smoother upgrade when it comes to ROS2. Is that a legit pathway?

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[tylerjw]

Pinging @AndyZe to see what he thinks of your challenges.

When you say "RT for active compliance" what do you mean? Is this gravity compensation or something similar?

[AndyZe]

Yep, moveit_servo is in a good state. It can easily handle 100 Hz.

For multiple arms, you'll have to launch multiple Servo nodes and give each one a namespace. See the parameter_ns in the launch files, e.g. here: https://github.com/ros-planning/moveit/blob/2ac4db94ffee73933deda961a71262d3a893acb1/moveit_ros/moveit_servo/launch/cpp_interface_example.launch#L5

You'll probably want to launch a ros_control joint_group_position_controller or joint_group_velocity controller for each arm.

This stuff has been done before, so I don't foresee any major road blocks for you.

[AndyZe]

In MoveIt2, Servo works well already. It actually has more unit tests and is probably more performant overall than the ROS1 version. So yeah, upgrading to MoveIt2 Servo sounds like a good idea at some point.

Edit: the ros2_control stuff isn't ready yet and I don't think any robot drivers are available. So don't use ROS2 yet.

[AndyZe]

I'm going to close the issue, reopen it or start a new issue if you have any questions.

[gavanderhoorn]

Doesn't the discussion hinge on what definition of real-time @tianshiz uses?

Servo is not deterministic, which would be the classic definition of real-time. Neither in ROS 1, nor ROS 2.

That's also impossible, as we typically don't use a real-time transport. But apart from that it hasn't been written with determinism in mind.

If real-time means fast enough here, then it current ROS+MoveIt+servo may be sufficient.

One case where determinism matters is with direct interfaces to servos: jitter on control signals can very quickly adversely affect the quality of motion. If you have some filtering or an intermediary controller between servo and the servos (hah), determinism is somewhat less crucial, but again, this would depend on what @tianshiz wants to do exactly and how his/her robots are setup and interfaced.

[tianshiz]

thanks for the input guys. gavanderhoorn is right, ultimately I'm analyzing the realtime capabilities of ROS2 + Moveit Servo on Ubuntu w/ Xenomai or RT_PREEMPT

From my understanding, Xenomai is considered hard realtime and RT_PREEMPT is soft. For my requirements, at minimum a soft realtime is necessary when moving the wheels/arms/gripper. But, it would be good to know what the limits are. Is a real hard real time system possible? Something that could loosely be considered space grade? For example SpaceX's falcon 9 uses a custom variant of RT Linux, so I see this route as possible and more economical and flexible than getting a dedicated RTOS.

So my question is, would moveit servo in ROS2 with a RTOS meet any determinism and jitter requirements? Maybe I made too broad of an assumption based off the realtime on ROS2 slide.

I understand that in ROS1, none of that can be accomplished, I can only get things to happen fast enough. But if ROS2 could meet my real time needs, I would at least rebuild my architecture around moveit servo and make it a smoother transition in ROS2.

[AndyZe]

You are right. We used to be able to claim it was realtime (as presented at ROSCon) but @tylerjw 's big reshuffling PR nuked that capability. Now, it just runs as fast as it can if it can't keep up with the desired rate.

I probably should have been more critical in reviewing that big PR.

[AndyZe]

Well, if hard realtime is really important, you could work from a commit prior to this one: https://github.com/ros-planning/moveit/pull/2103

Or open a PR to restore that functionality.

[tylerjw]

To be more specific, what I did was change the main processing from happening in threads to happening in ROS timers. I've since been reconsidering that decision. I did this mostly because it would play nicer with the rest of a ROS system in a more performant way. It has the drawback though that I gave up the ability to specify the threads that those parts of the system ran on. If we restored running on threads instead of timers you could set a RT scheduling policy for those threads.

One thing to note though is that in ROS1 messages pass through a TCP socket (both into from whatever you have sending commands, and out of to whatever you are controlling) and the network stack is not deterministic (the time it takes to deliver a message depends on the load on the system inside the same system and on the network if done between machines). I made it more performant by allowing you to send those messages without a copy (and if you use the pool allocation without an allocation most of the time). One big advantage of ROS2 is the messaging is done over DDS and you have much more control over how that works, you can make it much more deterministic that way (I believe soft RT in this case). Another thing to note is that servo depends on the joint position topics and that message contains a vector which will cause a dynamic memory allocation (malloc) each time it is received. To achieve RT you'd need a different malloc implementation that was RT.

[gavanderhoorn]

It has the drawback though that I gave up the ability to specify the threads that those parts of the system ran on. If we restored running on threads instead of timers you could set a RT scheduling policy for those threads.

Timers are events, handled by CallbackQueues.

CallbackQueues can be managed by your own spinners, which can be run in a thread of your choosing.

It's well possible that doing this in a thread with RT priority (under RT preempt) will make things a lot more deterministic.

For one thing, you wouldn't be handling timers and topics and services (etc) with the same callback queue.

[tylerjw]

Timers are events, handled by CallbackQueues.

CallbackQueues can be managed by your own spinners, which can be run in a thread of your choosing.

It's well possible that doing this in a thread with RT priority (under RT preempt) will make things a lot more deterministic.

For one thing, you wouldn't be handling timers and topics and services (etc) with the same callback queue.

That's awesome, I should try to get a demo of that working. That would be awesome to have an example for how to do that. I didn't realize you could create your own spinners.

[gavanderhoorn]

The roscpp/Overview/Callbacks and Spinning page has a good overview of some techniques.

Especially the later sections.

[AndyZe]

Probably mostly an academic exercise. We should find out what kind of control rate it can hit these days.

And Tyler - I think the performance gains were worth it.

[gavanderhoorn]

Probably mostly an academic exercise.

What is?

Reducing jitter would seem pretty advantageous.

Not every controller has the massive filtering URs have.

[tylerjw]

I think what @gavanderhoorn is saying that the way it now is could be made RT by creating our own spinners that are on RT threads.

In the end I think it is probably more valuable to show that we can hold a good control rate for common robots with servo very deterministically. I think that could even be really good for some of our current projects.

[gavanderhoorn]

could be made RT

Well .. it'll probably be possible to reduce jitter by handling your timer events separately.

It's similar to how many ros_control drivers work: start a thread with RT priority and let that thread call hw_iface.read(); ctrlr_mgr.update(); hw_iface.write();.

[tylerjw]

For performance reasons I also want to make servo a nodelet (the ros2 port of this is a composable node) and backport some of the other improvements Adam made in his port.

[tylerjw]

I'm going to reopen this because I want this conversation to stay around and be easy to find so I can refer to it until I have some time to work on trying some of the stuff we discussed here. @tianshiz, if you get a chance to try any of these ideas for your project please give us feedback.

[AndyZe]

btw I did some benchmarking today. It looks like the control loop period varied from about 4.95kHz to 10.1 kHz.

[AndyZe]

There are occasional spikes as slow as 98.6 Hz, from what I saw. I'm guessing that's due to the extra calculations near a singularity

[gavanderhoorn]

Sustaining a 10kHz rate on a real-time kernel is already non-trivial, let alone on a regular one.

---

Edit: non-trivial when you want to do anything more than some simple calculations, and with low jitter.

Involving any part of MoveIt is going to have massive impact on that as well.

[tianshiz]

Just wanted to check in on this, is the plan still for MoveIt2(on ROS2) to support realtime given a RT kernel? I just want to know if this is on the roadmap or being pushed back by the team.

[stevensu1838]

Hi @AndyZe ,

I'd love to make Admittance Control of UR5 with your jog_arm package. I am thinking of using a 6 Dof Force sensor to read the contact force at the end-effetor of the robot and then convert this force signal to the twist message that jog_arm uses to move the arm. You are the only robot expert I know. May I please ask if I am doing right?

I've seen moveit it tutorials of jog arm for ROS Melodic, ROS Noetic and ROS 2, can you please tell me which version of jog_arm(Realtime Arm Servoing) is the best one to use for controlling a UR5?

[AndyZe]

Hi @stevensu1838, how are you?

Personally I wouldn't use Melodic anymore. It's getting old. It's a hard choice between Noetic and ROS2. They all work, though.

Did you see the FZI package that does admittance control? It's probably a great option but I haven't used it myself yet. https://static1.squarespace.com/static/51df34b1e4b08840dcfd2841/t/5e202e87f0b63e06cfdf6064/1579167378467/04_Arne_Roennau_Cartesian_Controllers_ROS-I_V4.pdf

[Giggus]

HI @AndyZe ,

I just tried moveit_servo in ROS1 (Noetic) but it seems like it's not controlling a pose but rather a point and rotations about it. I.e. if I send a linear velocity cmd the current orientation of the end-effector is not preserved. Moveit_servo in ROS2 on the other end, does seem to control a pose instead. Am I right?

Would it be possible to use the moveit_servo package of ROS2 with ROS1 ?

In addition, do you know if moveit_servo package of ROS2 does move the robot in the null space of the main task so to avoid joint limits?

[ZhouJankin]

Hi @Giggus I've tried to use the moveit_servo in ROS1(melodic) by the tutorials in this page: https://ros-planning.github.io/moveit_tutorials/doc/realtime_servo/realtime_servo_tutorial.html but I can't find the moveit_servo pkg(roscd moveit_servo) and unable to publish topic to /servo_server/delta_twist_cmds would you like to give me some advise about how to use it or launch it in ROS1?

[Giggus]

HI @ZhouJankin If you can't find moveit_servo in your workspace you probably need to install ros-melodic-moveit-servo. Alternatively, you can clone the moveit repo (melodic branch) and copy paste in your workspace moveit_servo. With the latter you can modify the algorithm as well if you need to.

Hope it helps!

[ZhouJankin]

Hi there @Giggus The problem solved immediately after I run the install. I've searched this problem for a whole day and even ready to update ROS2. Really appreciate you and wish you a good day!