つづき
Roll 180°、それ以外0°として指定したいものの、このままオイラー角では使えません。
オイラー角を回転行列へ変換します。
実際のところ
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 ) # 目標位置の設定 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}") # Roll 180°の回転行列を作成 # Roll(X): 180°, Pitch(Y): 0°, Yaw(Z): 0° roll = np.radians(180) pitch = np.radians(0) yaw = np.radians(0) # 回転行列の計算 Rx = np.array([ [1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)] ]) Ry = np.array([ [np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)] ]) Rz = np.array([ [np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1] ]) # 合成回転行列の計算 (順番: Z → Y → X) target_orientation = Rx @ Ry @ Rz print("\n目標姿勢（回転行列）:") print(target_orientation) # 初期角度の設定 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", # 全ての回転軸の姿勢を考慮 initial_position=initial_position, max_iter=1000 ) # 結果の表示 print("\n計算された関節角度（ラジアン => 度）:") 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}°") # 順運動学で最終的な位置と姿勢を確認 final_frame = chain.forward_kinematics(joint_angles) print("\n最終的なエンドエフェクタの位置:") print(f"X: {final_frame[0, 3]:.3f}") print(f"Y: {final_frame[1, 3]:.3f}") print(f"Z: {final_frame[2, 3]:.3f}") print("\n最終的な姿勢行列:") print(final_frame[:3, :3]) except Exception as e: print(f"エラーが発生しました: {e}") # 現在の位置を確認 current_position = chain.forward_kinematics(initial_position) print("\n初期位置での手先位置:") print(current_position[:3, 3])
結果
空間系を指定しない（前回のもの）
計算された関節角度（ラジアン => 度）: Joint 0: 0.00000000 => 0.00° Joint 1: +0.16759269 => +9.60° Joint 2: -0.34032077 => -19.50° Joint 3: +0.28041829 => +16.07° Joint 4: -0.81002203 => -46.41° Joint 5: 0.00000000 => 0.00° Joint 6: 0.00000000 => 0.00° Joint 7: 0.00000000 => 0.00°
空間系を指定する（今回）
計算された関節角度（ラジアン => 度）: Joint 0: 0.00000000 => 0.00° Joint 1: -0.75274636 => -43.13° Joint 2: -0.49511530 => -28.37° Joint 3: +1.01860571 => +58.36° Joint 4: +1.17755105 => +67.47° Joint 5: +0.51352504 => +29.42° Joint 6: +2.17411863 => +124.57° Joint 7: +2.02237533 => +115.87°
