def execute_movement(position_sequence, delay_sequence):
    if len(position_sequence) != len(delay_sequence):
        rospy.logerr("KATANA MOVEMENT ERROR")
    jmag_msg = jmag()
    for index, position in enumerate(position_sequence):
        jmag_msg.header.stamp = rospy.Time.now()
        jmag_msg.goal_id.stamp = rospy.Time.now()
        jmag_msg.goal.jointGoal.name=joint_names
        jmag_msg.goal_id.id = 'Katana Command - ' + str(jmag_msg.header.stamp.secs) + ' . ' + str(jmag_msg.header.stamp.nsecs)
        jmag_msg.goal.jointGoal.position=position
        #rospy.loginfo(jmag_msg)
        #rospy.loginfo("Rate : " + str(1/delay_sequence[index]))
        pub.publish(jmag_msg)
        rate = rospy.Rate(1.0/delay_sequence[index])
        rate.sleep()
def katana_ctrl():
    pub = rospy.Publisher('/katana_arm_controller/joint_movement_action/goal', jmag, queue_size=10)
    rospy.init_node('katana_arm_our_controler', anonymous=True)
    jmag_msg = jmag()
    jmag_msg.header.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
    jmag_msg.goal_id.stamp = rospy.Time.now()
    jmag_msg.goal_id.id = 'SimpleKatana test - ' + str(jmag_msg.header.stamp.secs) + ' . ' + str(jmag_msg.header.stamp.nsecs)
    jmag_msg.goal.jointGoal.name=joint_names
    jmag_msg.goal.jointGoal.position = [-1.85,1.57,0.175,-0.35,-1.57,0.175,0.175]

    rate = rospy.Rate(0.9)
    rate.sleep()
    while not rospy.is_shutdown():
        jmag_msg.header.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
        jmag_msg.goal_id.stamp = rospy.Time.now()
        jmag_msg.goal_id.id = 'SimpleKatana test - ' + str(jmag_msg.header.stamp.secs) + ' . ' + str(jmag_msg.header.stamp.nsecs)
        jmag_msg.goal.jointGoal.position = [jmag_msg.goal.jointGoal.position[0],jmag_msg.goal.jointGoal.position[1],-jmag_msg.goal.jointGoal.position[2],-jmag_msg.goal.jointGoal.position[3],jmag_msg.goal.jointGoal.position[4],jmag_msg.goal.jointGoal.position[5],jmag_msg.goal.jointGoal.position[6]]
        rospy.loginfo(jmag_msg)
        pub.publish(jmag_msg)
        rate.sleep()