MoveItで自作ロボットの逆運動を計算しましょ
ROS+MoveItで検索しても最終的な出力がRvizに表示が多く，
関節角度を数字で求めている例が少なかったので執筆．
（ポートフォリオとして書くより有益な情報の共有を目的で書いた方が好き）
MoveItのインストール
基本的には公式サイト参照．
MoveIt Motion Planning Framework
自分は下記のコマンドでインストールしました．
sudo apt install ros-melodic-moveit
ロボットの作成
URDF形式のファイルでロボットのリンク構成を記述します．
MoveItの中でロボット定義に関してXacroとSRDFと言うファイルがあります．
URDFファイル
<robot name="masiro_arm"> ~~ <link name="base_link" /> <joint name="joint0" type="fixed"> <parent link="base_link" /> <child link="link0" /> <origin xyz="0 0 0" rpy="0 0 0" /> <limit lower="-1.5" upper="1.5" effort="0" velocity="0" /> </joint> <link name="link0"> <visual> <geometry> <box size="0.01 0.09 0.01" /> </geometry> <origin xyz="0 0.045 0" rpy="0 0 0" /> <material name="red" /> </visual> </link> ~~ </robot>
link -> joint -> link -> ...
といった感じに続いていく．作成したロボットアームはこんな感じ
色を全部同じにしたせいで分かりにくい．
と言うことで作るときは複色使いましょう，，，
- link
見た目を設定するvisual，慣性を設定するinertial，
衝突を設定するcollisionがあり，中心の位置をlink原点に対しての相対位置で設定できる．
linkの原点は一番根元にある．
公式のピクチャが分かりやすいかな．
urdf/XML/link - ROS Wiki - joint
type属性で固定，回転．直進を指定できる．
繋ぐlinkをparentとchildで指定して，
jointの原点位置を親linkの原点に対しての相対位置で設定する．（visualの中心位置ではないので注意!!）
limitで可動域や速度とかを設定する．
やっぱりこれも公式のピクチャが，，，
urdf/XML/joint - ROS Wiki
MoveIt Setup Assistant
MoveIt Setup Assistantを利用してURDFファイルから
MoveItで使用できるパッケージを作成する．
下記のコマンドでAssistantをインストール．
sudo apt-get install ros-melodic-franka-description
MoveIt Setup Assistant — moveit_tutorials Kinetic documentation
下記のコマンドで実行
roslaunch moveit_setup_assistant setup_assistant.launch
設定する項目は4箇所
- Self-Collisions
- Planning Groups
- ROS Control
Self-Collisionの設定
1. Self Collisionsを選択
2. Generate CollisionMatrixを選択
3. maxrix viewにすると分かりやすい！！
4. 今は全ての衝突(Collision)を無効にしている状態
前後のリンクの衝突を無視するとかにすればいいと思う．
Planning Groupsの設定
1. 適当な名前を設定 今回はarm
2. 運動学のソルバーを選択
KDLはおそらく下記のページの解法を用いているけど，まだ未学習．
LMA KinematicsはLM法を用いた解法．
3. ルートパスの生成アルゴリズムを選択
RRTという手法を用いて経路最適化を計算．
4. Groupの中で何がリンクかジョイントかなどを教示する．
上の写真ではベースとなるリンクと先端のリンクを教示しています．
ROS Control
ROSControllerについては詳しくは勉強できていませんが
赤枠の箇所をクリックしてControllerを作成します．
逆運動ノードの作成
ソースコードは以下のレポジトリーに
APIでは以下を参考にしてください．
planning_interface: moveit::planning_interface::MoveGroup Class Reference
エンドエフェクタの位置を設定または取得するプラグラム
# set position & orientation pose_goal = geometry_msgs.msg.Pose() pose_goal.position.x = 0.089973697223 pose_goal.position.y = 0.180020156677 pose_goal.position.z = -0.335890154759 pose_goal.orientation.x = 2.00719893294e-06 pose_goal.orientation.y = -0.25878908403 pose_goal.orientation.z = 6.86465394841e-05 pose_goal.orientation.w = 0.965933851395 move_group.set_pose_target(pose_goal) plan = move_group.go(wait=True) move_group.stop() move_group.clear_pose_targets() # get position & orientation print get_current_pose()
セットとゲットはこれかな
# set move_group.set_pose_target(pose_goal) # get move_group.clear_pose_targets()
ジョイントの角度を設定または取得するプラグラム
# set joint joint_goal = move_group.get_current_joint_values() joint_goal[0] = 0 joint_goal[1] = 0 joint_goal[2] = 0 joint_goal[3] = pi/3 joint_goal[4] = 0 joint_goal[5] = 0 move_group.go(joint_goal, wait=True) move_group.stop() # get joint print move_group.get_current_joint_values()
レポジトリーのmasiro_arm_configのdemo.launchを動かしながら，
別のターミナルでmasiro_kinematicのノードを動かすとRviz上のアームが動きます．
この姿勢をセットした後にジョイント角度を取得し，その角度をパブリッシュすればOK（まだ実装できてない）
