search

start-jsk / rtmros_hironx

hironx controller and applications using rtmros packages
http://wiki.ros.org/rtmros_hironx
10 stars 26 forks source link

clearOfGroup after setTargetPose breaks sequencer #505

Closed k-okada closed 7 years ago

k-okada commented 7 years ago

add test to check https://github.com/tork-a/rtmros_nextage/issues/332 situation

k-okada commented 7 years ago

test code for rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch

ipython -i `rospack find nextage_ros_bridge`/script/nextage.py <<EOF

def getJointAnglesOfGroup(limb):
    angles = robot.flat2Groups(robot.getJointAngles())
    groups = robot.Groups
    for i in range(len(groups)):
        if groups[i][0] == limb:
            return angles[i]
    print self.configurator_name, 'could no find limb name ' + limb
    print self.configurator_name, ' in' + filter(lambda x: x[0], groups)
robot.getJointAnglesOfGroup = getJointAnglesOfGroup

robot.goInitial()

rpy = robot.getCurrentRPY('LARM_JOINT5')

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

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

print("go to posA")
robot.setTargetPose('larm', posA, rpy, 5)
print("wait for  posA")
robot.waitInterpolation()
print(robot.getJointAnglesOfGroup('larm'))

print("go to posB")
robot.setTargetPose('larm', posB, rpy, 10)
import time
time.sleep(2)
print("clear larm")
robot.clearOfGroup('larm')  # Ongoing command cancelled. Movement stops. 
print(robot.getJointAnglesOfGroup('larm'))
#robot.setJointAnglesOfGroup('larm', robot.getJointAnglesOfGroup('larm'), 0.1, wait=True) #ok
#robot.setJointAngles(robot.getJointAngles(), 0.1) # ng
#robot.setJointAnglesOfGroup('torso', [0], 0.1, wait=True) #ok

print("go to posA > move fast!!!")
robot.setTargetPose('larm', posA, rpy, 10)  # The problematic brupt move occurs here.
print("wait for  posA")
robot.waitInterpolation()

###########
###########
###########
poseA =[ 45, 0, -100, -15.2, 9.4, -3.2]
poseB =[0.6, 0, -100, -15.2, 9.4, -3.2]

robot.goInitial()
print("go to poseA")
robot.setJointAnglesOfGroup('larm', poseA, 5, wait=False)
print("wait for  poseA")
robot.waitInterpolation()

print("go to poseB")
robot.setJointAnglesOfGroup('larm', poseB, 5, wait=False)
import time
time.sleep(2)
print("clear larm")
robot.clearOfGroup('larm')  # Ongoing command cancelled. Movement stops. 

print("go to poseA > move fast???")
robot.setJointAnglesOfGroup('larm', poseA, 5, wait=False)
print("wait for  poseA")
robot.waitInterpolation()

###########
###########
###########
poseA =[ 45, 0, -100, -15.2, 9.4, -3.2]
poseB =[0.6, 0, -100, -15.2, 9.4, -3.2]

robot.goInitial()
print("go to poseA")
robot.setJointAnglesSequenceOfGroup('larm', [poseA], [5])
print("wait for  poseA")
robot.waitInterpolation()

print("go to poseB")
robot.setJointAnglesSequenceOfGroup('larm', [poseB], [5])
import time
time.sleep(2)
print("clear larm")
robot.clearOfGroup('larm')  # Ongoing command cancelled. Movement stops. 

print("go to poseA > move fast???")
############ WORKAROUND
poseA =[ 45, 0, -100, -15.2, 9.4, -3.2]
robot.setJointAnglesSequenceOfGroup('larm', [poseA], [5])
print("wait for  poseA")
robot.waitInterpolation()

EOF
130s commented 7 years ago

On Travis only TEST_TYPE=work_with_315_1_10 failed at the following:

:
[hironx_ros_bridge.rosunit-test_hironx_limb/test_rarm_setJointAngles_Clear][ERROR]
CORBA.COMM_FAILURE(omniORB.COMM_FAILURE_WaitingForReply, CORBA.COMPLETED_MAYBE)
  File "/usr/lib/python2.7/unittest/case.py", line 331, in run
    testMethod()
  File "/home/travis/catkin_ws/install_isolated/share/hironx_ros_bridge/test/test_hironx_limb.py", line 79, in test_rarm_setJointAngles_Clear
    self.robot.clearOfGroup("rarm")
  File "/home/travis/catkin_ws/install_isolated/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py", line 1127, in clearOfGroup
    self.setJointAnglesOfGroup(limb, angles, 0.1, wait=True)
  File "/home/travis/catkin_ws/install_isolated/lib/python2.7/dist-packages/hrpsys/hrpsys_config.py", line 1101, in setJointAnglesOfGroup
    ret = self.seq_svc.setJointAnglesOfGroup(gname, angles, tm)
  File "/home/travis/catkin_ws/install_isolated/lib/python2.7/dist-packages/hrpsys/SequencePlayerService_idl.py", line 185, in setJointAnglesOfGroup
    return _omnipy.invoke(self, "setJointAnglesOfGroup", _0_OpenHRP.SequencePlayerService._d_setJointAnglesOfGroup, args)
--------------------------------------------------------------------------------

[hironx_ros_bridge.rosunit-test_hironx_limb/test_rarm_setJointAngles_NoWait][ERROR]
CORBA.TRANSIENT(omniORB.TRANSIENT_ConnectFailed, CORBA.COMPLETED_NO)
  File "/usr/lib/python2.7/unittest/case.py", line 331, in run
    testMethod()
  File "/home/travis/catkin_ws/install_isolated/share/hironx_ros_bridge/test/test_hironx_limb.py", line 45, in test_rarm_setJointAngles_NoWait
    self.limbbody_init()
  File "/home/travis/catkin_ws/install_isolated/share/hironx_ros_bridge/test/test_hironx_limb.py", line 10, in limbbody_init
    self.robot.setSelfGroups()
  File "/home/travis/catkin_ws/install_isolated/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py", line 590, in setSelfGroups
    self.seq_svc.addJointGroup(item[0], item[1])
  File "/home/travis/catkin_ws/install_isolated/lib/python2.7/dist-packages/hrpsys/SequencePlayerService_idl.py", line 179, in addJointGroup
    return _omnipy.invoke(self, "addJointGroup", _0_OpenHRP.SequencePlayerService._d_addJointGroup, args)
--------------------------------------------------------------------------------

[hironx_ros_bridge.rosunit-test_hironx_limb/test_rarm_setJointAngles_Wait][ERROR]
CORBA.TRANSIENT(omniORB.TRANSIENT_ConnectFailed, CORBA.COMPLETED_NO)
  File "/usr/lib/python2.7/unittest/case.py", line 331, in run
    testMethod()
  File "/home/travis/catkin_ws/install_isolated/share/hironx_ros_bridge/test/test_hironx_limb.py", line 22, in test_rarm_setJointAngles_Wait
    self.limbbody_init()
  File "/home/travis/catkin_ws/install_isolated/share/hironx_ros_bridge/test/test_hironx_limb.py", line 10, in limbbody_init
    self.robot.setSelfGroups()
  File "/home/travis/catkin_ws/install_isolated/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py", line 590, in setSelfGroups
    self.seq_svc.addJointGroup(item[0], item[1])
  File "/home/travis/catkin_ws/install_isolated/lib/python2.7/dist-packages/hrpsys/SequencePlayerService_idl.py", line 179, in addJointGroup
    return _omnipy.invoke(self, "addJointGroup", _0_OpenHRP.SequencePlayerService._d_addJointGroup, args)
--------------------------------------------------------------------------------

[hironx_ros_bridge.rosunit-test_hironx_target/testGetCurrentPose][passed]
[hironx_ros_bridge.rosunit-test_hironx_target/testGetReferencePose][passed]
[hironx_ros_bridge.rosunit-test_hironx_target/testGetterByFrame][passed]
[hironx_ros_bridge.rosunit-test_hironx_target/testSetTargetPoseBothArm][passed]
[hironx_ros_bridge.rosunit-test_hironx_target/test_get_geometry_methods_noarg][passed]
[hironx_ros_bridge.rosunit-test_hironx_target/test_setTargetPoseRelative_rpy][passed]

SUMMARY
 * RESULT: FAIL
 * TESTS: 25
 * ERRORS: 3
 * FAILURES: 0
130s commented 7 years ago

Same as https://github.com/start-jsk/rtmros_hironx/pull/505#issuecomment-304074643, only the test on hrpsys 315_1_10 failed exactly at the same testcases. As in https://github.com/fkanehiro/hrpsys-base/pull/1064#issuecomment-257197543, some of the features were introduced AFTER 315.5.x or so, so that they are not available for earlier version.

@k-okada I don't know if it's right but I added c5c3f37 since I did not know any other way to let the tests pass than this. Please feel free to reset that commit and add yours if anythiing.

k-okada commented 7 years ago

squashed commit

130s commented 7 years ago

squashed commit

Took this as an approval.