Wrist 1 joint has issues wrapping/get stuck

[Steven Gray]

The wrist 1 joint has control issues that seem to be related to wraparound. It tends to get 'stuck' and then suddenly jerk, like here. This happens when traveling in both directions, shown here and here.

Wrist 2 and 3 don't seem to exhibit this behavior, as seen here. Shoulder pan and elbow joint are also free of this behavior.

I initially noticed that trajectory execution was consistently failing due to wrist 1 being unable to track.

[Steven Gray]

The angle range where it can't track doesn't affect reaching into the bins, but it make it impossible to reach into the boxes on the conveyor belt. The image shows the planned pose in orange and how far wrist joint 1 is from tracking that angle after executing the plan.

[Deanna Hood]

I'm having a hard time reproducing this @stgray. I can say that the controller parameters haven't changed from ARIAC 2017, and nobody has reported this before. This leads me to think that this is either unlikely to occur (perhaps your machine is faster or slower than most?) or that other teams have all managed to work around it. Generating your trajectories in smaller steps might help. There aren't any changes planned to the ARIAC software to modify the controller parameters.

[Steven Gray]

@d_hood Hmm, this is very odd. I am running ariac 2.0.5 and Gazebo 8.2 (to fix the logging playback issues in 8.3). I can reproduce this without running any of my code at all, just running the stock gear launch file.

roslaunch osrf_gear sample_environment.launch

Here I'll command the initial pose the arm starts up in except send wrist 1 joint to -2.0.

rostopic pub /ariac/arm/command trajectory_msgs/JointTrajectory \
"{joint_names: ['elbow_joint', 'linear_arm_actuator_joint', 'shoulder_lift_joint', \
'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], \
points: [ \
{time_from_start: {secs: 1}, positions: [2.14, 0.0, -2.0, 3.14, -2.0, -1.51, 0.0]} \
]}" -1

And then when I check the resulting joint angles after execution, I see that wrist 1 is at stuck near 0 at -4.7e-6.

rostopic echo /ariac/joint_states

yields

name: [elbow_joint, linear_arm_actuator_joint, shoulder_lift_joint, shoulder_pan_joint,
  wrist_1_joint, wrist_2_joint, wrist_3_joint, vacuum_gripper_joint]
position: [2.140669681251361, 1.4509577245737127e-06, -2.000047024080933, 3.1399998252943595, -4.7381833621074065e-06, -1.5099802746684983, 1.8830520218671154e-06, 0.0]

If I command wrist 1 to -6.0 instead, the joint state reads approximately 0.28, so it does seem like a possible wrapping issue. Though if I command +6.0, I get 6.0 from the joint states.

Do you see the same behavior at all?

[Steven Gray]

The workaround is just to restrict wrist 1 joint from [0, 2pi] or [-2pi, 0], but this feels like a possible joint controller bug.

[Attila Vidács]

@passo-sync I have similar problem with turning wrist_1 around. It just won't reach the desired destination. (restrict to [0,2pi] might help...)

[Deanna Hood]

I can reliably reproduce it with that trajectory message thanks. I have a fix proposed in https://bitbucket.org/osrf/ariac/pull-requests/83/optionally-use-joint-limited-ur10 if you're building from source and want to test it out. Add joint_limited_ur10: true to your config file. If you're using moveit, you might need to add limited:=true to your ur10_moveit_planning_execution invocation.

[Steven Gray]

@d_hood Thanks, passing that parameter through is a good solution.

[Deanna Hood]

excellent. I'm pushing out a new release now that will allow users to opt-in.

[Deanna Hood]

Anyone encountering this issue should see the release notes for ariac2.0.6 in https://bitbucket.org/osrf/ariac/wiki/2018/updates for how to enable the joint-limited ur10.