Example of using TRAC_IK library to move the Fetch robot arm (without using MoveIt)

Question

I am having trouble using the TRAC_IK solver to move Fetch robot arm without using MoveIt. Previously I used MoveIt, which has all the examples and code I need to run. However, I found out that TRAC_IK is more accurate with Fetch robot.

Does anyone have any example of using TRAC_IK in moving the arm? Also, how to publish the joint states directly to the robot?

I have this code here, but it doesn't work.

#!/usr/bin/env python

import rospy
from trac_ik_python.trac_ik import IK
from sensor_msgs.msg import JointState

def calculate_joint_angles(ik_solver, pose):
    # Assuming the pose is [x, y, z, qx, qy, qz, qw]
    x, y, z, qx, qy, qz, qw = pose
    seed_state = [0] * ik_solver.number_of_joints
    return ik_solver.get_ik(seed_state, x, y, z, qx, qy, qz, qw)

if __name__ == '__main__':
    rospy.init_node("simple_disco")

    base_link = "base_link"
    end_link = "wrist_roll_joint"
    ik_solver = IK(base_link, end_link)

    # Publisher for joint states
    pub = rospy.Publisher('/joint_states', JointState, queue_size=10)

    joint_names = ["torso_lift_joint", "shoulder_pan_joint",
                   "shoulder_lift_joint", "upperarm_roll_joint",
                   "elbow_flex_joint", "forearm_roll_joint",
                   "wrist_flex_joint", "wrist_roll_joint"]

    # Define your end-effector poses here
    # These need to be defined as [x, y, z, qx, qy, qz, qw]
    disco_poses = [[0.5, 0.5, 0.5, 0, 0, 0, 1],
                   [0.5, 1, 0.5, 0, 0, 0, 1],
                   [0.5, 0.5, 0.5, 0, 0, 0, 1],
                   [0.5, 1, 0.5, 0, 0, 0, 1]]  # Define your poses here

    rate = rospy.Rate(1)  # Adjust the rate as needed

    for pose in disco_poses:
        if rospy.is_shutdown():
            break

        joint_angles = calculate_joint_angles(ik_solver, pose)

        if joint_angles:
            joint_state = JointState()
            joint_state.header.stamp = rospy.Time.now()
            # joint_state.name = ik_solver.joint_names
            joint_state.name = joint_names
            joint_state.position = joint_angles

            pub.publish(joint_state)
            rospy.loginfo("Moving to pose")
        else:
            rospy.logerr("No IK solution found for the given pose")

        rate.sleep()

    rospy.loginfo("Disco sequence complete")