Physical robot movement stutters a lot.

[MrVolans]

Running a robot movement using moveit commander over a direct ethernet connection produces highly jittery movement at any speed. Its naturally less perceptible at the stock speeds that moveit configured it to run at, but since I actually intend it to run an industrial application over ROS Just need it to run at normal operation speeds. Uncapping the joints_limits.yaml gave me the speed increase, but it revealed just how rough the robot runs.

Edit: The speculations below are likely totally unrelated and not the cause:

I suspect its not an actual issue with the driver nor the connection, but the fact that I am also running a simulator in the background which is polluting the joint_state feedback topic causing the trajectory execution to behave sporadically.
See the image below. I have two nodes publishing to the state topic:

This is due to this issue: #88
Since the documentation is rudimentary at best I can't say i haven't missed something, but if anybody has any idea what to do about it, please let me know.

[anion0278]

@MrVolans Hello, could you please provide an update on what you were able to discover? We are seeing a similar shuttering in our case, and just like you, we are using the aubo_robot_simulator node, which could potentially pollute topics. Have you found a way to avoid using the simulator node or reduce its impact (e.g. by using topic remapping, etc.)?

[MrVolans]

I forgot to send the response, sorry for the wait. Yes I tested out remapping the simulator to a dummy topic, this did nothing as far as i can see. Looking at the source code I figured the simulator is needed to dissect the trajectory into waypoints that the driver then executes. I did not manage to bypass the simulator. I found the stuttering might be related to Real Time performance of the device running MoveIt, and so tried compiling an RT kernel for my system. (For reference, could you provide some specs of the pc you're using?) This appears to provide minimal improvements, but compared to the desired smooth movement, this is a marginal difference and probably not the root cause. I suspect the control pc might need the same treatment, but without deeper insight into the routing on the control pc side I really can't say I am sure that this is the solution. This was part of my b.eng. thesis, so please do report any findings back, I would be very interested in some insight into a possible solution.

[anion0278]

@MrVolans thank you for providing more information. I tried to investigate the potential topic pollution by recording the sources of the messages from the following topics: /joint_states, /feedback_states and /robot_status, since I suspected it of receiving messages from both the aubo_robot_simulator node and aubo_driver . Just in case someone needs to do the same, the recorder script gist is here.
Nevertheless, the obtained recordings did not show any polluting messages, which is also supported by your attempt to remap the topics.

My next best suggestion was about using Real-Time kernel, that's why I am glad that you shared your experience with that. Right now I have no access to the PC utilized in the testing scenario, but I will try to provide the specs later.

At the same time, I am not 100% sure if I understood your steps: you've compiled RT kernel for your system, but what did you mean by "control PC" ? Do you mean you have a distributed ROS graph, where a low-end unit (Jetson Nano?) is running the driver node and high-end "control PC" is running MoveIt?

[MrVolans]

By Control PC I am referring to the linux machine in the robot control box, which I checked the kernel on and its just a generic 14.04 kernel, I apologize for the confusion.
No I don't run a distributed ROS system, its all running on an HP desktop pc, which I compiled a 20.04 RT kernel for where I got it down to about 100us max latency running cyclictest under headless ubuntu. I did consider isolating the move_group and/or aubo_driver node to another device to decrease the impact of other processes taking up resources. I didn't get the chance to though.

[anion0278]

Could you, please, also provide information on which ROS distro and which MoveIt version you are using? Have you also applied the "MoveIt update" from the UpdateMoveitLib dir (not sure what it does though)?

[MrVolans]

I am running MoveIt:
`Package: ros-noetic-moveit

Version: 1.1.13-2focal.20230801.171807

Priority: optional

Section: misc

Maintainer: Dave Coleman dave@picknik.ai

Installed-Size: 22.5 kB

Depends: ros-noetic-moveit-commander, ros-noetic-moveit-core, ros-noetic-moveit-planners, ros-noetic-moveit-plugins, ros-noetic-moveit-ros, ros-noetic-moveit-setup-assistant`

ROS:
`Package: ros-noetic-desktop

Version: 1.5.0-1focal.20230620.211638

Priority: optional

Section: misc

Maintainer: Mikael Arguedas mikael@osrfoundation.org

Installed-Size: 14.3 kB

Depends: ros-noetic-angles, ros-noetic-common-tutorials, ros-noetic-geometry-tutorials, ros-noetic-joint-state-publisher-gui, ros-noetic-robot, ros-noetic-ros-tutorials, ros-noetic-roslint, ros-noetic-urdf-tutorial, ros-noetic-visualization-tutorials, ros-noetic-viz
`
I tried installing the moveit update lib and it broke my install of moveit. The updatelib is also labeled for Kinetic, so I am not using it atm.

[ian-chuang]

I am also having the same issue with just the aubo_driver and aubo_controller. The robot will stutter even at normal looking joint speeds.

I thought the problem had to do with the rib_buffer_size_, which is used in aubo_driver.cpp. It seems like the robot stores a buffer of the joint commands sent through the command robotServiceSetRobotPosData2Canbus. I thought the stuttering had to do with the rib_buffer_size_ dropping to 0 while the robot was running a trajectory. However even after disabling the publishIOMsg thread and reducing the rate of the timerCallback, which seems to help keep the buffer size above 0, I still get stuttering though not as bad.

I have been in contact with some people at Aubo about developing a ServoJ feature similar to that on the Universal Robot. They actually sent a demo ServoJ function that they were already working on but it suffers from the same issue where the TCP/Canbus connection is unreliable and can cause stuttering. I have been trying to work with them for months since around January, but they aren't very responsive and I still haven't been able to get into direct contact with one of their software engineers.

I proposed a solution to them about implementing a version of ServoJ akin to that on the Universal Robot ROS Driver, where you essentially run servoj in a loop locally on the robot and then remotely update the target joint positions for it. This ensures that even with an inconsistent network that servoj can run continuously. See these links here for how it works:
https://forum.universal-robots.com/t/realtime-control/9495/4
https://backend.orbit.dtu.dk/ws/portalfiles/portal/117833332/Universal_Robot_report.pdf

However, they haven't responded to me about this at all.

The reason we need a reliable ServoJ function is because the ROS driver only supports joint trajectory control and it still stutters. With servoj, it can allow us to integrate aubo robot with ros_control, ros hardware interface, and all of the already-made ros controllers (joint trajectory control, force control, compliance control, twist control)

I apologize for going on this tangent but I've been having a really hard time working with Aubo and wanted to share it with others about what I've found so far.

[MrVolans]

@ian-chuang thanks for sharing. I agree with your points and share the same experience with AUBO communication. We sell their robots where I work, but still have a hard time getting response on emails.

[Joinjiang]

RT kernel was not an effective way for me in my case, and aubo_robot_simulator node will not pollute any topic actually, which not pub any msg on joint state topic and you can comment it. I have connect aubo many times, but no effective response to me

[anion0278]

I will provide additional information regarding my (unsuccessful) attempts to identify the root cause of stuttering.

TL;DR;

The most accurate assumption at this point is that the problem lies within the waypoint transformation calculation in the aubo_robot_simulator Python script.

Detailed description of experiments:

I have experimented with the following approaches:

• Using a real-time kernel and configuring real-time priority and niceness for AUBO/AUBO+MoveIt nodes. The result remained the same: stuttering persisted.
• Adjusting values in aubo_driver.cpp and aubo_driver.h by varying in orders of magnitude. Additionally, I explored modifying the number of threads in driver_node.cpp's AsyncSpinner. While not the most scientific approach, given the multitude of constants, it was the most straightforward option. Result: stuttering persisted in any configuration.
• Introducing a delay in setRobotJointsByMoveIt() before robotServiceSetRobotPosData2Canbus() to address the assumption that the robot controller might be overwhelmed with a large number of move requests, potentially skipping some waypoints. Result: The execution of movements slowed down (confirming the effectiveness of the delay), but the abrupt robot moves persisted in the same locations.

Despite these tests, it remained unclear whether the issue might be concealed in how the AUBO driver processes the trajectory within its intricate pipelines. Here, I'll briefly describe my understanding of the internal organization of the driver and the relationships between nodes in the AUBO Driver:

• aubo_i3_controller/follow_joint_trajectory action recieving commands from MoveIt - commands are processed in joint_trajectory_action.cpp (referred to as aubo_joint_trajectory_action in the ROS-graph), which publishes the joint_path_command topic.
• joint_path_command is consumed in aubo_robot_simulator (Python script), where the trajectory undergoes transformation through specific calculations before being published to the moveItController_cmd topic.
• moveItController_cmd is consumed in aubo_driver.cpp, which sends the motion command to the robot controller.
• (aubo_robot_simulator.cpp is not used at all).

Somewhere in this processing (over-)complexity, there may be a point causing the transformed MoveIt-generated trajectory to become invalid, resulting in stuttering (potentially due to race conditions or invalid computed accelerations/velocities).

To isolate the influence of MoveIt path planning (which inherently involves randomness), I prepared a script script allowing the recording and replaying of the same Moveit-generated trajectory consistently.

Using this script, I ran and recorded multiple executions of the same trajectory (ROS bags, and videos of robot movement from the Teach Pendant), analyzing joint variables, velocity, and acceleration data from the joint_path_command and moveItController_cmd topics. I then compared this data with the "ethalon" Moveit-generated trajectory.
The attached Excel document illustrates that the aubo_robot_simulator transforms the same trajectory differently each time (refer to moveItController_cmd charts, apologies for the file's less-than-optimal organization and preparation). Moreover, recorded videos show that, although the stuttering takes place in more or less the same locations along the trajectory, it does not occur every time.

The graph also highlights another "issue" - the disparity between the data published by MoveIt and aubo_joint_trajectory_action: when transforming the trajectory from MoveIt inside aubo_joint_trajectory_action, the data for axes 1, 2, and 3 are swapped (yes, the colors of the legend are correct, it is the issue of the data itself). However, from the moveItController_cmd graph, it is evident that the Python script of aubo_robot_simulator accounts for the swapped axis order and corrects them, thus it is not a big issue.

Apologies for the lengthy explanation, but I hope it proves helpful to someone on the "quest for the truth".

If you require any data from the experiments I conducted, please let me know.

[Hunger-beat]

@anion0278 Thank you for your work.I also found that the joint_path_command topic they posted was wrong.The correct order of the first three joint angles is 1, 2, 3, but the published order is 3, 1, 2.This has bothered me for a long time.Thank you.

[Joinjiang]

@anion0278 Thank you for your work.I also found that the joint_path_command topic they posted was wrong.The correct order of the first three joint angles is 1, 2, 3, but the published order is 3, 1, 2.This has bothered me for a long time.Thank you.

would you mind point out where code is wrong about your said ?

[Hunger-beat]

@anion0278 Thank you for your work.I also found that the joint_path_command topic they posted was wrong.The correct order of the first three joint angles is 1, 2, 3, but the published order is 3, 1, 2.This has bothered me for a long time.Thank you.

would you mind point out where code is wrong about your said ?

Sorry,I didn't look at their code in detail.As @anion0278 said, this problem has been noticed in aubo_robot_simulator, so it does not affect the actual execution effect. If you are like me and just want to use the data published by the joint_path_command topic, just pay attention to changing the order.Thanks.

[anion0278]

@Joinjiang if you would like to find a precise location where this swap is occurring, try to investigate the joint order provided in joint_path_command topic (which provides mapping for aubo_robot_simulator and the one provided in corresponding controller definition, for example i5

Snippet from joint_path_command:

...
joint_names: 
  - foreArm_joint
  - shoulder_joint
  - upperArm_joint
  - wrist1_joint
  - wrist2_joint
  - wrist3_joint 
...

Snippet from controller definition:

    joints:
      - shoulder_joint
      - upperArm_joint
      - foreArm_joint
      - wrist1_joint
      - wrist2_joint
      - wrist3_joint

As you can see the order is swapped exactly as me and @Hunger-beat described.

[Joinjiang]

@Joinjiang if you would like to find a precise location where this swap is occurring, try to investigate the joint order provided in joint_path_command topic (which provides mapping for aubo_robot_simulator and the one provided in corresponding controller definition, for example i5

Snippet from joint_path_command:

...
joint_names: 
  - foreArm_joint
  - shoulder_joint
  - upperArm_joint
  - wrist1_joint
  - wrist2_joint
  - wrist3_joint 
...

Snippet from controller definition:

    joints:
      - shoulder_joint
      - upperArm_joint
      - foreArm_joint
      - wrist1_joint
      - wrist2_joint
      - wrist3_joint

As you can see the order is swapped exactly as me and @Hunger-beat described.

thank you for your kindly help! i have got it

[Joinjiang]

@Joinjiang if you would like to find a precise location where this swap is occurring, try to investigate the joint order provided in joint_path_command topic (which provides mapping for aubo_robot_simulator and the one provided in corresponding controller definition, for example i5
Snippet from joint_path_command:

...
joint_names: 
  - foreArm_joint
  - shoulder_joint
  - upperArm_joint
  - wrist1_joint
  - wrist2_joint
  - wrist3_joint 
...

Snippet from controller definition:

    joints:
      - shoulder_joint
      - upperArm_joint
      - foreArm_joint
      - wrist1_joint
      - wrist2_joint
      - wrist3_joint

As you can see the order is swapped exactly as me and @Hunger-beat described.

thank you for your kindly help! i have got it

to my investigation, it seems that the joint name order in actionclient goal message is not corresponding to the order in the controller.yaml, so it has to remap order and it is not aubo package error! if someone has another ideas about solving this problem, please let me konw.

[Joinjiang]

i have got the exact reasons: follow_joint_trajectory/goal organizes the joint data in the alphabetical form.

[anion0278]

@Joinjiang however I think the swapped order is not the reason for the observed shuttering (because if it was, then the robot would completely go haywire and neither segment of the trajectory would have been executed properly).

[Joinjiang]

@Joinjiang however I think the swapped order is not the reason for the observed shuttering (because if it was, then the robot would completely go haywire and neither segment of the trajectory would have been executed properly).

yes, may be use the velocity control better than position control in this package