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!

[pazeshun]

@Michi-Tsubaki
I checked if a limb-specific controller (e.g., :larm-controller) accepts canceling or overwriting on HIRONXJSK.

• :angle-vector
  • :cancel-angle-vector works as expected
  • :stop-motion works as expected
• :angle-vector-sequence
  • :cancel-angle-vector works as expected
  • :stop-motion does not work
    • Robot does not stop during :angle-vector-sequence motion even if :stop-motion is called
    • Just after finishing :angle-vector-sequence motion, motion commanded by :stop-motion is executed immediately and dangerously fast motion occurs

On the basis of these results, I think Nextage Open has more problems in limb-specific controllers than HIRONXJSK.
It may be due to old hrpsys version.

(Appendix) commands I used:

$ rlwrap roseus hironxjsk-interface.l
1.irteusgl$ hironxjsk-init :type :larm-controller
3.irteusgl$ setq init-av (send *hironxjsk* :angle-vector)
;; :angle-vector and :cancel-angle-vector
10.irteusgl$ send *hironxjsk* :larm :move-end-pos #f(0 50 0) :world
11.irteusgl$ send *ri* :angle-vector (send *hironxjsk* :angle-vector) 10000
12.irteusgl$ send *ri* :cancel-angle-vector
14.irteusgl$ send *hironxjsk* :angle-vector init-av
15.irteusgl$ send *ri* :angle-vector (send *hironxjsk* :angle-vector) 3000
;; :angle-vector and :stop-motion
16.irteusgl$ send *hironxjsk* :larm :move-end-pos #f(0 50 0) :world
17.irteusgl$ send *ri* :angle-vector (send *hironxjsk* :angle-vector) 10000
18.irteusgl$ send *ri* :stop-motion
20.irteusgl$ send *hironxjsk* :angle-vector init-av
21.irteusgl$ send *ri* :angle-vector (send *hironxjsk* :angle-vector) 1000
;; :angle-vector-sequence and :cancel-angle-vector
22.irteusgl$ send *hironxjsk* :larm :move-end-pos #f(0 50 0) :world
23.irteusgl$ setq av1 (send *hironxjsk* :angle-vector)
24.irteusgl$ send *hironxjsk* :larm :move-end-pos #f(0 50 0) :world
25.irteusgl$ setq av2 (send *hironxjsk* :angle-vector)
26.irteusgl$ send *ri* :angle-vector-sequence (list av1 av2) (list 10000 10000)
27.irteusgl$ send *ri* :cancel-angle-vector
29.irteusgl$ send *hironxjsk* :angle-vector init-av
30.irteusgl$ send *ri* :angle-vector (send *hironxjsk* :angle-vector) 1000
;; :angle-vector-sequence and :stop-motion
35.irteusgl$ send *ri* :angle-vector-sequence (list av1 av2) (list 10000 10000)
36.irteusgl$ send *ri* :stop-motion

[Michi-Tsubaki]

Thank you for your report!

It may be due to old hrpsys version.

Yeah, in general, we should upgrade if we can, anyway let's discuss/try it after February (thesis). 2025年12月18日(木) 19:07 Shun Hasegawa ***@***.***>:

…

*pazeshun* left a comment (start-jsk/rtmros_hironx#534) <#534 (comment)> @Michi-Tsubaki <https://github.com/Michi-Tsubaki> I checked if a limb-specific controller (e.g., :larm-controller) accepts canceling or overwriting on HIRONXJSK. - :angle-vector - :cancel-angle-vector works as expected - :stop-motion works as expected - :angle-vector-sequence - :cancel-angle-vector works as expected - :stop-motion does not work - Robot does not stop during :angle-vector-sequence motion even if :stop-motion is called - Just after finishing :angle-vector-sequence motion, motion commanded by :stop-motion is executed immediately and dangerously fast motion occurs On the basis of these results, I think Nextage Open has more problems in limb-specific controllers than HIRONXJSK. It may be due to old hrpsys version. (Appendix) commands I used: $ rlwrap roseus hironxjsk-interface.l 1.irteusgl$ hironxjsk-init :type :larm-controller 3.irteusgl$ setq init-av (send *hironxjsk* :angle-vector) ;; :angle-vector and :cancel-angle-vector 10.irteusgl$ send *hironxjsk* :larm :move-end-pos #f(0 50 0) :world 11.irteusgl$ send *ri* :angle-vector (send *hironxjsk* :angle-vector) 10000 12.irteusgl$ send *ri* :cancel-angle-vector 14.irteusgl$ send *hironxjsk* :angle-vector init-av 15.irteusgl$ send *ri* :angle-vector (send *hironxjsk* :angle-vector) 3000 ;; :angle-vector and :stop-motion 16.irteusgl$ send *hironxjsk* :larm :move-end-pos #f(0 50 0) :world 17.irteusgl$ send *ri* :angle-vector (send *hironxjsk* :angle-vector) 10000 18.irteusgl$ send *ri* :stop-motion 20.irteusgl$ send *hironxjsk* :angle-vector init-av 21.irteusgl$ send *ri* :angle-vector (send *hironxjsk* :angle-vector) 1000 ;; :angle-vector-sequence and :cancel-angle-vector 22.irteusgl$ send *hironxjsk* :larm :move-end-pos #f(0 50 0) :world 23.irteusgl$ setq av1 (send *hironxjsk* :angle-vector) 24.irteusgl$ send *hironxjsk* :larm :move-end-pos #f(0 50 0) :world 25.irteusgl$ setq av2 (send *hironxjsk* :angle-vector) 26.irteusgl$ send *ri* :angle-vector-sequence (list av1 av2) (list 10000 10000) 27.irteusgl$ send *ri* :cancel-angle-vector 29.irteusgl$ send *hironxjsk* :angle-vector init-av 30.irteusgl$ send *ri* :angle-vector (send *hironxjsk* :angle-vector) 1000 ;; :angle-vector-sequence and :stop-motion 35.irteusgl$ send *ri* :angle-vector-sequence (list av1 av2) (list 10000 10000) 36.irteusgl$ send *ri* :stop-motion — Reply to this email directly, view it on GitHub <#534 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/BDXQMBUD2BVBQZ5RKA7K6Q34CJ4F7AVCNFSM6AAAAACPNFAYOGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZTMNRZGQ4DEOBYGQ> . You are receiving this because you were mentioned.Message ID: ***@***.***>