ブログ二足歩行ロボット研修二足歩行ロボット研修（kora編）
MoveItROS

二足歩行ロボット研修（kora編）[28] MoveItを使ってIKを解く方法について

ブログ

こんにちは。koraです。

前回の記事では MoveIt Setup Assistant を使って自作ロボットを MoveIt で動かせるようにセットアップをしました。今回は Python スクリプトから MoveIt を使って IK を解いてみたいと思います。

Move Groupについて

MoveIt Setup Assistant で生成される demo.launch を実行すると、MoveIt の機能を一通り試すことができます。このとき ROS ノードがいくつか起動されますが、その数はそんなに多くありません。

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 モデルは特に変化しません。

次回

今回作成したスクリプトの応用で、両足の関節角度を求めることができるようになります。次はこれと歩行シミュレーションを組み合わせてモーションの作成にとりかかりたいと思います。

タイトルとURLをコピーしました