setJointAngles moves arms too quick

[130s]

Similar to start-jsk/rtmros_hironx#411, a NXO user is experiencing an unexpected servo off issue.

from nextage_ros_bridge import nextage_client
from hrpsys import rtm
rtm.nshost=...
rtm.nsport=...
robot = nextage_client.NextageClient()
robot.init()
robot.checkEncoders()
rpy = robot.getCurrentRPY('LARM_JOINT5')
pos = robot.getCurrentPosition('LARM_JOINT5')

print pos
print rpy

robot.setTargetPose('larm',pos, rpy, 5)

robot.waitInterpolation()

She says servo goes off at this moment.

However, with the following run before setTargetPose the issue does not happen.

robot.setJointAnglesOfGroup('torso',[-5], 5, wait=True)

Looking at the robot's/server's side log:

In ModelLoader.log, I see some errors at the end, which I have no idea if it's related to this issue.

ready
loading /opt/jsk/etc/HIRONX/model/main.wrl
Humanoid node
Joint nodeWAIST
  Segment node WAIST_Link
  Joint nodeCHEST_JOINT0
    Segment node CHEST_JOINT0_Link
    Joint nodeHEAD_JOINT0
      Segment node HEAD_JOINT0_Link
      Joint nodeHEAD_JOINT1
        Segment node HEAD_JOINT1_Link
        VisionSensorCAMERA_HEAD_R
        VisionSensorCAMERA_HEAD_L
    Joint nodeRARM_JOINT0
      Segment node RARM_JOINT0_Link
      Joint nodeRARM_JOINT1
        Segment node RARM_JOINT1_Link
        Joint nodeRARM_JOINT2
          Segment node RARM_JOINT2_Link
          Joint nodeRARM_JOINT3
            Segment node RARM_JOINT3_Link
            Joint nodeRARM_JOINT4
              Segment node RARM_JOINT4_Link
              Joint nodeRARM_JOINT5
                Segment node RARM_JOINT5_Link
    Joint nodeLARM_JOINT0
      Segment node LARM_JOINT0_Link
      Joint nodeLARM_JOINT1
        Segment node LARM_JOINT1_Link
        Joint nodeLARM_JOINT2
          Segment node LARM_JOINT2_Link
          Joint nodeLARM_JOINT3
            Segment node LARM_JOINT3_Link
            Joint nodeLARM_JOINT4
              Segment node LARM_JOINT4_Link
              Joint nodeLARM_JOINT5
                Segment node LARM_JOINT5_Link
Node is inconvertible and removed from the scene graph
Node is inconvertible and removed from the scene graph
Warning: Mass is zero. <Model>HiroNX <Link>CHEST_JOINT0
Warning: Mass is zero. <Model>HiroNX <Link>WAIST
The model was successfully loaded ! 
cache found for /opt/jsk/etc/HIRONX/model/main.wrl
cache found for /opt/jsk/etc/HIRONX/model/main.wrl
cache found for /opt/jsk/etc/HIRONX/model/main.wrl
cache found for /opt/jsk/etc/HIRONX/model/main.wrl
cache found for /opt/jsk/etc/HIRONX/model/main.wrl
cache found for /opt/jsk/etc/HIRONX/model/main.wrl
loading 
 cannot be found.
Retrying to load the file as a standard VRML file
 cannot be found.
loading failed.
 cannot be found.

In rtcd.log then the issue occurred, lots of limit errors occurred.

:
Upper joint limit error LARM_JOINT0
Upper joint limit error LARM_JOINT2
Upper joint limit error LARM_JOINT3
Upper joint limit error LARM_JOINT4
Upper joint limit error LARM_JOINT4
Upper joint limit error LARM_JOINT4
Upper joint limit error LARM_JOINT4
Upper joint limit error LARM_JOINT4
Upper joint limit error LARM_JOINT4
Upper joint limit error LARM_JOINT4
Upper joint limit error LARM_JOINT4
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.26024 , Error=2.26024 > 0.18 (limit), servo_state = ON, q=-2.08024
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.263023 , Error=-0.263023 > 0.18 (limit), servo_state = ON, q=0.083023
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.26253 , Error=2.26253 > 0.18 (limit), servo_state = ON, q=-2.08253
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.264481 , Error=0.264481 > 0.18 (limit), servo_state = ON, q=-0.0844814
Timeover: processing time =  6.0[ms]
 0.0,  1.0,  0.0,  0.0,  0.0,  5.0,  0.0,  0.0, 
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.26024 , Error=2.26024 > 0.18 (limit), servo_state = ON, q=-2.08024
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.263023 , Error=-0.263023 > 0.18 (limit), servo_state = ON, q=0.083023
error limit over LARM_JOINT2(11), qRef=-0.0162838, qCurrent=-2.26253 , Error=2.24625 > 0.18 (limit), servo_state = ON, q=-2.08253
error limit over LARM_JOINT3(12), qRef=0.0317515, qCurrent=-0.264481 , Error=0.296233 > 0.18 (limit), servo_state = ON, q=-0.0844814
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.26024 , Error=2.26024 > 0.18 (limit), servo_state = ON, q=-2.08024
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.263023 , Error=-0.263023 > 0.18 (limit), servo_state = ON, q=0.083023
error limit over LARM_JOINT2(11), qRef=-0.107084, qCurrent=-2.26253 , Error=2.15545 > 0.18 (limit), servo_state = ON, q=-2.08253
error limit over LARM_JOINT3(12), qRef=0.208801, qCurrent=-0.264481 , Error=0.473283 > 0.18 (limit), servo_state = ON, q=-0.0844814
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.25978 , Error=2.25978 > 0.18 (limit), servo_state = ON, q=-2.07978
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.262424 , Error=-0.262424 > 0.18 (limit), servo_state = ON, q=0.0824237
error limit over LARM_JOINT2(11), qRef=-0.291508, qCurrent=-2.26207 , Error=1.97056 > 0.18 (limit), servo_state = ON, q=-2.08207
error limit over LARM_JOINT3(12), qRef=0.568405, qCurrent=-0.263902 , Error=0.832307 > 0.18 (limit), servo_state = ON, q=-0.083902
error limit over LARM_JOINT4(13), qRef=0.193237, qCurrent=-0.00312932 , Error=0.196367 > 0.18 (limit), servo_state = ON, q=0.176871
Timeover: processing time = 10.0[ms]
 0.0,  0.0,  0.0,  0.0,  0.0, 10.0,  0.0,  0.0, 
error limit over LARM_JOINT5(14), qRef=-0.299952, qCurrent=-0.00125572 , Error=-0.298696 > 0.18 (limit), servo_state = ON, q=-0.181256

(Entire rtcd.log BEFORE and AFTER the issue happened.

[130s]

On simulator, I confirmed that I can replicate the problematic behavior that could lead to the unexpected servo off.

The issue occurs after 12 seconds in this video

term-1$ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch

term-2$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py
:
[hrpsys.py] initialized successfully
:
robot.goInitial(tm=2)
rpy = robot.getCurrentRPY('LARM_JOINT5')

posA = [0.13650680113947125, 0.5715476591372091, 0.20562243422467796]
print(posA)

posB = [0.45842124152338815, 0.26311200372284116, 0.28046365811768403]
print(posB)

robot.setTargetPose('larm', posA, rpy, 5)
robot.waitInterpolation()

robot.setTargetPose('larm', posB, rpy, 10)
import time
time.sleep(2)
robot.clearOfGroup('larm')  # Ongoing command cancelled. Movement stops. 

# This doesn't seem to mitigate the issue as opposed to what the customer reports.
# This might not be the right place to be inserted.
robot.setJointAngles(robot.getJointAngles(), 2)  # The problematic abrupt move occurs here.

robot.setJointAnglesOfGroup('torso', [-5], 5, wait=True)

rpy = robot.getCurrentRPY('LARM_JOINT5')
robot.setTargetPose('larm', posA, rpy, 10)
robot.waitInterpolation()

And I also confirmed that removing the line robot.setJointAngles(robot.getJointAngles(), 2) after clearOfGroup the issue does not happen.

This tells me that robot.getJointAngles() gets the joint positions that the previous command aimed at, regardless the actual positions at the time.

[k-okada]

It seems this PR contains several different issues,

1. send

robot.setTargetPose('larm',pos, rpy, 5)
robot.waitInterpolation()

goes servo off. From fkanehiro/hrpsys-base#519, you have to call goInitial() before start using setTargetPose and your observation of However, with the following run before setTargetPose the issue does not happen. makes sense.

Note this only happens on robot which uses hrpsys <= 315.3.0.

robot.clearOfGroup('larm')  # Ongoing command cancelled. Movement stops. 
robot.setJointAnglesOfGroup('torso', [-5], 5, wait=True)  # The problematic brupt move occurs here

This is bug, see start-jsk/rtmros_hironx#505 for workaround and fkanehiro/hrpsys-base#1141 for more comprehensive solution.

Until #505 gets merged, you can solve this problem by any of following line

robot.setJointAnglesOfGroup('larm', robot.getJointAnglesOfGroup('larm'), 0.1, wait=True) # see #505 to how to define getJointAnglesOfGroup
robot.setJointAnglesOfGroup('larm', robot.flat2Groups(robot.getJointAngles())[3], 0.1, wait=True) #ok 3: for larm, 2 for rarm
robot.setJointAnglesOfGroup('torso', [0], 0.1, wait=True) # if you know the current torso location

robot.setTargetPose('larm', posB, rpy, 10)
robot.clearOfGroup('larm')  # Ongoing command cancelled. Movement stops. 
robot.setJointAngles(robot.getJointAngles(), 2)

Once you call setTargetPose of setJointAnglesOfGroup is called, please do not call setJointAngles, when we use group interpolator on sequenceplayer, fullbody controller is override by that group controller,

[130s]

robot.setTargetPose('larm', posB, rpy, 10)
robot.clearOfGroup('larm')  # Ongoing command cancelled. Movement stops. 
robot.setJointAngles(robot.getJointAngles(), 2)

Once you call setTargetPose of setJointAnglesOfGroup is called, please do not call setJointAngles, when we use group interpolator on sequenceplayer, fullbody controller is override by that group controller,

Combined with the discussion in the upstream fkanehiro/hrpsys-base#1148 (in Japanese),

You should not mix using setJointAngles and some other methods that takes kinematics group as an arg, such as setTargetPose, clearOfGroup etc., which interpolates by the specified group while setJointAngles does so for the full body.

This may sound complicated, and it IS too complicated for the api users. We're discussing to come up with simpler, user-friendly description in fkanehiro/hrpsys-base#1148.

So the command set in #332 (comment) causes an issue, but the following, which the same user reported offline, works around this issue.

robot.goInitial(tm=2)
rpy = robot.getCurrentRPY('LARM_JOINT5')

posA = [0.13650680113947125, 0.5715476591372091, 0.20562243422467796]
print(posA)

posB = [0.45842124152338815, 0.26311200372284116, 0.28046365811768403]
print(posB)

robot.setTargetPose('larm', posA, rpy, 5)
robot.waitInterpolation()

robot.setTargetPose('larm', posB, rpy, 10)
import time
time.sleep(2)
robot.clearOfGroup('larm')  # Ongoing command cancelled. Movement stops. 

# This doesn't seem to mitigate the issue as opposed to what the customer reports.
# This might not be the right place to be inserted.
#robot.setJointAngles(robot.getJointAngles(), 2)  # The problematic abrupt move occurs here.
robot.setJointAnglesOfGroup('torso', [0], 0, wait=True)

robot.setJointAnglesOfGroup('torso', [-5], 5, wait=True)

rpy = robot.getCurrentRPY('LARM_JOINT5')
robot.setTargetPose('larm', posA, rpy, 10)
robot.waitInterpolation()

Only difference is this line:

#robot.setJointAngles(robot.getJointAngles(), 2)  # The problematic abrupt move occurs here.
robot.setJointAnglesOfGroup('torso', [0], 0, wait=True)