Move Group Python Interface: go_to_joint_state() error

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.

edit retag close merge delete

1

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.

this is most likely a visualisation problem: have you unchecked the Loop Animation checkbox in the Planned Path part of the MotionPlanning display?

Things never signalling completion sounds like joint states of your robot are not coming through, so MoveIt cannot determine the motion has been successful.

( 2020-04-30 13:06:50 -0500 )edit

Thank you, that did stop the looping animation.

Ok, when I do a rostopic list, I see both the move_group/joint_states topic and the /joint_states topic. When I rostopic echo /joint_states and run go_to_joint_state() part of the code, the values representing the change of position do not change. So yes, it would appear that my joint_states are not coming through.

I am currently at a loss on how to make sure they are received, but I will keep working on it.

( 2020-05-01 05:23:07 -0500 )edit

Update:

Using rostopic echo joint_states and rosrun tf tf_echo I can now see the positions changing, when they definitely were not before. The arm is now moving to the correct position, and unfortunately I am unsure of what I have done to get it to do that, so I apologize to everyone else trying to get through this.

My issue now is that time it takes for the arm to get to the desired location is a bit unreasonable (about 2 minutes in which you can see the arm slowly moving towards its goal. Does anyone have a fix for the rate in which it would move?

Is this a IK plugin deal? or just a parameter I need to adust somewhere?

Thanks friends

EDIT:

This can be changed in the joint_limits.yaml file in the <your>_moveit_config package.

( 2020-05-01 06:15:49 -0500 )edit
1

Seems like you've figured things out.

re: joint limits: you'll want to make sure your .urdf or .xacro contain the correct limits. config/joint_limits.yaml in your MoveIt config is really only meant to be used to override whatever is present in your .urdf or .xacro. Except acceleration and jerk limits, as those cannot be encoded in a URDF right now.

For MoveIt specifically you'll want to make sure to have proper velocity and acceleration limits, as otherwise it will assume 1 rad/s and 1 rad/s^2 for all your joints. Depending on your limits this can certainly result in some pretty slow motions.

( 2020-05-01 06:52:09 -0500 )edit

Thanks again.

( 2020-05-01 08:05:30 -0500 )edit