Servo goes off with head rolling

d130s commented 8 years ago

At IN [3], all servo went off with "gacha" sound. This happens twice for me today.

ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py -- --host hiro

:
In [1]: robot.checkEncoders()
[hrpsys.py]  calib-joint all 
[hrpsys.py]  done
Turn on Hand Servo

In [2]: robot.setTargetPoseRelative('head', 'HEAD_JOINT1', dp=0.45, tm=12)(array([ 0.    ,  0.    ,  0.5695]), (0.0, 0.45246578493159667, -0.018593892456891147))
('head', 'WAIST', [0.0, 0.0, 0.56950000000000001], [0.0, 0.45246578493159667, -0.018593892456891147], 12)
setTargetPose failed. Maybe SequencePlayer failed to solve IK.
Currently, returning IK result error
(like the one in https://github.com/start-jsk/rtmros_hironx/issues/103) is not implemented. Patch is welcomed.
Out[2]: False

In [3]: robot.setTargetPoseRelative('head', 'HEAD_JOINT1', dp=0.3, tm=12)
(array([ 0.    ,  0.    ,  0.5695]), (0.0, 0.3024657849315966, -0.01859389245689115))
('head', 'WAIST', [0.0, 0.0, 0.56950000000000001], [0.0, 0.3024657849315966, -0.01859389245689115], 12)
Out[3]: True

In [4]: 
Do you really want to exit ([y]/n)? 
Segmentation fault (コアダンプ)
roboworks@M14xR2:~/cws_hironx$ ssh tork@hiro
tork@hiro's password: 
Welcome to QNX Neutrino!
Sat Dec  5 13:51:27 2015 on /dev/ttyp0
Last login: Sat Dec 5 13:42:04 2015 on /dev/ttyp0
edit the file .profile if you want to change your environment.
To start the Photon windowing environment, type "ph".
$ tail -n 50 /opt/jsk/var/log/rtcd.log 
[ServoSerial] sending : FA AF 02 00 24 01 01 01 27  - -1
[ServoSerial] Failed to send packet to servo(id:2)
[ServoSerial] sending : FA AF 03 00 24 01 01 01 26  - -1
[ServoSerial] Failed to send packet to servo(id:3)
[ServoSerial] sending : FA AF 04 00 24 01 01 01 21  - -1
[ServoSerial] Failed to send packet to servo(id:4)
[ServoSerial] sending : FA AF 05 00 24 01 01 01 20  - -1
[ServoSerial] Failed to send packet to servo(id:5)
[ServoSerial] sending : FA AF 06 00 24 01 01 01 23  - -1
[ServoSerial] Failed to send packet to servo(id:6)
[ServoSerial] sending : FA AF 07 00 24 01 01 01 22  - -1
[ServoSerial] Failed to send packet to servo(id:7)
[ServoSerial] sending : FA AF 08 00 24 01 01 01 2D  - -1
[ServoSerial] Failed to send packet to servo(id:8)
[ServoSerial] sending : FA AF 09 00 24 01 01 01 2C  - -1
[ServoSerial] Failed to send packet to servo(id:9)
[ServoSerial] setTorqueOn(2)
[ServoSerial] setTorqueOn(3)
[ServoSerial] setTorqueOn(4)
[ServoSerial] setTorqueOn(5)
[ServoSerial] setTorqueOn(6)
[ServoSerial] setTorqueOn(7)
[ServoSerial] setTorqueOn(8)
[ServoSerial] setTorqueOn(9)
[getCurrentPose] linkaname = HEAD_JOINT1, frame_name = WAIST
IK Fail, iter = 50
  err : 0 ( 0 0 0) < 1e-08
      : 1.00759e-06 ( 0.00100376 -7.24135e-06 1.02465e-19) < 1e-06
[setTargetPose] IK failed
[getCurrentPose] linkaname = HEAD_JOINT1, frame_name = WAIST
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.25592 , Error=2.25592 > 0.18 (limit), servo_state = ON, q=-2.07592
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.261325 , Error=-0.261325 > 0.18 (limit), servo_state = ON, q=0.0813249
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.26632 , Error=2.26632 > 0.18 (limit), servo_state = ON, q=-2.08632
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.255312 , Error=0.255312 > 0.18 (limit), servo_state = ON, q=-0.0753117
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.25592 , Error=2.25592 > 0.18 (limit), servo_state = ON, q=-2.07592
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.261325 , Error=-0.261325 > 0.18 (limit), servo_state = ON, q=0.0813249
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.26632 , Error=2.26632 > 0.18 (limit), servo_state = ON, q=-2.08632
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.255312 , Error=0.255312 > 0.18 (limit), servo_state = ON, q=-0.0753117
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.25592 , Error=2.25592 > 0.18 (limit), servo_state = ON, q=-2.07592
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.261325 , Error=-0.261325 > 0.18 (limit), servo_state = ON, q=0.0813249
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.26632 , Error=2.26632 > 0.18 (limit), servo_state = ON, q=-2.08632
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.255312 , Error=0.255312 > 0.18 (limit), servo_state = ON, q=-0.0753117
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.25519 , Error=2.25519 > 0.18 (limit), servo_state = ON, q=-2.07519
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.260126 , Error=-0.260126 > 0.18 (limit), servo_state = ON, q=0.0801263
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.2658 , Error=2.2658 > 0.18 (limit), servo_state = ON, q=-2.0858
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.253714 , Error=0.253714 > 0.18 (limit), servo_state = ON, q=-0.0737135
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.25061 , Error=2.25061 > 0.18 (limit), servo_state = ON, q=-2.07061
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.252615 , Error=-0.252615 > 0.18 (limit), servo_state = ON, q=0.0726148
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.26137 , Error=2.26137 > 0.18 (limit), servo_state = ON, q=-2.08137
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.245203 , Error=0.245203 > 0.18 (limit), servo_state = ON, q=-0.0652031

d130s commented 8 years ago

Another trial where the same issue happened at IN[2].

In [1]: robot.checkEncoders()
[hrpsys.py]  calib-joint all 
[hrpsys.py]  done
Turn on Hand Servo

In [2]: robot.setTargetPoseRelative('head', 'HEAD_JOINT1', dp=0.1, tm=12)
(array([ 0.    ,  0.    ,  0.5695]), (0.0, 0.11981760482061002, -0.008919586954625709))
('head', 'WAIST', [0.0, 0.0, 0.56950000000000001], [0.0, 0.11981760482061002, -0.008919586954625709], 12)
Out[2]: True

In [3]: 
Do you really want to exit ([y]/n)? 
Segmentation fault (コアダンプ)
roboworks@M14xR2:~/cws_hironx/src/rtmros_hironx$ ssh tork@hiro
tork@hiro's password: 
Welcome to QNX Neutrino!
Sat Dec  5 14:07:00 2015 on /dev/ttyp0
Last login: Sat Dec 5 13:56:12 2015 on /dev/ttyp0
edit the file .profile if you want to change your environment.
To start the Photon windowing environment, type "ph".
$ tail -n 30 /opt/jsk/var/log/rtcd.log
[ServoSerial] Failed to send packet to servo(id:9)
[ServoSerial] setTorqueOn(2)
[ServoSerial] setTorqueOn(3)
[ServoSerial] setTorqueOn(4)
[ServoSerial] setTorqueOn(5)
[ServoSerial] setTorqueOn(6)
[ServoSerial] setTorqueOn(7)
[ServoSerial] setTorqueOn(8)
[ServoSerial] setTorqueOn(9)
[getCurrentPose] linkaname = HEAD_JOINT1, frame_name = WAIST
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.25824 , Error=2.25824 > 0.18 (limit), servo_state = ON, q=-2.07824
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.258468 , Error=-0.258468 > 0.18 (limit), servo_state = ON, q=0.0784681
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.26575 , Error=2.26575 > 0.18 (limit), servo_state = ON, q=-2.08575
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.25697 , Error=0.25697 > 0.18 (limit), servo_state = ON, q=-0.0769698
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.25824 , Error=2.25824 > 0.18 (limit), servo_state = ON, q=-2.07824
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.258468 , Error=-0.258468 > 0.18 (limit), servo_state = ON, q=0.0784681
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.26575 , Error=2.26575 > 0.18 (limit), servo_state = ON, q=-2.08575
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.25697 , Error=0.25697 > 0.18 (limit), servo_state = ON, q=-0.0769698
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.25824 , Error=2.25824 > 0.18 (limit), servo_state = ON, q=-2.07824
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.258468 , Error=-0.258468 > 0.18 (limit), servo_state = ON, q=0.0784681
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.26575 , Error=2.26575 > 0.18 (limit), servo_state = ON, q=-2.08575
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.25697 , Error=0.25697 > 0.18 (limit), servo_state = ON, q=-0.0769698
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.2577 , Error=2.2577 > 0.18 (limit), servo_state = ON, q=-2.0777
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.257289 , Error=-0.257289 > 0.18 (limit), servo_state = ON, q=0.0772895
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.26525 , Error=2.26525 > 0.18 (limit), servo_state = ON, q=-2.08525
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.255951 , Error=0.255951 > 0.18 (limit), servo_state = ON, q=-0.075951
error limit over RARM_JOINT2(5), qRef=0, qCurrent=-2.25333 , Error=2.25333 > 0.18 (limit), servo_state = ON, q=-2.07333
error limit over RARM_JOINT3(6), qRef=0, qCurrent=0.250197 , Error=-0.250197 > 0.18 (limit), servo_state = ON, q=0.0701975
error limit over LARM_JOINT2(11), qRef=0, qCurrent=-2.26087 , Error=2.26087 > 0.18 (limit), servo_state = ON, q=-2.08087
error limit over LARM_JOINT3(12), qRef=0, qCurrent=-0.24808 , Error=0.24808 > 0.18 (limit), servo_state = ON, q=-0.0680799

k-okada commented 8 years ago

humm, joint limit has changed due to new head parts?

130s commented 8 years ago

Some additional observation:

In the command examples above, I didn't run goInitial. But there was another incident where I did run it, and the same error still occurred.
From MoveIt!, I was able to move both of 2 head joints without any issue.
- To do that with Hiro I needed a "hack" -- since in hironx_moveit_config package there's not yet head joints integrated, I used nextage_moveit_config just to move the head.
I haven't confirmed whether the same issue happened or not with the default head model.

humm, joint limit has changed due to new head parts?

Hm, then I can't yet explain why the same issue doesn't occur with MoveIt!.

130s commented 8 years ago

I also wonder in the log why error limits were reported for arm joints but not for head.

start-jsk / rtmros_hironx

Servo goes off with head rolling #411