こんにちは。koraです。
前回の記事では MoveIt Setup Assistant を使って自作ロボットを MoveIt で動かせるようにセットアップをしました。今回は Python スクリプトから MoveIt を使って IK を解いてみたいと思います。
Move Groupについて
MoveIt Setup Assistant で生成される demo.launch を実行すると、MoveIt の機能を一通り試すことができます。このとき ROS ノードがいくつか起動されますが、その数はそんなに多くありません。
- 関節の状態を publish する joint_state_publisher
- MoveIt の中核となる move_group
- tf を publish する robot_state_publisher
- ROS のログを処理する rosout
- GUI を提供する Rviz
MoveIt の多様な機能は主に move_group と呼ばれるノードで実行されます。基本的には move_group が提供するインターフェースを介して MoveIt を利用するかたちになります。
MoveItでIKを解く方法
Python スクリプトを使って IK を解いてみます。
公式の Tutorial には Action を使って軌道計画を実行する例 (Move Group Python Interface) が掲載されています。しかし今回は IK のみ取得するので GetPositionIK Service を使います。
GetPositionIK Service を利用するスクリプト
スクリプト全体は次のようになります。
#!/usr/bin/env python # coding: utf-8 import sys import rospy from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import JointState from moveit_msgs.msg import Constraints from moveit_msgs.msg import RobotState from moveit_msgs.msg import PositionIKRequest from moveit_msgs.srv import GetPositionIK if __name__ == '__main__': # ROSノード初期化 rospy.init_node('test_get_position_ik') # JointStateを取得 joint_state = rospy.wait_for_message('joint_states', JointState) print(joint_state) # RobotStateを作成 robot_state = RobotState() robot_state.joint_state = joint_state pose_stamped = PoseStamped() pose_stamped.header.frame_id = 'base_link' pose_stamped.pose.position.x = 0.0 pose_stamped.pose.position.y = -0.1 pose_stamped.pose.position.z = -0.2 pose_stamped.pose.orientation.x = 0.0 pose_stamped.pose.orientation.y = 0.0 pose_stamped.pose.orientation.z = 0.0 pose_stamped.pose.orientation.w = 1.0 constraints = Constraints() try: request = PositionIKRequest() request.group_name = 'r_leg' request.robot_state = robot_state request.constraints = constraints request.pose_stamped = pose_stamped compute_ik = rospy.ServiceProxy('/compute_ik', GetPositionIK) response = compute_ik(request) print(response) except rospy.ServiceException as e: print("Servie call failed: {}".format(e))
スクリプトの解説
まずは必要なモジュールをインポートします。
import sys import rospy from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import JointState from moveit_msgs.msg import Constraints from moveit_msgs.msg import RobotState from moveit_msgs.msg import PositionIKRequest from moveit_msgs.srv import GetPositionIK
ROS ノードを初期化します。
# ROSノード初期化 rospy.init_node('test_get_position_ik')
sensor_msgs/JointState メッセージを購読し、現在の各関節の状態を取得します。これを元に moveit_msgs/RobotState メッセージを作成します。この
RobotState は後で move_group に送られ、IK 計算の初期状態として用いられます。
ROS トピックを購読するときはサブスクライバを利用するのが一般的ですが、一度だけしか購読しない場合は rospy.wait_for_message() を使うこともできます。これはサブスクライバもコールバックもコーディングせずに ROS メッセージを購読できる便利な機能です。
# JointStateを取得 joint_state = rospy.wait_for_message('joint_states', JointState) # RobotStateを作成 robot_state = RobotState() robot_state.joint_state = joint_state
足先の目標座標と目標姿勢を指定する geometry_msgs/PoseStamped メッセージを作成します。frame_id は座標系、position は目標座標 (単位は m)、orientation は目標姿勢 (クォータニオン)です。このロボットは原点が腰にあり、Y軸が左向き、Z軸が上向きの座標系なので、右足のY座標とZ座標は通常マイナスになります。
pose_stamped = PoseStamped() pose_stamped.header.frame_id = 'base_link' pose_stamped.pose.position.x = 0.0 pose_stamped.pose.position.y = -0.1 pose_stamped.pose.position.z = -0.2 pose_stamped.pose.orientation.x = 0.0 pose_stamped.pose.orientation.y = 0.0 pose_stamped.pose.orientation.z = 0.0 pose_stamped.pose.orientation.w = 1.0
IKを解く際に守るべき制約を設定します。moveit_msgs/Constraints メッセージによって、関節の可動範囲などを設定できます。ただし、この例では制約を設けないので空のメッセージとします。
constraints = Constraints()
GetPositionIK Service に送る moveit_msgs/PositionIKRequest を作ります。group_name には IK を解きたいロボットの部位を指定します。ここで入力する名前は MoveIt Setup Assistant で設定した Planning Group のいずれかです。robot_state、constraints、pose_stamped にはそれぞれ上記で作成した RobotState と PoseStamped と Constraints を代入します。
request = PositionIKRequest() request.group_name = 'r_leg' request.robot_state = robot_state request.constraints = constraints request.pose_stamped = pose_stamped
rospy.ServiceProxy() で GetPositionIK Service のハンドラを作成し、先程作成したリクエストを送ります。
compute_ik = rospy.ServiceProxy('/compute_ik', GetPositionIK) response = compute_ik(request) print(response)
実行結果
スクリプトを実行して以下のような出力が表示されたら成功です。足先を目標座標に移動させるための各関節の角度が得られます。
GetPositionIK Service のレスポンスの name と position に着目します。name と position の中身は一対一で対応しており、例えば l_hip_pitch は 0.0 rad、r_hip_pitch は -0.4181069243030325 rad を意味します。
solution: joint_state: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: "base_link" name: [l_hip_pitch, l_hip_roll, l_hip_yaw, l_knee, l_ankle_pitch, l_ankle_roll, l_shoulder_pitch, l_shoulder_roll, neck_yaw, neck_pitch, r_hip_pitch, r_hip_roll, r_hip_yaw, r_knee, r_ankle_pitch, r_ankle_roll, r_shoulder_pitch, r_shoulder_roll] position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4181069243030325, -0.22385451175099771, -0.09821914277099789, -1.0513461791257177, -0.6443788214130809, 0.24400628571072067, 0.0, 0.0] velocity: [] effort: [] multi_dof_joint_state: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: "base_link" joint_names: [] transforms: [] twist: [] wrench: [] attached_collision_objects: [] is_diff: False error_code: val: 1
なお、このスクリプトを実行しても Rviz に表示されている 3D モデルは特に変化しません。
次回
今回作成したスクリプトの応用で、両足の関節角度を求めることができるようになります。次はこれと歩行シミュレーションを組み合わせてモーションの作成にとりかかりたいと思います。