Move Group Python Interface: go_to_joint_state() error

edit

Kinetic Ubuntu 16.04

After following the Move Group Python Interface tutorial (http://docs.ros.org/kinetic/api/movei...) with the panda arm, I attempted to modify the code for my custom robot.

I changed the following code from the original panda code:

def go_to_joint_state(self):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group

## BEGIN_SUB_TUTORIAL plan_to_joint_state
##
## Planning to a Joint Goal
## ^^^^^^^^^^^^^^^^^^^^^^^^
## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first
## thing we want to do is move it to a slightly better configuration.
# We can get the joint values from the group and adjust some of the values:
joint_goal = group.get_current_joint_values()
print "before assignment"
joint_goal[0] = 0
joint_goal[1] = -pi/4
joint_goal[2] = 0
joint_goal[3] = -pi/2
#joint_goal[4] = 0
#joint_goal[5] = pi/3
#joint_goal[6] = 0

# The go command can be called with joint values, poses, or without any

# parameters if you have already set the pose or joint target for the group
print "after assignment"
group.go(joint_goal, wait=True)
print "waiting"
# Calling ``stop()`` ensures that there is no residual movement
group.stop()
print "stopping"
## END_SUB_TUTORIAL

# For testing:
# Note that since this section of code will not be included in the tutorials
# we use the class variable rather than the copied state variable
current_joints = self.group.get_current_joint_values()
return all_close(joint_goal, current_joints, 0.01)

Some Notes:

-joint_goal[4]-joint_goal[6] are commented out because my robot is only 4 DOF and it was throwing an "out of range error" when they were included.

-the "print 'before assignment'", "print 'after assignment'", "print 'waiting'", and "print 'stopping"' commands were just for tracking.

Expected:

Arm will move from start position (straight up) to joint position prescribed by joint_goal[0]-joint_goal[3] and stop. It will then continue to the "go_to_pose_goal()" part of the script. This motion is exactly what the panda_arm was doing.

Actual Result:

Arm moves from start position (straight up) to joint position prescribed by joint_goal[0]-joint_goal[3], BUT immediately resets to the start position and repeats the motion over and over.

"before assignment" and "after assignment" both print but nothing else. This eventually times out, and it moves to the "go_to_pose_goal()" part of the script (where it complains about no motion plan, one problem at a time Matt!)

From my understanding, "group.go(joint_goals,wait=True)" is sending those joint_goals, but it seems to continue to "wait" and never be "True" until ultimately it times out.

If someone has a moment and some wisdom, I would greatly appreciate it.