ikpyは名前の通りPythonでikを解く時に使えるシンプルなIK実装です。
作り捨て、単純な挙動をつくるだけ、あるいは私のようにMoveItなんて高尚なものは使えない人におすすめ。
実際のところ
スクリプト
手先の方向は考えず、単純に三次元空間を解くだけのケースを考えます
""" IKPyを使用した7軸ロボットアームの逆運動学計算スクリプト このスクリプトは、URDFファイルから7軸ロボットアームのキネマティクスチェーンを作成し、 指定された目標位置に対して逆運動学(IK)を計算します。 主な機能: - URDFファイルからロボットアームのキネマティクスチェーンを構築 - 目標位置に対する逆運動学計算の実行 - 初期姿勢の設定と調整 - 計算結果の表示（ラジアンおよび度数法） - エラー発生時の現在位置確認 使用ライブラリ: - ikpy: 逆運動学計算 - numpy: 数値計算 """ import numpy as np from ikpy.chain import Chain import os # URDFからチェーンを作成 chain = Chain.from_urdf_file( os.path.abspath("./minimalURDF.urdf.xml"), base_elements=["arm_base_link"], # ベースリンクの名前を指定 active_links_mask=[False] + [True] * 7 # 最初のリンクはFalse、残りの7つをTrue ) # より近い目標位置に設定 target_position = np.array([0.4, 0.21, -0.38]) # メートル単位での目標位置 print("\n目標位置 [m]:") print(f"X: {target_position[0]:.3f}") print(f"Y: {target_position[1]:.3f}") print(f"Z: {target_position[2]:.3f}") # エンドエフェクタの目標姿勢を指定するパラメータ # None の場合は姿勢を考慮せず位置のみで IK を解く # 姿勢を指定する場合は回転行列(3x3)または四元数を指定可能 target_orientation = None if target_orientation is None: print("\n目標姿勢: 指定なし\n") # 初期角度を適切な値に設定 initial_position = [0] * len(chain.links) # 各関節の初期角度を中間位置に設定 initial_position[1] = 0.0 # joint1: (-0.785 to 0.785) initial_position[2] = 0.0 # joint2: (-2.094 to 2.094) initial_position[3] = 0.0 # joint3: (-3.001 to 3.001) initial_position[4] = 0.0 # joint4: (-3.001 to 3.001) initial_position[5] = 0.0 # joint5: (-3.001 to 3.001) initial_position[6] = 0.0 # joint6: (-3.001 to 3.001) initial_position[7] = 0.0 # joint7: (-6.197 to 6.197) try: # IKを解く joint_angles = chain.inverse_kinematics( target_position, target_orientation=target_orientation, # orientation_modeは以下の値を取ることができます: # "all": 全ての回転軸(X,Y,Z)の姿勢を考慮します # "X": X軸周りの回転のみを考慮します # "Y": Y軸周りの回転のみを考慮します # "Z": Z軸周りの回転のみを考慮します # None: 姿勢を考慮せず、位置のみでIKを解きます # 姿勢の制約を減らすことで解が見つかりやすくなりますが、 # エンドエフェクタの向きの制御が限定されます orientation_mode=None, initial_position=initial_position, # max_iter: 最大反復回数。大きいほど解の精度が上がる可能性があるが計算時間が増加する # 一般的な値: 100-10000。小さすぎると解が収束せず、大きすぎると計算時間が無駄に長くなる max_iter=1000 ) # 結果の表示 print("計算された関節角度（ラジアン => 度）:") degrees = np.degrees(joint_angles) for i, (rad, deg) in enumerate(zip(joint_angles, degrees)): rad_str = f"{rad:+.8f}" if rad != 0 else " 0.00000000" deg_str = f"{deg:+.2f}" if deg != 0 else " 0.00" print(f"Joint {i}: {rad_str} => {deg_str}°") except Exception as e: print(f"エラーが発生しました: {e}") # 現在の位置を確認 current_position = chain.forward_kinematics(initial_position) print("\n初期位置での手先位置:") print(current_position[:3, 3]) # 位置のみを表示
