Fast way for executing trajectory

[AlirezaMsb]

Hi, We are working on Kinove Gen3 (6dof), in a project. Currently when we send a joint trajectory to the arm through the ROS controller, usually it takes around 1.5 to 2 seconds to execute the trajectory after the arm has received it, which the time goes to "preprocessing the trajectory",... I am wondering whether there is any way to skip this waiting time to speed up the execution time. (For example, would this problem be solved if we use the API?) For us, the most important feature is to keep the end effector in a certain plane while avoiding self-collision, and that's why we are sending a trajectory.

Also, we are pushing the arm to move as fast as possible. As a result, I get so many errors in the Kinova's pkg about the acceleration and speed but I keep modifying the trajectory until the arm can execute it. After a while, the Kinova's ROS pkg stops working (without giving out any error, we just realize that the arm is not moving) and we have to relaunch it again. Is there any way to solve this problem? E.g. Is there any service, that I can check the joint trajectory with that one to make sure that the acceleration and speed are acceptable before actually sending the execution command?

Alireza

[felixmaisonneuve]

Hi @AlirezaMsb,

Do you have a short code example to show me how you are using the joint trajectory with the ROS controller? I am not sure I completely understand what you mean by that. I don't know if you can skip this (because I don't really know where it is coming from), but I am sure we can figure out a way to work around it. Using the API might be a way to do it If you need the end-effector to stay on a plane, don't you want to use a cartesian Trajectory instead of a joint trajectory?

Do you have an example of a speed or acceleration error? It makes me think of a time parametrisation issue (like in #233 and #206), but this is only if you are using moveit. Maybe you have software limits set or something else. In any case, a example of the error message would be very helpful.

You say "after a while". How long is "after a while"? We are aware of some bugs after longer use time. For example the API (so your ROS stack), might randomly get disconnected after ~11h-15h.

I have some API calls that allow you to check if the trajectory is valid if you are using waypoints. But since I am not sure what you are doing here, I would really appreciate a code example of such trajectory (or whatever command you use) to better understand.

Best, Felix

[AlirezaMsb]

@felixmaisonneuve Hey Felix, I send the trajectories through MoveIt which eventually uses the follow_joint_trajectory controller that is available with Kinova driver (joint_trajectory_controller/follow_joint_trajectory/) . The waiting time is from the kortex_driver.launch file. Whenever I send a trajectory to the robot it goes through the following steps in the driver terminal:

So this procedure takes between 1 to 2s from what I saw, but since I want to execute trajectories one after another, I am looking for a way to omit this waiting time. I am happy to hear that we can have a work around to omit this waiting time! One reason that I am using Joint trajectory and MoveIt is because the possibility of self-collision in the robot that I am using (which two Kinova arms are part of it) is very high, and I need to check collision before any planning for the specific task that we have.

Do you have an example of a speed or acceleration error? It makes me think of a time parametrisation issue (like in https://github.com/Kinovarobotics/ros_kortex/issues/233 and https://github.com/Kinovarobotics/ros_kortex/issues/206), but this is only if you are using moveit. Maybe you have software limits set or something else. In any case, a example of the error message would be very helpful.

It is the same error as what other people have mentioned in the issues that you mentioned. I solved that problem by regenerating the trajectory when I get an error in the execution time and resending the new one, so eventually even if I get an error I still move the robot by modifying the trajectory as fast as possible considering a time limitation that I have. The only problem is here that I do that in the execution time, but I would like to be able to do it before commanding the robot, that would give me much more time to regenerate a good trajectory too.

You say "after a while". How long is "after a while"? We are aware of some bugs after longer use time. For example the API (so your ROS stack), might randomly get disconnected after ~11h-15h.

We do use the robots for a long time (usually between 6 to 12 hours). Today, we will be using the robot for about 12 hours, so I'll see when it starts disconnecting. As for the validation method and trajectory execution method any possible method would be very much appreciated!

[AlirezaMsb]

So yesterday we used the robot for like around 10 hours (we relaunched the pkg from time to time for our development purposes). My first impression was like what you mentioned that at the beginning the robot was executing the trajectories without that disconnection problem, but before I was going to finish my work, I saw that disconnection problem; I only observed the system, and cared about the timing part that you mentioned yesterday though. edit: Another update on the disconnection problem, today the Kinova started disconnecting after an hour of use, and kept disconnecting.

[felixmaisonneuve]

Ok, do you always do the same sequences? When you create a joint_trajectory using MoveIt, it creates a sequence with each actuators position, velocity and acceleration for every ms of execution. This is then sent to the arm and it has to analyze every thing to make sure the trajectory can be executed (or you get the errors you saw). This is what's taking so long. You could probably avoid it if you sent a cartesian point directly, but for that you need to know the final position of the arm. Another way would be to send a angular WaypointList choosing specific points from the MoveIt sequence. However, you need to know the trajectory you want to do before sending it for these two options

For the disconnection bug, we do not currently know why this happens, but this is definitely it. We have received feedback from other clients with a similar description as yours : happens after > 10h of use, then it happens after ~30minutes of use after a reboot. I don't know if this could be related to the temperature in the arm. I don't know when this will be fixed either.

[AlirezaMsb]

Thanks for your help @felixmaisonneuve ,

Ok, do you always do the same sequences?

I don't always do the same sequence, but I can add a step to validate each trajectory, and modify it before executing it in our system. I am currently doing that at the execution time. Is there any API command or ROS service available for checking the trajectory before sending it? By looking at the source code, I think the acceleration and speed validation is happening in the following line: https://github.com/Kinovarobotics/ros_kortex/blob/a46606e94f8437402cad91a7d7064f1c7f58ea91/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp#L160

However, you need to know the trajectory you want to do before sending it for these two options

I know the trajectories before sending to the robot, then I think the solutions that you mentioned are possible. One thing to confirm, is fast trajectory execution okay with the two options that you mentioned?

[felixmaisonneuve]

I looked deeper into this and realized the action server is already using Waypoints. I thought it was using PlayPreComputedJointTrajectory, but it was changed last year when we updated the repo for the Kortex2.3 API. You could maybe try to revert back to PreComputedJointTrajectory action server, but it might lead to other errors listed in the patch notes.

Looking at the code I also realized we sleep for 1s before sending the trajectory. The first thing you could do is remove the ClearFaults and the sleep commands for the action server https://github.com/Kinovarobotics/ros_kortex/blob/a46606e94f8437402cad91a7d7064f1c7f58ea91/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp#L199-L201

I think it would be a bad idea to keep the ClearFaults command if you remove the sleep one, so I would remove both, but you will have to clear them manually if something goes wrong.

I also found we put a random duration of 0.5s at the beginning of the trajectory : https://github.com/Kinovarobotics/ros_kortex/blob/a46606e94f8437402cad91a7d7064f1c7f58ea91/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp#L121-L127

I assume this causes the arm to wait for 0.5s at the beginning of the trajectory execution.
You could try to reduce this delay a bit. I am not sure at what value it starts to give errors, but I think 0.5s might be overkill a bit and it could be reduced.

If those two optimizations are not enough, you could try to adapt the logic to ignore some waypoints. https://github.com/Kinovarobotics/ros_kortex/blob/a46606e94f8437402cad91a7d7064f1c7f58ea91/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp#L110-L145

MoveIt produces a lot of trajectory points and some could probably be removed from the WaypointList. For example, keeping the first trajectory point, the last one and maybe 1-2 or 3 points in between. This could reduce the computation time of the ValidateWaypointList, but I do not know how much time it will cut. Computation does not usually take too long, so maybe a couple ms, but I doubt it will save more than 100ms. If you decide to optimize the waypoint selection, the first waypoint should not be required, so you could remove it entirely and only use the last trajectory point and some other random points if necessary. The challenge will probably be to tweek the waypoint_duration so you keep a fast execution time but you do not get errors. This could help completely remove the 0.5s delay at the beginning of the trajectory.

[AlirezaMsb]

@felixmaisonneuve Thanks for your help Felix. I tried the modifications that you mentioned, and the system became faster. I think we can close this issue now.
Alireza