目標姿勢の精度が厳しくなった？ #45

y-yosuke · 2020-05-31T14:24:58Z

nextage_moveit_tutorial_poses.py にて姿勢を指定する下記リンクの行の姿勢で

tork_moveit_tutorial/script/nextage_moveit_tutorial_poses.py

Lines 19 to 22 in 3fff70b

    
           pose_target_1.orientation.x = 0.0 
        
           pose_target_1.orientation.y = -0.707 
        
           pose_target_1.orientation.z = 0.0 
        
           pose_target_1.orientation.w = 0.707

MoveIt Commander Python 利用側メッセージ

ABORTED: No motion plan found. No execution attempted.

MoveIt 側エラー

[ERROR] [1590934584.100473872, 37.869000000]: RRTConnect: Unable to sample any valid states for goal tree

となり解が得られないようになりました．
以前は問題なく動いていたように記憶しています．

アボートされる姿勢の記述のうち 0.707 の精度不足が問題のようで

    pose_target_1.orientation.x = 0.0
    pose_target_1.orientation.y = -math.sqrt(2.0) / 2.0
    pose_target_1.orientation.z = 0.0
    pose_target_1.orientation.w = math.sqrt(2.0) / 2.0

に変更したら解が得られて意図したとおりに動作しました．

単純に精度だけの問題でちゃんと計算すれば良いのでしたら該当するソースコードを修正してPRします．
もしくはそのような修正の他にもっと一般性の高い修正方法があればご教示いただけたらありがたいです．

The text was updated successfully, but these errors were encountered:

k-okada · 2020-06-01T02:15:53Z

これ，どのタイミングで動かなくなったでしょうか？前はkinetic で動いていたけど，melodicにしたら動かなくなった？あるいは前はmelodicで動いていたけど，最近動かなくなった？ moveit/moveit@bed7cc2 みたいな変更があったんじゃないか，と思うんですが，この辺は変更がなされ続けるものかとも思うので， https://github.com/ros-planning/moveit/blob/melodic-devel/moveit_commander/src/moveit_commander/move_group.py#L362 みたいな関数をつかってゴールを緩めに設定して動くのであれば，それが良いともいますプラス，この設定方法をチュートリアル内に書けば，結構重要な情報を伝えられるのではないかともいます．元の設定で実行ー＞エラーが出るー＞エラーの見方ー＞IKの制約をゆるくするー＞動いた．

…

-- ◉ Kei Okada 2020年5月31日(日) 23:25 Yosuke Yamamoto <[email protected]>:

nextage_moveit_tutorial_poses.py にて姿勢を指定する下記リンクの行の姿勢で https://github.com/tork-a/tork_moveit_tutorial/blob/3fff70bbe0fee59a384ef765a39bc2250cd66295/script/nextage_moveit_tutorial_poses.py#L19 MoveIt Commander Python 利用側メッセージ ABORTED: No motion plan found. No execution attempted. MoveIt 側エラー [ERROR] [1590934584.100473872, 37.869000000]: RRTConnect: Unable to sample any valid states for goal tree となり解が得られないようになりました．以前は問題なく動いていたように記憶しています．アボートされる姿勢の記述のうち 0.707 の精度不足が問題のようで pose_target_1.orientation.x = 0.0 pose_target_1.orientation.y = -0.707 pose_target_1.orientation.z = 0.0 pose_target_1.orientation.w = 0.707 の部分を pose_target_1.orientation.x = 0.0 pose_target_1.orientation.y = -math.sqrt(2.0) / 2.0 pose_target_1.orientation.z = 0.0 pose_target_1.orientation.w = math.sqrt(2.0) / 2.0 に変更したら解が得られて意図したとおりに解が得られて動作しました．単純に精度だけの問題でちゃんと計算すれば良いのでしたら該当するソースコードを修正してPRします．もしくはそのような修正の他にもっと一般性の高い修正方法があればご教示いただけたらありがたいです． — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#45>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AADYNXG76MIGDWE7JEUZOQDRUJSENANCNFSM4NPFZXRA> .

y-yosuke · 2020-06-01T03:47:52Z

本件，チュートリアルのとおり ROS Kinetic で行っています．
（ROS Melodic の Debian Packages に rtmros_hironx はありますが rtmros_nextage は無いようだったので）

2019年2月ぐらいにチュートリアルの Kinetic 化の作業をした時には従来のコードで動作を確認しているはずですが，その後は確認していないかもしれません．

set_goal_tolerance( 0.1 ) や set_goal_orientation_tolerance( 0.1 ) を設定して精度を緩くしてみましたが Abort されます．

Abort される，動作するの境目を桁数を変えて試してみました．

OK: pose_target_1.orientation.w = math.sqrt(2.0) / 2.0
OK: pose_target_1.orientation.w = 0.70710681187
OK: pose_target_1.orientation.w = 0.7071068118
OK: pose_target_1.orientation.w = 0.707106811
OK: pose_target_1.orientation.w = 0.70710681
NG: pose_target_1.orientation.w = 0.7071068
NG: pose_target_1.orientation.w = 0.707106
NG: pose_target_1.orientation.w = 0.70710
NG: pose_target_1.orientation.w = 0.7071
NG: pose_target_1.orientation.w = 0.707

Goal Tolerance とクォータニオンからIKの可計算性は同じものを意味しているのでしょうか？
別ならクォータニオンからIKの計算の段階で弾かれている可能性もあるかと思いました．

k-okada · 2020-06-01T06:36:39Z

あ，そうか．IKはhttps://github.com/tork-a/rtmros_nextage/blob/indigo-devel/nextage_ik_plugin/CHANGELOG.rst でikfast を使っているので，解ける解けない，ということはないかと思います．

ちなみに，rtmros_nextage をソースからワークスペースに入れて実行するとどうなります？

y-yosuke · 2020-06-01T06:45:56Z

ワークスペースに rtmros_nextage を入れてパスも確認して実行しても同じ挙動です．

OK: pose_target_1.orientation.w = math.sqrt(2.0) / 2.0
NG: pose_target_1.orientation.w = 0.707

y-yosuke · 2020-06-01T06:55:47Z

OK-NG 境界が1つずれました．

debian package rtmros_nextage

OK: pose_target_1.orientation.w = 0.70710681
NG: pose_target_1.orientation.w = 0.7071068
NG: pose_target_1.orientation.w = 0.707106

↓

build rtmros_nextage in workspace

OK: pose_target_1.orientation.w = 0.70710681
OK: pose_target_1.orientation.w = 0.7071068
NG: pose_target_1.orientation.w = 0.707106

Goal Tolerance を 0.1 に緩くするのが効かないのは同じです．

k-okada · 2020-06-01T06:56:53Z

あmelodicで，，という意味でした．

y-yosuke · 2020-06-01T06:57:34Z

melodic でやってみます．少し時間がかかると思いますが．

y-yosuke · 2020-06-01T07:25:46Z

ROS Melodic にて下記パッケージの組み合わせで，
元の nextage_moveit_tutorial_poses.py のまま正常に動作しました．

rtmros_hironx は debian package
rtmros_nextage と tork_moveit_tutorial はワークスペースに clone してビルド

ROS Kinetic でのみの問題ということでしょうか？

y-yosuke · 2020-06-01T07:49:51Z

同じような話題の Issue
MoveGroup sensitive to length of orientation values #1458
moveit/moveit#1458

y-yosuke · 2020-06-01T09:37:31Z

ROS Kinetic です．
当然といえば当然ですけど，normalize したら正常に動きます．

    import numpy
    q_orig = [ pose_target_1.orientation.x, pose_target_1.orientation.y, pose_target_1.orientation.z, pose_target_1.orientation.w ]
    q_norm = q_orig / numpy.linalg.norm( q_orig )
    pose_target_1.orientation.x = q_norm[0]
    pose_target_1.orientation.y = q_norm[1]
    pose_target_1.orientation.z = q_norm[2]
    pose_target_1.orientation.w = q_norm[3]

k-okada · 2020-06-01T09:47:36Z

なるほど．たしかにそうですね．
melodicでset_goal_tolerance は効きますでしょうか？

何れも

kinetic にx,y,z,wはnormalizeすることの注意書き．
melodic はこれに加えて，tolerance のセットが効くならその話を追加．

y-yosuke · 2020-06-01T10:26:52Z

ROS Melodic で set_goal_orientation_tolerance() が効いているか確かめました．
下記のような行を追加して（コード全体は下の方に掲載）tolerance の値をいじってみました．

    group.set_goal_orientation_tolerance( 1e-10 )
    rospy.loginfo( "Goal Orientation Tolerance: {}".format( group.get_goal_orientation_tolerance() ) )

ROS Melodic では既存のコードのままで動いている状態なので値をセットせずにget_goal_orientation_tolerance() で値を取得してみると 0.001 でした．

0.001 から数字を小さくしてセットすると 1e-10 までは意図したとおりに動作し，
1e-11 ですと 0.707 の方の姿勢（pose_target_1）だけ Abort され，
1e-12 より小さいと -1.0 の方の姿勢（pose_target_2）も Abort されました．

コード全体

#!/usr/bin/env python

from tork_moveit_tutorial import *


if __name__ == '__main__':
    
    init_node()
    
    group = MoveGroupCommander("right_arm")
    
    group.set_goal_orientation_tolerance( 1e-10 )
    rospy.loginfo( "Goal Orientation Tolerance: {}".format( group.get_goal_orientation_tolerance() ) )    

    # Pose Target 1
    rospy.loginfo( "Start Pose Target 1")
    pose_target_1 = Pose()
    
    pose_target_1.position.x = 0.4
    pose_target_1.position.y = -0.4
    pose_target_1.position.z = 0.15
    pose_target_1.orientation.x = 0.0
    pose_target_1.orientation.y = -0.707
    pose_target_1.orientation.z = 0.0
    pose_target_1.orientation.w = 0.707
    
    rospy.loginfo( "Set Target to Pose:\n{}".format( pose_target_1 ) )
    group.set_pose_target( pose_target_1 )
    group.go()
    
    # Pose Target 2
    rospy.loginfo( "Start Pose Target 2")
    pose_target_2 = Pose()
    
    pose_target_2.position.x = 0.3
    pose_target_2.position.y = -0.3
    pose_target_2.position.z = 0.5
    pose_target_2.orientation.y = -1.0
    
    rospy.loginfo( "Set Target to Pose:\n{}".format( pose_target_2 ) )
    group.set_pose_target( pose_target_2 )
    group.go()

y-yosuke · 2020-06-01T10:30:48Z

ROS Melodic の方は set_goal_orientation_tolerance() は効いているが，
1e-11 よりも小さな値にしないと Abort にならないので，
クオータニオンは正規化されているか，正規化しなくてもそれなりに計算をしているようです．

y-yosuke · 2020-06-01T10:32:17Z

反面 ROS Kinetic の方はなぜ精度良く正規化されていないクオータニオンが弾かれるのか，また過去には問題は出なかったのかが謎のままですが，対処法はわかりました．

y-yosuke · 2020-09-15T10:29:06Z

今のところ ROS の Kinetic と Melodic は調べて Kinetic は Orientatoin の正規化が明示的に必要で，Melodic は Pose を渡した先で都合をつけてくれているようで明示的には正規化は必要ないという結論でした．

全てのバージョンで明示的に正規化の手順を入れてしまえば済む話なのですが，せっかく Melodic では正規化は意識せずとも Orientation を指定できるのでもったいないような気がして Kinetic のみ正規化の手順を入れるようにするために https://answers.ros.org/question/335529/get-the-ros-version-installed-using-python-code/ などを参考に次の2つの関数を考えました．

def get_ros_version():
    
    new_proc = subprocess.Popen(["rosversion", "-d"], stdout=subprocess.PIPE)
    ros_version = new_proc.communicate()[0]
    
    return ros_version.rstrip('\n')
    

def normalize_orientation( pose ):
    
    q_orig = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]
    q_norm = q_orig / numpy.linalg.norm( q_orig )
    pose.orientation.x = q_norm[0]
    pose.orientation.y = q_norm[1]
    pose.orientation.z = q_norm[2]
    pose.orientation.w = q_norm[3]
    
    rospy.loginfo( "Orientation Normalized" )
    
    return pose

これらを使って Kinetic の場合のみ次のように明示的に正規化して，Kinetic でも問題なく動作生成がなされ実行することができました．

（前略）...
    ros_version = get_ros_version()
    if ros_version == 'kinetic':
        pose_target_larm_1 = normalize_orientation( pose_target_larm_1 )
...（後略）

このようにしてみて次のような少し引っかかる点がありました．

ROS のバージョン情報を取得する方法はこれで良いのか？
- rospy で標準的に API のような形で提供されていないのか？
ROS のバージョン情報に基づいて Python の実行制御をする方法はこれで良いのか？
- c++ の #ifdef のような感じで ROS のバージョンにより Python 構文の選択ができるのならそちらのほうが良い？？？

スマートな方法があればお教えいただけたらと思います．

534o · 2020-09-16T05:37:12Z

os.environ["ROS_DISTRO"]
rospy.get_param('/rosdistro')

でしょうか．
https://github.com/search?p=3&q=%22os.environ%5B%27ROS_DISTRO%5D%22&type=Code
でみると，jskのコードがでてくるから余り一般的ではないかもしれないけど．

y-yosuke · 2020-09-16T06:27:17Z

ありがとうございます．
試してみたところ

rospy.get_param('/rosdistro') では文字列の最後に \n が含まれていたので rstrip(' \n') が必要でした．

    ros_version = rospy.get_param('/rosdistro')
    ros_version = ros_version.rstrip('\n')

os.environ["ROS_DISTRO"] は末尾に \n はなく kinetic だけ入ってきたので一番シンプルで良いと思いました．

    ros_version = os.environ["ROS_DISTRO"]

k-okada · 2020-11-23T03:12:13Z

@y-yosuke お忙しいところすいません．この変更をしたpull request って出ていますでしょうか？

y-yosuke · 2020-11-23T03:32:05Z

ローカルPC上にあるままでブランチもプッシュしていないかもしれません．確認します． @k-okada

y-yosuke · 2020-11-23T03:59:17Z

すみません，まだスクリプトは Nextage の botharms しか正規化対応していませんが，ROS のバージョンチェックと正規化の関数は src/tork_moveit_tutorial/moveit_tutorial_tools.py に get_ros_version() と normalize_orientation( pose ) としてありますので各スクリプトから利用できる状態です．
とりあえずですが，プルリクエストは先ほど出しました．
#47

y-yosuke · 2020-11-23T05:01:43Z

中途半端なブランチから mod-kinetic-normalizing-orientation ブランチを作ってしまいました．
https://github.com/tork-a/tork_moveit_tutorial/network

y-yosuke mentioned this issue Jun 4, 2020

チュートリアル関連パッケージ ROS Melodic 対応状況 #46

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

目標姿勢の精度が厳しくなった？ #45

目標姿勢の精度が厳しくなった？ #45

y-yosuke commented May 31, 2020 •

edited

Loading

k-okada commented Jun 1, 2020 via email

y-yosuke commented Jun 1, 2020 •

edited

Loading

k-okada commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

y-yosuke commented Jun 1, 2020 •

edited

Loading

k-okada commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

k-okada commented Jun 1, 2020

y-yosuke commented Jun 1, 2020 •

edited

Loading

y-yosuke commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

y-yosuke commented Sep 15, 2020

534o commented Sep 16, 2020

y-yosuke commented Sep 16, 2020 •

edited

Loading

k-okada commented Nov 23, 2020

y-yosuke commented Nov 23, 2020

y-yosuke commented Nov 23, 2020 •

edited

Loading

y-yosuke commented Nov 23, 2020

目標姿勢の精度が厳しくなった？ #45

目標姿勢の精度が厳しくなった？ #45

Comments

y-yosuke commented May 31, 2020 • edited Loading

k-okada commented Jun 1, 2020 via email

y-yosuke commented Jun 1, 2020 • edited Loading

k-okada commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

y-yosuke commented Jun 1, 2020 • edited Loading

k-okada commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

k-okada commented Jun 1, 2020

y-yosuke commented Jun 1, 2020 • edited Loading

y-yosuke commented Jun 1, 2020

y-yosuke commented Jun 1, 2020

y-yosuke commented Sep 15, 2020

534o commented Sep 16, 2020

y-yosuke commented Sep 16, 2020 • edited Loading

k-okada commented Nov 23, 2020

y-yosuke commented Nov 23, 2020

y-yosuke commented Nov 23, 2020 • edited Loading

y-yosuke commented Nov 23, 2020

y-yosuke commented May 31, 2020 •

edited

Loading

y-yosuke commented Jun 1, 2020 •

edited

Loading

y-yosuke commented Jun 1, 2020 •

edited

Loading

y-yosuke commented Jun 1, 2020 •

edited

Loading

y-yosuke commented Sep 16, 2020 •

edited

Loading

y-yosuke commented Nov 23, 2020 •

edited

Loading