KUKAで与えられた座標にロボットを動かす

まずはシミュレータ上で動かす
１）RViz上で指定した位置に動かす
２）座標を指定して動かす（１点）
３）座標を指定して動かす（スクリプトを用いて連続した値を送り続ける：G-codeを模擬）

まずはRvizから。
逆運動学のエンジンにはtrack_ikを用いる。
まずrobot_state_publisherノードをrosparamに読み込む。
load_urdf.launch

<launch>
    <!-- ロボットのURDFファイルを読み込み -->
    <param name="robot_description" command="$(find xacro)/xacro $(find ik_test)/urdf/kr6.urdf" />

    <!-- robot_state_publisherノードを起動 -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
</launch>

逆運動学計算

#!/usr/bin/env python
import rospy
from trac_ik_python.trac_ik import IK
from geometry_msgs.msg import Pose

def main():
    rospy.init_node("trac_ik_urdf_example")

    # URDFのリンク名を指定 (URDFファイルに基づく)
    base_link = "base_link"
    end_effector_link = "link_6"

    # Trac-IKインスタンスの初期化
    ik_solver = IK(base_link, end_effector_link)

    # ターゲットの位置と姿勢を設定 (x, y, z, qx, qy, qz, qw)
    target_x, target_y, target_z = 0.5, 0.0, 0.3
    target_qx, target_qy, target_qz, target_qw = 0.0, 0.0, 0.0, 1.0

    # 逆運動学の計算
    initial_joint_angles = [0.0] * ik_solver.number_of_joints  # 初期関節角度
    joint_angles = ik_solver.get_ik(
        initial_joint_angles,
        target_x, target_y, target_z,
        target_qx, target_qy, target_qz, target_qw
    )

    a1 = joint_angles[0]
    a2 = joint_angles[1]
    a3 = joint_angles[2]
    a4 = joint_angles[3]
    a5 = joint_angles[4]
    a6 = joint_angles[5]


    # 計算結果の表示
    if joint_angles:
        rospy.loginfo("Inverse Kinematics Solution Found:")
        rospy.loginfo(joint_angles)
    else:
        rospy.logwarn("No IK solution found for the target pose.")

if __name__ == "__main__":
    main()

一応計算はできた。

上記が完了したら実機で検証する
実機での検証にはkuka-kvp-command-interface(
https://github.com/roboticsdojo/kuka_kvp_command_interface
)を用いる。