short time plan leads joint_trajectory_controller error

[fukui-masashi-k]

Arm: kinova gen3 6dof
Firmware and ros_kortex is latest one.

I am trying to move an arm by using trajectory generation with moveit.

When executing trajectory which command value is very close to the current value(for example, within the tolerance), the kinova controller sometimes executes the trajectory and sometimes does not.

When I checked log, I found that the internal state transitions between ACTION_START and ACTION_PREPROCESS_END were different.

Therefore, I would like to have the following three questions answered.

①What is happening internally?
②I recognize this state as a bug in the following line.
when is this bug fixed?

ros_kortex/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp

Line 282 in bf71d8e

// FIXME KOR-3563 Sometimes the notifications arrive in the wrong order so it is possible to receive

③If an action command is sent in the TIMED_OUT state, it often does not recover. Is there a way to recover from TIMED_OUT? The '/gen3/in/clear_faults' topic has been sent, but there is no prospect of recovery.

"""log"""
(Callback log is added by me to

ros_kortex/kortex_driver/src/non-generated/driver/joint_trajectory_action_server.cpp

Line 253 in bf71d8e

)

Successful case
[ INFO] [1742258345.340065073]: New goal received.
[ INFO] [1742258345.340369340]: Joint Trajectory Goal is accepted.
[ INFO] [1742258345.340605349]: use_hard_limit.
[ INFO] [1742258345.397757300]: Callback.
[ INFO] [1742258345.420815733]: Callback.
[ INFO] [1742258345.961519769]: send trajectory.
[ INFO] [1742258345.989850200]: send trajectory end.
[ INFO] [1742258345.989949052]: State changed from IDLE to PRE_PROCESSING_PENDING

[ INFO] [1742258346.067411088]: Callback.
[ INFO] [1742258346.067498227]: ACTION_PREPROCESS_END
[ INFO] [1742258346.067518324]: Preprocessing has finished in the arm and goal has been accepted.
[ INFO] [1742258346.067550965]: State changed from PRE_PROCESSING_PENDING to TRAJECTORY_EXECUTION_PENDING

[ INFO] [1742258346.088605136]: Callback.
[ INFO] [1742258346.088692115]: ACTION_START
[ INFO] [1742258346.088718292]: Trajectory has started.
[ INFO] [1742258346.088753109]: State changed from TRAJECTORY_EXECUTION_PENDING to TRAJECTORY_EXECUTION_IN_PROGRESS

[INFO] [1742258346.588620849]: Callback.
[ INFO] [1742258346.763825788]: Callback.
[ INFO] [1742258346.913941978]: Callback.
[ INFO] [1742258346.942065621]: Callback.
[ INFO] [1742258346.942144472]: Trajectory has finished in the arm.
[ INFO] [1742258347.442316146]: Trajectory execution succeeded.
[ INFO] [1742258347.442730081]: Controller 'gen3_joint_trajectory_controller' successfully finished
[INFO] [1742258347.494688769]: State changed from TRAJECTORY_EXECUTION_IN_PROGRESS to IDLE

[ INFO] [1742258347.494824550]: Callback.
[ INFO] [1742258347.496481027]: Callback.
[ INFO] [1742258347.514904480]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1742258347.515083943]: Execution completed: SUCCEEDED

Failure case
[ INFO] [1742258257.240966116]: New goal received.
[INFO] [1742258257.241197834]: Joint Trajectory Goal is accepted.
[ INFO] [1742258257.241375118]: use_hard_limit.
[ INFO] [1742258257.289921744]: Callback.
[ INFO] [1742258257.311848953]: Callback.
[ INFO] [1742258257.853183389]: send trajectory.
[ INFO] [1742258257.881084660]: send trajectory end.
[ INFO] [1742258257.881161238]: State changed from IDLE to PRE_PROCESSING_PENDING

[ INFO] [1742258257.983476854]: Callback.
[ INFO] [1742258257.983553880]: ACTION_START
[ INFO] [1742258257.983741852]: Callback.
[ INFO] [1742258257.983792350]: ACTION_PREPROCESS_END
[ INFO] [1742258257.983810878]: Preprocessing has finished in the arm and goal has been accepted.
[ INFO] [1742258257.983843391]: State changed from PRE_PROCESSING_PENDING to TRAJECTORY_EXECUTION_PENDING

[ INFO] [1742258258.480116547]: Callback.
[ INFO] [1742258258.679882864]: Callback.
[ INFO] [1742258258.858952878]: Callback.
[ INFO] [1742258258.880209899]: Callback.
[ERROR] [1742258268.024801918]: Controller is taking too long to execute trajectory (the expected upp
er bound for the trajectory execution was 10.783959 seconds). Stopping trajectory.
[INFO] [1742258268.024934754]: Canceling execution for gen3_joint_trajectory_controller
[ INFO] [1742258268.025359021]: Completed trajectory execution with status TIMED_OUT ...
[ INFO] [1742258268.025496176]: Execution completed: TIMED_OUT

[aalmrad]

Hello @fukui-masashi-k ,

Regarding the first question: For such an edge case, sometimes Moveit may not generate a valid trajectory or the controller may skip execution entirely. This might happen because the trajectory may be interpreted as a zero-time or zero-distance trajectory and therefore no valid. I recommend that the trajectory has a larger delta for a more consistent behaviour.

As for the second question: We do not have future plans to develop or optimize the current repository since ROS1 will reach its end of life soon and our focus will be on the ROS2 repository instead. We advise all of our clients to consider moving to ros2 at their earliest convenience.

As for the last question: If you enter a TIME_OUT state you have to restart Moveit to command the robot again.

Best,
Abed

[fukui-masashi-k]

Thank you for reply @aalmrad .

-For such an edge case,
I think this is not edge case.
In the case where the test should be started from point A, it is conceivable that a command to move to point A will be sent once, whether the robot is at point A or not.
In this case, if developer do not want to consider interference, the final posture of the previous test is often set to point A.

-sometimes Moveit may not generate a valid trajectory or the controller may skip execution entirely.
This problem occurs when MoveIt! generates trajectory which is about 0.3 seconds, so MoveIt!'s behavior may not be the main cause.

-the controller may skip execution entirely
Is this problem occurring for the following reasons?
The state transition is strange because the kinova arm is skipping the calculation because it is judging the trajectory as not moving, but the calculation is being performed in ros_kortex.

-As for the second question: We do not have future plans to develop or optimize the current repository since ROS1 will reach its end of life soon and our focus will be on the ROS2 repository instead. We advise all of our clients to consider moving to ros2 at their earliest convenience.
Doesn't the same problem occur with ROS2?

-As for the last question: If you enter a TIME_OUT state you have to restart Moveit to command the robot again.
I understand that this means restarting kortex_driver.launch.

Best Regards.