dengdua

import rospy

from sensor_msgs.msg import JointState

from std_msgs.msg import Header

from trac_ik_python.trac_ik import IK

 

# 初始化 ROS 节点

rospy.init_node('dofbot_ik_node')

 

# 创建一个发布 JointState 的 ROS 发布器

joint_pub = rospy.Publisher('joint_states', JointState, queue_size=10)

 

# 创建 trac_ik 的对象

ik_solver = IK('base_link', 'arm_end_link', timeout=0.1)

 

# 设置目标位置

target_position = [1.0, 0.0, 1.0] # 以米为单位

 

# 获取当前机械臂关节角度

current_joint_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

 

# 使用 trac_ik 计算目标关节角度

joint_angles = ik_solver.get_ik(current_joint_angles, target_position[0], target_position[1], target_position[2], 0, 0, 0)

 

# 发布关节角度消息

joint_state = JointState()

joint_state.header = Header()

joint_state.header.stamp = rospy.Time.now()

joint_state.name = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']

joint_state.position = joint_angles

joint_pub.publish(joint_state)