Command overwritting does not work on angle-vector-sequence

[Affonso-Gui]

I have had some strange behavior related to this on last experiment and just found out what was going wrong:

Sending a new goal to the robot controllers does not overwrite ongoing angle-vector sequence goals.

How to reproduce:
Send some angle-vector-sequence and then call :stop-motion before the motion ends.
Expected behavior:
The old goal gets overwritten with a new one holding the current joint states, making the robot smoothly stop the motion.
Actual behaviour:
A new command with the current joint states is stacked and is only executed when the current motion finishes. To make things worse this command is executed on min-time, making the robot move super fast (which got me some fingers broken last time...)

Going to have a deeper look in the afternoon.

@pazeshun

[pazeshun]

Sending a new goal to the robot controllers does not overwrite ongoing angle-vector sequence goals.

This is known issue for me, but I totally forgot to tell you. Sorry...
This is caused by old hrpsys API.

In hrpsys_ros_bridge, different function is called between when joint trajectory has one element (:angle-vector) and when joint trajectory has more than one element (:angle-vector-sequence).
In latest released hrpsys_ros_bridge (1.4.2), the function called in the former case is setJointAngles:
https://github.com/start-jsk/rtmros_common/blob/1.4.2/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp#L216
The function called in the latter case is playPattern:
https://github.com/start-jsk/rtmros_common/blob/1.4.2/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp#L219
The movement started by setJointAngles can be overwritten when new setJointAngles or playPattern (I didn't confirm) is called.
However, the movement started by playPattern cannot be overwritten.
The new command is stacked and executed after the whole movement by playPattern finishes.

With start-jsk/rtmros_common#765 and k-okada/rtmros_common#10 (maybe you already use), setJointAnglesSequence is used instead of playPattern when hrpsys version is not less than 315.5.0:
https://github.com/k-okada/rtmros_common/blob/2913445fef17a136bfb7ba276ead2c1be52e3dc1/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp#L219-L224
setJointAnglesSequence can be overwritten with new command.
However, Hiro has hrpsys 315.1.9 and cannot use setJointAnglesSequence.

To make things worse this command is executed on min-time, making the robot move super fast

Maybe this is caused by short duration in your command, because the stacked command includes duration (hrpsys only uses execution duration without starting timestamp) and that duration is calculated with time_from_start in joint trajectory:
https://github.com/start-jsk/rtmros_common/blob/1.4.2/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp#L205-L210
Although start-jsk/rtmros_common#1052 manages starting timestamp, it is conducted in hrpsys_ros_bridge loop and that loop doesn't stop with playPattern, so execution is slow when time_from_start is long.

[Affonso-Gui]

Maybe this is caused by short duration in your command

Yes, in this case I was using :stop-motion, which defaults to zero time (min-time).

The movement started by setJointAngles can be overwritten when new setJointAngles or playPattern (I didn't confirm) is called.

I made some confirmation experiments with the following results:

• playPattern cannot be overwritten by either setJointAngles and playPattern
• setJointAngles can be successfully overwritten by other setJointAngles
• When setJointAngles is overwritten by playPattern, the robot servos go down after finishing the angle-vector sequence execution video

Maybe this is a good timing to try to update the hrpsys version.

[Affonso-Gui]

Fixed by updating to the newest hrpsys version (fkanehiro/hrpsys-base#1283)!
💯

[pazeshun]

Fixed by updating to the newest hrpsys version (fkanehiro/hrpsys-base#1283)!

Also, start-jsk/rtmros_common#1071 is required.

We keep this issue open, because this issue remains in default HIRONX and NEXTAGE OPEN (using hrpsys 315.1.7).

[pazeshun]

@Affonso-Gui
I found that this issue still occurs when you command to only one arm (using :rarm-controller or :larm-controller in roseus).
Please be careful.

This is because hrpsys 315.16.0 including fkanehiro/hrpsys-base#1237 is not released.
That PR is required to use setJointAnglesSequenceOfGroup (overwritable version of playPatternOfGroup (one arm version of playPattern) ), so start-jsk/rtmros_common#1071 uses setJointAnglesSequenceOfGroup only when hrpsys version is 315.16.0 and upper.
https://github.com/start-jsk/rtmros_common/pull/1071/files#diff-58737e307d5564178dd2ba38f8f46d72R502-R510
However, hrpsys 315.16.0 is not released, so playPatternOfGroup is used when you command to only one arm.

[Affonso-Gui]

@pazeshun Thanks for the heads up!