search

start-jsk / rtmros_hironx

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

宿題 #423

Open k-okada opened 8 years ago

k-okada commented 8 years ago

１。普通にlaunchを立ち上げるとcollisiondetectorのコンポーネントが入らないです。なので入れるようにするにはどうしたらいいかしらべて、入れてみてください。 ２。その時に両腕の前腕がセルフコリジョンする動作をつくってください。いきなりだと危ないので、まず１０cmぐらい離れたし姿勢になって、１cmづつ、ちょっとづつ近づくようにしてみてください。 ３。たぶん、手のモデルが入っていないので、手のモデルをいれてください。BoundingBoxでよいです。ちょっとおおきいぐらいでよいです。１、２cmぐらい。で、両腕の手と手がぶつかる動作を２と同じようにして動かして見てください。

できたら実機で試しましょう。 @pazeshun

pazeshun commented 8 years ago

RTCTREE_NAMESERVERS=hiro014:15005 rtls hiro014:15005 か RTCTREE_NAMESERVERS=hiro014:2809 rtls hiro014:2809 はどうなっているかな

すみません、これの意味がわからないのですが、以下のような操作であってますでしょうか？

$ export RTCTREE_NAMESERVERS=hiro014:2809
$ rtls hiro014:2809
rtls: Cannot list ports.
$ export RTCTREE_NAMESERVERS=hiro014:15005
$ rtls hiro014:15005
rtls: Cannot list ports.

どっちもエラーが出てます。

k-okada commented 8 years ago

はい．

pazeshun@pazeshun-ThinkPad-T460s:~$ export RTCTREE_NAMESERVERS=hiro014:15005
pazeshun@pazeshun-ThinkPad-T460s:~$ rtls hiro014:15005/

だ．最後に/がひつよう． export するとその後のshellで，その環境変数が有効

RTCTREE_NAMESERVERS=hiro014:15005 rtls hiro014:15005

すると，この行だけ環境変数が有効．

pazeshun commented 8 years ago 
$ export RTCTREE_NAMESERVERS=hiro014:15005
$ rtls hiro014:15005/
sc.rtc  ModelLoader  el.rtc   hiro014.host_cxt/  fk.rtc    seq.rtc
sh.rtc  log.rtc      *co.rtc  ic.rtc            rmfo.rtc  RobotHardware0.rtc

co.rtcには*がついてます。

k-okada commented 8 years ago

ソースのバージョンは315.1.10 かな？co.rtcができていて＊はエラーかな．rtcat -l で分かるはず． https://github.com/fkanehiro/hrpsys-base/blob/315.1.10/rtc/CollisionDetector/CollisionDetector.cpp みると， co: onInitialize() は表示されているはずなんだけどなぁ．

pazeshun commented 8 years ago

すみません、私の一つ前のコメントは、CollisionDetectorを入れなかった場合のものでした。 入れると、前に上げたエラーがipython側で出るのと、

$ rtls hiro014:15005/
*sc.rtc  ModelLoader  *el.rtc           *co.rtc  *fk.rtc    *seq.rtc
*sh.rtc  *log.rtc     hiro014.host_cxt/  *ic.rtc  *rmfo.rtc  RobotHardware0.rtc

のようになります。

pazeshun commented 8 years ago 
$ rtcat -l hiro014:15005/co.rtc
rtcat: Zombie object: hiro014:15005/co.rtc

のようです。

k-okada commented 8 years ago

お，ぞんびだとアレだな．どうやってrtcd再起動している？ slay rtcdみたいな？ NameServerをリスタートしないとゾンビは生きていそうなので，リブートかomniNamesをslayして， NameServer.sh を立ち上げ直すか

pazeshun commented 8 years ago

ソースのバージョンは315.1.10 かな？

多分、ソースの元のバージョンは315.1.7で、それに、OpenCVがインストールされていなくてもCollisionDetectorを使えるようにするパッチ（https://patch-diff.githubusercontent.com/raw/fkanehiro/hrpsys-base/pull/428.diff ）を若干編集して当てたものです。 今、現在のhrpsys-baseとのdiffをとってみようかと思って、hrpsys-baseをクローンしています。

k-okada commented 8 years ago

package.xml か CHANGELOG.rst みたらバージョンはわかるともう．

pazeshun commented 8 years ago

お，ぞんびだとアレだな．どうやってrtcd再起動している？ slay rtcdみたいな？ NameServerをリスタートしないとゾンビは生きていそうなので，リブートかomniNamesをslayして， NameServer.sh を立ち上げ直すか

とりあえず再起動してました。

pazeshun commented 8 years ago

package.xml か CHANGELOG.rst みたらバージョンはわかるともう．

どこにあるのかわかりませんが、とりあえずsvnのタグから315.1.7であることがはっきりしました。残ってるdiffを確認すると、当てたパッチとBVutil.cppの変更のみのようなので、あってると思います。

k-okada commented 8 years ago

ああ，315.1.7のときはros package ではなかったんだ． どこかで，死んでいるんだけどなぁ． http://www.openrtm.org/openrtm/ja/book/export/html/1335 をみると，onInitializeのまえのコンストラクタで死んでいるのかな． https://github.com/fkanehiro/hrpsys-base/blob/315.1.7/rtc/CollisionDetector/CollisionDetector.cpp#L65 それともonInitializeまではでているけど，バッファにたまっている時に死んでいるか．

https://github.com/start-jsk/rtmros_hironx/blob/indigo-devel/hironx_ros_bridge/launch/hironx_ros_bridge.launch#L50-L56 は，立ち上がっていないよね？

.so がrtcdにロードされているとデバッグがしずらいから，Compを動かして見る作戦もある．rtcdを立ち上た，あと，QNXで RTCTREE_NAMESERVERS=hiro014:15005 SIMULATOR_NAME=RobotHardware0 /CollisionDetectorComp -o corba.nameservers:hiro014:15005 -o naming.formats:co.rtc -o exec_cxt.periodic.type:PeriodicExecutionContext -o exec_cxt.periodic.rate:2000 -o logger.file_name:/tmp/collision_detector%p.log -o example.CollisionDetector.config_file://opt/jsk/etc/HIROINX/hrprtc/Robot.conf みたいにして， rtls で co.rtcがエラー無くあって，rtcat -l すると，STOPEDかなにかになっているはず．ここで， coのcreateCompはしないけど，activateとconnectをするようにしたら，.soをつかったことと同じになる． https://github.com/start-jsk/rtmros_hironx/blob/indigo-devel/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py#L318-L329 https://github.com/fkanehiro/hrpsys-base/blob/315.9.0/python/hrpsys_config.py

.soとCompはnodelet とnodeの関係．

pazeshun commented 8 years ago

https://github.com/start-jsk/rtmros_hironx/blob/indigo-devel/hironx_ros_bridge/launch/hironx_ros_bridge.launch#L50-L56 は，立ち上がっていないよね？

手元PCではhironx.pyを立ち上げているだけなので、これは立ち上がっていないという認識です。

pazeshun commented 8 years ago

QNX再起動後、

$ rtls hiro014:15005/
ModelLoader  RobotHardware0.rtc  hiro014.host_cxt/

となっているのを確認して、QNXの/opt/jsk/bin上で

$ RTCTREE_NAMESERVERS=hiro014:15005 SIMULATOR_NAME=RobotHardware0 ./CollisionDetectorComp -o corba.nameservers:hiro014:15005 -o naming.formats:co.rtc -o exec_cxt.periodic.type:PeriodicExecutionContext -o exec_cxt.periodic.rate:2000 -o logger.file_name:/tmp/collision_detector%p.log -o example.CollisionDetector.config_file://opt/jsk/etc/HIROINX/hrprtc/Robot.conf
Logger::Logger: streambuf address = 0x80bd938
Error: cannot open logfile: ./rtc667685.log
CollisionDetector0: onInitialize()
Number of Ports: 4

Port0 (name): CollisionDetector0.qRef
---interfaces---
---properties---
port.port_type: DataInPort
dataport.data_type: IDL:RTC/TimedDoubleSeq:1.0
dataport.subscription_type: Any
dataport.dataflow_type: push,pull
dataport.interface_type: corba_cdr
----------------

Port1 (name): CollisionDetector0.qCurrent
---interfaces---
---properties---
port.port_type: DataInPort
dataport.data_type: IDL:RTC/TimedDoubleSeq:1.0
dataport.subscription_type: Any
dataport.dataflow_type: push,pull
dataport.interface_type: corba_cdr
----------------

Port2 (name): CollisionDetector0.q
---interfaces---
---properties---
port.port_type: DataOutPort
dataport.data_type: IDL:RTC/TimedDoubleSeq:1.0
dataport.subscription_type: flush,new,periodic
dataport.dataflow_type: push,pull
dataport.interface_type: corba_cdr
----------------

Port3 (name): CollisionDetector0.CollisionDetectorService
---interfaces---
I/F name: service0
I/F type: CollisionDetectorService
Polarity: PROVIDED
---properties---
port.port_type: CorbaPort
----------------

とでき、この時

$ rtls hiro014:15005/
co.rtc  ModelLoader  RobotHardware0.rtc  hiro014.host_cxt/
$ rtcat -l hiro014:15005/co.rtc
co.rtc  Inactive
  Category       example
  Description    collisoin detector component
  Instance name  CollisionDetector0
  Type name      CollisionDetector
  Vendor         AIST
  Version        1.0
  Parent         
  Type           Monolithic
 +Extra properties
-Execution Context 0
  State  Running
  Kind   Periodic
  Rate   2000.0
  Owner  CollisionDetector0
-DataInPort: qRef
  dataport.data_type          IDL:RTC/TimedDoubleSeq:1.0
  dataport.dataflow_type      push,pull
  dataport.interface_type     corba_cdr
  dataport.subscription_type  Any
  port.port_type              DataInPort
-DataInPort: qCurrent
  dataport.data_type          IDL:RTC/TimedDoubleSeq:1.0
  dataport.dataflow_type      push,pull
  dataport.interface_type     corba_cdr
  dataport.subscription_type  Any
  port.port_type              DataInPort
-DataOutPort: q
  dataport.data_type          IDL:RTC/TimedDoubleSeq:1.0
  dataport.dataflow_type      push,pull
  dataport.interface_type     corba_cdr
  dataport.subscription_type  flush,new,periodic
  port.port_type              DataOutPort
-CorbaPort: CollisionDetectorService
  port.port_type  CorbaPort
  Interface:
    Instance name  service0
    Type name      CollisionDetectorService
    Polarity       Provided

となっていました。

pazeshun commented 8 years ago

Runningになっている気がするのですが、あっているのでしょうか？

k-okada commented 8 years ago

inactive なんじゃないかな．ここでactivateするとactivate状態になる．rtactというコマンドラインもある．http://www.openrtm.org/pub/OpenRTM-aist/tools/rtshell/3.0/ja/rtact.html

k-okada commented 8 years ago

activateするとはじめてonExecuteが毎周期うごく．コンポーネントってやつ．ROS2はこのスタイルになるので経験しておくのが大吉．

pazeshun commented 8 years ago

前の状態から、

$ rtcon hiro014:15005/RobotHardware0.rtc:q hiro014:15005/co.rtc:qCurrent
$ rtact hiro014:15005/co.rtc
$ rtcat -l hiro014:15005/co.rtc
rtcat: CORBA.BAD_PARAM(omniORB.BAD_PARAM_IndexOutOfRange, CORBA.COMPLETED_NO)
rtcat: Zombie object: hiro014:15005/co.rtc

となり、activeにするとコンポーネントがゾンビ化しました。それで、コンポーネントを実行してるQNXのターミナルを見てみたところ、

CollisionDetector0: onActivated(0)
ModelLoaderException :  cannot be found.
failed to load model[]
CollisionDetector0: onDeactivated(0)
Memory fault (core dumped)

ということで、モデルがなくてロードできず、死んでいたみたいです。

k-okada commented 8 years ago

/opt/jsk/etc/HIROINX/hrprtc/Robot.conf がちょんと読み込まれて，このRobot.confに model: ....main.wrl となっていればいいはずだけど，

pazeshun commented 8 years ago

/opt/jsk/etc/HIROINX/hrprtc/Robot.confにはmodel: /opt/jsk/etc/HIRONX/model/main.wrlとなっており、

$ pwd
/opt/jsk/etc/HIRONX/model
$ ls
BASE.wrl                               RWP.wrl
BASE_BASE.wrl                          RWR-00.wrl
BODY.wrl                               RWR-J0.wrl
CY.wrl                                 RWR-J1.wrl
DemoTray.wrl                           RWR-J2.wrl
DemoTray_Base.wrl                      RWR-J3.wrl
DemoTray_Tray.wrl                      RWY.wrl
LEP.wrl                                WR.wrl
LHANDCAM_01.wrl                        ball_blue.wrl
LRWR.wrl                               ball_orange.wrl
LSP.wrl                                dice.wrl
LSY.wrl                                hironx.dae
LWP.wrl                                hironx.robot.xml
LWR-00.wrl                             kawada-hironx.dae
LWR-J0.wrl                             main.wrl
LWR-J1.wrl                             main.wrl.bak20140222
LWR-J2.wrl                             main.wrl.bak20140707
LWR-J3.wrl                             main.wrl.bak20140819
LWY.wrl                                main.wrl~
MODEL_B.wrl                            main_dof15.wrl
NP.wrl                                 main_dof15_fs.wrl
NY.wrl                                 main_dof15_fs.wrl.bak20140819
REP.wrl                                main_dof23.wrl
RHANDCAM_01.wrl                        main_fs_hrpsys.wrl
RSP.wrl                                main_fs_ivplan.wrl
RSY.wrl

なので、読み込めないはずはないと思うのですが・・・。

k-okada commented 8 years ago 
 example.CollisionDetector.config_file://opt/jsk/etc/HIROINX/hrprtc/Robot.conf
 example.CollisionDetector.config_file:/opt/jsk/etc/HIROINX/hrprtc/Robot.conf

???

k-okada commented 8 years ago

rtcdRobotMode.conf で， example.CollisionDetector.config_file://opt/jsk/etc/HIROINX/hrprtc/Robot.conf に切り替えるてみてみるのかな．

pazeshun commented 8 years ago

rtcdRobotMode.confは、

corba.nameservers: localhost:15005
logger.enable: YES
logger.log_level: NORMAL
#logger.log_level: DEBUG
#logger.log_level: TRACE
#logger.log_level: VERBOSE
#logger.log_level: PARANOID
logger.file_name: stdout
exec_cxt.periodic.type: hrpExecutionContext
exec_cxt.periodic.rate: 200

manager.is_master: YES
manager.modules.load_path: /opt/jsk/lib
manager.modules.preload: hrpEC.so,RobotHardware.so
manager.components.precreate: RobotHardware

## All Robots
example.RobotHardware.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
example.SequencePlayer.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
example.StateHolder.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
example.ForwardKinematics.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
example.DataLogger.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
## Walking
example.AutoBalancer.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
example.Stabilizer.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
## Impedance
example.AbsoluteForceSensor.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
example.ImpedanceController.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
example.RemoveForceSensorLinkOffset.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robo
t.conf
## SoftErrorLimit
example.VirtualForceSensor.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
example.TorqueFilter.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
## SoftErrorLimit
example.KalmanFilter.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
example.SoftErrorLimiter.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
## Collision
example.CollisionDetector.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf
## Grasper
example.ServoController.config_file: /opt/jsk/etc/HIRONX/hrprtc/Robot.conf

となっているのですが、これらすべて書き換えでしょうか？

k-okada commented 8 years ago

コピペミス logger.log_level: VERBOSE にしてみようか．だめならPARANOIDで

pazeshun commented 8 years ago

コピペミスを修正しましたが、同じ症状が発生し、VERBOSEやPARANOIDにしても同じエラー表示しかされませんでした。今日はもう帰らないといけないので、一旦切り上げます。

k-okada commented 8 years ago

はい VERBOSEにしたらrtcdから大量にログが書かれているはずなので明日またlogファイルを確認してみてください

2016年9月7日水曜日、pazeshunnotifications@github.comさんは書きました:

コピペミスを修正しましたが、同じ症状が発生し、VERBOSEやPARANOIDにしても同じエラー表示しかさ れませんでした。今日はもう帰らないといけないので、一旦切り上げます。

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/start-jsk/rtmros_hironx/issues/423#issuecomment-245229665, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3IrP_s8vm2twLcxH99wj6YlQKSlBks5qnodigaJpZM4HKIzB .

◉ Kei Okada

emijah commented 8 years ago

@pazeshun 「様」は不要ですよ。：）

すでに解決しているところに茶々を入れてしまいますが、math.hはC用のヘッダなので、どちらかというとextern ”C"の中に入っていた方がいいかもしれませんね。c++のヘッダはmathなのですが、sin, cosなどが見えず、std::sin, std::cosが見えるように using std::sin; using std::cos; を追記する必要が出てくることが多々あります。

pazeshun commented 8 years ago

@emijah

math.hはC用のヘッダなので、どちらかというとextern ”C"の中に入っていた方がいいかもしれませんね

こうすると、前に上げたエラーが出てきます。下のリンク先と同じように、古いシステムゆえの問題だと思います。 http://www.sourceware.org/ml/gsl-discuss/2000/msg00224.html

pazeshun commented 8 years ago

@k-okada PARANOIDにして、ログを確認してみたのですが、CollisionDetector周りででているエラーについての新規情報はありませんでした。

Sep 13 06:06:40.236 PARANOID: SoftErrorLimiter0: on_state_update(1000)
Sep 13 06:06:40.236 PARANOID: SoftErrorLimiter0: onStateUpdate(1000)
Sep 13 06:06:40.236 TRACE: CollisionDetector0: on_activated(1000)
co: onActivated(1000)
Sep 13 06:06:40.236 TRACE: manager: Manager::getORB()
Sep 13 06:06:40.236 TRACE: CollisionDetector0: getProperties()
Sep 13 06:06:40.241 TRACE: hrpEC: get_component_state()
Sep 13 06:06:40.252 TRACE: hrpEC: get_component_state()
Sep 13 06:06:40.264 TRACE: hrpEC: get_component_state()
Sep 13 06:06:40.274 TRACE: hrpEC: get_component_state()
Sep 13 06:06:40.285 TRACE: hrpEC: get_component_state()
Memory fault (core dumped)
k-okada commented 8 years ago

https://github.com/fkanehiro/hrpsys-base/blob/315.1.10/rtc/CollisionDetector/CollisionDetector.cpp#L144 の直後に std::cerr << prop["dt"] << std::endl; みたいにしてみよう． できれば， (dolist (p prop) (print p)) してみよう．

◉ Kei Okada

On Tue, Sep 13, 2016 at 3:59 PM, pazeshun notifications@github.com wrote:

@k-okada https://github.com/k-okada PARANOIDにして、ログを確認してみたのですが、CollisionDetector周りででているエラーについての新規情報はありませんでした。

Sep 13 06:06:40.236 PARANOID: SoftErrorLimiter0: on_state_update(1000) Sep 13 06:06:40.236 PARANOID: SoftErrorLimiter0: onStateUpdate(1000) Sep 13 06:06:40.236 TRACE: CollisionDetector0: on_activated(1000) co: onActivated(1000) Sep 13 06:06:40.236 TRACE: manager: Manager::getORB() Sep 13 06:06:40.236 TRACE: CollisionDetector0: getProperties() Sep 13 06:06:40.241 TRACE: hrpEC: get_component_state() Sep 13 06:06:40.252 TRACE: hrpEC: get_component_state() Sep 13 06:06:40.264 TRACE: hrpEC: get_component_state() Sep 13 06:06:40.274 TRACE: hrpEC: get_component_state() Sep 13 06:06:40.285 TRACE: hrpEC: get_component_state() Memory fault (core dumped)

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/start-jsk/rtmros_hironx/issues/423#issuecomment-246592653, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3CZPhHwtjmLr5CF-ozhg8Q2UrR9Lks5qpknBgaJpZM4HKIzB .

pazeshun commented 6 years ago

詳細は次のコメントで述べますが、実機でcollision detectorが動くようになりました。

(追記) hironx_client.pyにhttps://github.com/start-jsk/rtmros_hironx/issues/423#issuecomment-175513814 と同じ編集をし、実機をつないで

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

とした後、robot.checkEncorders(), robot.startImpedance("rarm"), robot.startImpedance("larm")として、両腕をインピーダンス制御にします。 この状態で両腕を人間が動かし、衝突する姿勢に持ってこうとしても、衝突直前で腕が止まり、それ以上動かせなくなることを確認しました。 collision detectorが動いていないと、腕が衝突し、サーボが切れて胸の赤LEDが点滅するようになります。 (追記終わり)

まず、hironx_client.pyにhttps://github.com/start-jsk/rtmros_hironx/issues/423#issuecomment-175513814 と同じ編集をした上で、実機に接続してroslaunch hironx_ros_bridge hironx_ros_bridge_real.launch nameserver:=hiro014とジョイントキャリブを済ませます。 実機が左上の姿勢の時に、右上のエンドエフェクタが干渉する姿勢を指令すると、エンドエフェクタが実際に衝突する前に動きが止まるようになりました。 動画はhttps://drive.google.com/open?id=1PYSGffMFfJ8-qGJIbNIV5sfEw7chQCCv です。 また、停止直後にHiro内部のQNXでcat /opt/jsk/var/log/rtcd.logとして確認したログはhttps://drive.google.com/open?id=1q6B09Udz_zoHs3ODChFyvQiqD5ZWT5Pd です。 ログ最後の方は、

58/59 pair: RARM_JOINT5/LARM_JOINT5(13), distance = -5.51963e-09

となっており、collisionが検知されていることがわかります。

Euslispを使っている場合は、以下の方法で指令値を再現できます。

3.irteusgl$ send *hironx* :angle-vector #f(-0.5995 0.0 -100.0 15.1993 9.39961 3.18924 0.5995 0.0 -99.9995 -15.1993 9.39961 -3.19971 0.0 0.0 0.0)
4.irteusgl$ send *ri* :angle-vector (send *hironx* :angle-vector) 5000
5.irteusgl$ send *hironx* :rarm :inverse-kinematics (send *hironx* :larm :end-coords)
8.irteusgl$ progn (unix::sleep 10) (send *ri* :angle-vector (send *hironx* :angle-vector) 10000)
pazeshun commented 6 years ago

以前の作業は、https://github.com/start-jsk/rtmros_hironx/issues/423#issuecomment-245215405 の時点で止まっていましたが、このときモデルがロードできなかったのはtypoしていたからで、正しいコマンドは

RTCTREE_NAMESERVERS=hiro014:15005 SIMULATOR_NAME=RobotHardware0 ./CollisionDetectorComp -o corba.nameservers:hiro014:15005 -o naming.formats:co.rtc -o exec_cxt.periodic.type:PeriodicExecutionContext -o exec_cxt.periodic.rate:2000 -o logger.file_name:/tmp/collision_detector%p.log -o example.CollisionDetector.config_file:/opt/jsk/etc/HIRONX/hrprtc/Robot.conf

でした。 これをQNXの/opt/jsk/bin上で実行し、

Logger::Logger: streambuf address = 0x80bd938
Error: cannot open logfile: ./rtc4235301.log
CollisionDetector0: onInitialize()
Number of Ports: 4

Port0 (name): CollisionDetector0.qRef
---interfaces---
---properties---
port.port_type: DataInPort
dataport.data_type: IDL:RTC/TimedDoubleSeq:1.0
dataport.subscription_type: Any
dataport.dataflow_type: push,pull
dataport.interface_type: corba_cdr
----------------

Port1 (name): CollisionDetector0.qCurrent
---interfaces---
---properties---
port.port_type: DataInPort
dataport.data_type: IDL:RTC/TimedDoubleSeq:1.0
dataport.subscription_type: Any
dataport.dataflow_type: push,pull
dataport.interface_type: corba_cdr
----------------

Port2 (name): CollisionDetector0.q
---interfaces---
---properties---
port.port_type: DataOutPort
dataport.data_type: IDL:RTC/TimedDoubleSeq:1.0
dataport.subscription_type: flush,new,periodic
dataport.dataflow_type: push,pull
dataport.interface_type: corba_cdr
----------------

Port3 (name): CollisionDetector0.CollisionDetectorService
---interfaces---
I/F name: service0
I/F type: CollisionDetectorService
Polarity: PROVIDED
---properties---
port.port_type: CorbaPort
----------------

と表示された状態で、クライアントPC側からRTCを確認すると、

$ rtls hiro014:15005/
CollisionDetector0.rtc  ModelLoader  RobotHardware0.rtc  hiro014.host_cxt/

のようにCollisionDetector0.rtcが生成されているので、

$ rtcon hiro014:15005/RobotHardware0.rtc:q hiro014:15005/CollisionDetector0.rtc:qCurrent
$ rtact hiro014:15005/CollisionDetector0.rtc

としてactivateしてみると、QNX側で

CollisionDetector0: onActivated(0)
Memory fault (core dumped)

と表示され、モデルロードのエラーは出なくなりましたがやはりコアダンプしました。 print文デバッグの結果、https://github.com/fkanehiro/hrpsys-base/pull/1247 のような修正を加え、/opt/jsk/src/hrpsys-base-source-315.1.7で# make installすると、コアダンプしなくなりました。

後は、https://github.com/start-jsk/rtmros_hironx/blob/indigo-devel/hironx_ros_bridge/conf/conf.in にならって、/opt/jsk/etc/HIRONX/hrprtc/Robot.confにcollision checkするリンクのペアを以下のように定義し、

collision_pair: HEAD_JOINT0:RARM_JOINT2 HEAD_JOINT0:RARM_JOINT3 HEAD_JOINT0:RARM_JOINT4 HEAD_JOINT0:RARM_JOINT5 HEAD_JOINT0:LARM_JOINT2 HEAD_JOINT0:LARM_JOINT3 HEAD_JOINT0:LARM_JOINT4 HEAD_JOINT0:LARM_JOINT5 HEAD_JOINT1:RARM_JOINT2 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:LARM_JOINT2 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 CHEST_JOINT0:RARM_JOINT2 CHEST_JOINT0:RARM_JOINT3 CHEST_JOINT0:RARM_JOINT4 CHEST_JOINT0:RARM_JOINT5 CHEST_JOINT0:LARM_JOINT2 CHEST_JOINT0:LARM_JOINT3 CHEST_JOINT0:LARM_JOINT4 CHEST_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT1 RARM_JOINT0:LARM_JOINT2 RARM_JOINT0:LARM_JOINT3 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT1:LARM_JOINT0 RARM_JOINT1:LARM_JOINT1 RARM_JOINT1:LARM_JOINT2 RARM_JOINT1:LARM_JOINT3 RARM_JOINT1:LARM_JOINT4 RARM_JOINT1:LARM_JOINT5 RARM_JOINT2:LARM_JOINT0 RARM_JOINT2:LARM_JOINT1 RARM_JOINT2:LARM_JOINT2 RARM_JOINT2:LARM_JOINT3 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT3:LARM_JOINT0 RARM_JOINT3:LARM_JOINT1 RARM_JOINT3:LARM_JOINT2 RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT1 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT1 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5

make installすると上書かれるダミーのhrpIoを以下のように純正品に置き換えることで、実機がcollision check入りで動かせるようになりました。

# cp /opt/hiro/lib/libhrpIo.so /opt/jsk/lib/libhrpIo.so
pazeshun commented 6 years ago

/opt/jsk/src/hrpsys-base-source-315.1.7の現在のdiffは、https://drive.google.com/open?id=12RFBSlq2lDJ1NbaPKQUkl3wQSzzxfnBX のようになっています。 collision detectorを入れる作業を入れる前のdiffはhttps://drive.google.com/open?id=1dg-SLyRsRoKptBKoYSdzUgb4Owl1zqaa で、この二つの差が必要だった変更になります。

https://github.com/start-jsk/rtmros_hironx/issues/423#issuecomment-245195782 で言及したパッチと、https://github.com/start-jsk/rtmros_hironx/issues/423#issuecomment-245181418 、https://github.com/fkanehiro/hrpsys-base/pull/1247 で構成されています。

longjie commented 6 years ago

:+1: