Open Affonso-Gui opened 4 years ago
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 https://github.com/start-jsk/rtmros_common/pull/765 and https://github.com/k-okada/rtmros_common/pull/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 https://github.com/start-jsk/rtmros_common/pull/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.
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
setJointAngles
is overwritten by playPattern
, the robot servos go down after finishing the angle-vector sequence execution videoMaybe this is a good timing to try to update the hrpsys version.
Fixed by updating to the newest hrpsys version (https://github.com/fkanehiro/hrpsys-base/pull/1283)! :100:
Fixed by updating to the newest hrpsys version (fkanehiro/hrpsys-base#1283)!
Also, https://github.com/start-jsk/rtmros_common/pull/1071 is required.
We keep this issue open, because this issue remains in default HIRONX and NEXTAGE OPEN (using hrpsys 315.1.7).
@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 https://github.com/fkanehiro/hrpsys-base/pull/1237 is not released.
That PR is required to use setJointAnglesSequenceOfGroup
(overwritable version of playPatternOfGroup
(one arm version of playPattern
) ), so https://github.com/start-jsk/rtmros_common/pull/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.
@pazeshun Thanks for the heads up!
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