Goal start doesnt match current pose error

edit

Hi, I am new to ROS. My UR5 hit something while executing test_move.py from ur_modern driver. After that, this error pops up all the time when I try to run the same code test_move.py. "Goal start doesnt match current pose". How do I resolve this ? If reinitialize the joints needed, please tell me how to do that ?

I went through this link ( https://github.com/ros-industrial/ur_... ) and found out: Whenever you compute a trajectory, do the following step:

• Compute IK of your target.
• build your trajectory
• wait for a message on /joint_states
• update starting position based on the new value from /joint_state
• send your trajectory to the driver

Will this give me the solution. But how do i add initial state to my trajectory code ? Below is test_move.py code which i am running and getting the error:

def move1():
global joints_pos
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
try:
    joint_states = rospy.wait_for_message("joint_states", JointState)
    joints_pos = joint_states.position
    g.trajectory.points = [
        JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
        JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
        JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
        JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
    client.send_goal(g)
    client.wait_for_result()
except KeyboardInterrupt:
    client.cancel_goal()
    raise
except:
    raise

def main():
global client
try:
    rospy.init_node("test_move", anonymous=True, disable_signals=True)
    client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
    print "Waiting for server..."
    client.wait_for_server()
    print "Connected to server"
    parameters = rospy.get_param(None)
    index = str(parameters).find('prefix')
    if (index > 0):
        prefix = str(parameters)[index+len("prefix': '"):(index+len("prefix': '")+str(parameters)[index+len("prefix': '"):-1].find("'"))]
        for i, name in enumerate(JOINT_NAMES):
            JOINT_NAMES[i] = prefix + name
    print "This program makes the robot move between the following three poses:"
    print str([Q1[i]*180./pi for i in xrange(0,6)])
    print str([Q2[i]*180./pi for i in xrange(0,6)])
    print str([Q3[i]*180./pi for i in xrange(0,6)])
    print "Please make sure that your robot can move freely between these poses before proceeding!"
    inp = raw_input("Continue? y/n: ")[0]
    if (inp == 'y'):
        #move1()
        move_repeated()
        #move_disordered()
        #move_interrupt()
    else:
        print "Halting program"
except KeyboardInterrupt:
    rospy.signal_shutdown("KeyboardInterrupt")
    raise