ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Goal start doesnt match current pose error

asked 2018-10-02 11:42:26 -0500

renil93 gravatar image

updated 2018-10-03 00:30:30 -0500

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
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-10-11 01:51:22 -0500

mlautman gravatar image

Can you share your entire code? It looks like you are calling move_repeated but you haven't shared your code for that method.

If you read through the Move Group Python Interface tutorial you should get a good idea how to get the current state of the robot and command trajectories. If you are running with hardware you should replace the demo.launch call with the appropriate launch file to bring up Move Group with the appropriate hardware controllers.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2018-10-02 11:42:26 -0500

Seen: 348 times

Last updated: Oct 11 '18