pose_based_cartesian_traj_controller causes robot to flip around and sometimes hit joint limits

[cbteeple]

Summary

When using the pose_based_cartesian_traj_controller on my UR5e, I cannot get deterministic behavior. It seems like every other time I run a trajectory, the robot decides to flip the arm around between some waypoints even though the motion I am commanding would normally result in obvious smooth trajectories.

Versions

• ROS Driver version: dab6cf7
• Affected Robot Software Version(s): PolyScope 5.3.1
• Affected Robot Hardware Version(s): UR5e
• Robot Serial Number:
• UR+ product(s) installed:
• URCaps Software version(s):

Impact

When I try to run simple trajectories such as pick an place operations, the arm will sometimes decdie to try and flip the end effector 360 degrees about arbitrary axes. This happens differently for the exact same trajectory (I am sending orientation using quaternians directly). It seems like the robot's builtin inverse kinematics doesn't try to keep the end effector orientation close to its current state, so it ends up acting wild.

Additionally, when the robot flips around, sometimes it runs itself into joint limits, or moves quickly to "catch up" in the other configuration. This almost always causes a velocity or force e-stop when it whips the end effector around.

Issue details

This issue makes the pose_based_cartesian_traj_controller unusable for me since I cannot reliably command end effector poses without this crazy flipping behavior.

Use Case and Setup

• Simple pick and place operations
• Grasp approaches

Project status at point of discovered

When did you first observe the issue?

• In first couple of tries

Steps to Reproduce

1. Send a cartesian_control_msgs/FollowCartesianTrajectory message to the arm with a few waypoints in it.
2. The arm will decide to flip around all of the place.

Expected Behavior

I expect the arm to move from waypoint to waypoint with the shortest path in Cartesian space.

Actual Behavior

Sometimes trajectories actually complete with what seem like intelligent robot behavior (configuration doesn't flip around), but most of the time flipping occurs. This happens sometimes even when the motion is relatively small (2 cm, 10 degrees, etc.)

Workaround Suggestion

???

[cbteeple]

Another observation: It seems to happen the most when I command an end-effector angle that is not vertical. For example, moving from vertical to 10 degrees off-vertical in any direction causes the arm to try and rotate 350 degrees the other way rather than 10 degrees in the correct direction.

[cbteeple]

My current workaround is to interpolate trajectories in Cartesian space with a timestep of 0.01 sec. This works alright, but requires me to calculate my own acceleration curves and whatnot. This does seem to reliably prevent the robot from flipping between configurations though.

[stefanscherzinger]

@cbteeple Thanks for reporting this and for taking the time to fill the issue template!

I'm not sure at the moment what could be causing the problem.

even though the motion I am commanding would normally result in obvious smooth trajectories.

Although it's not a check for inverse kinematics, could you confirm with this mechanism that the motion as described by your trajectories is what you expect in Cartesian space?

If possible, could you provide a scenario (joint configuration and goal trajectory) in which this happens reproducible? That could give a baseline for studying if it's a classic singularity problem or really some controller deficiencies.

It would also be interesting to check whether the joint_based_cartesian_traj_controller presents similar behavior. That controller uses the same Cartesian space interpolation but solves inverse kinematics (accessible to us) on the ROS-side before sending joint commands to the robot.

@t-schnell : Just a wild guess, but could that be related to some blending mechanisms?

[Hubert51]

I have a similar problem. When I move tool place to a vertical pose, the strange movement will happen.
The following joint angles are starting state and target state.(expected a small tilted motion but get a strange movement)

q1 = [-101.53, -158.36, -87.37, -127.62, -97.45, 89.21]
q2 = [-103.05, -156.68, -101.28, -102.94, -99.30, 91.39]

I am using cartesian_control_msgs/FollowCartesianTrajectory(joint_based_cartesian_traj_controller) to send command. So my actual sending data are two poses. I get above two joints states from teach pendant

start_pose.position = [-0.33, -0.739, -0.257]
start_pose.orientation = [0.533, -0.575, 0.428, 0.450]

end_pose.position =  [-0.33, -0.739, -0.257]
end_pose.orientation = [0.482, -0.524, 0.487, 0.505]

I also tried to move robot through teach pendant and pos_joint_traj_controller, both of them worked good.

I can try to use pose_based_cartesian_traj_controller and give you result soon.

For the robot, I am using UR10e, with Ubuntu18.04 UR ROS driver

[hidalgoem]

@stefanscherzinger
@fmauch
We are using forward_cartesian_traj_controller to control the UR5e robot. The movement is linear as expected, however, when changing direction from one point to another or at the end of the trajectory it is creating jerks. Is there any way to limit the jerks? controlling acceleration/deceleration will help to overcome this issue I guess. But we don't know how to control the acceleration/deceleration? are there any ways to control that? maybe in ur5e_controllers.yaml or in joint_limits.yaml?

[Hubert51]

For my experience, you can split the one big pose_array into two small pose_array. So it will reach the goal, and then stop, and then change the direction. This method can avoid jerks.

[urmahp]

I have been looking a bit into this issue and the problem seems to be in the way the orientation is interpolated in the cartesian trajectory controller, since each dimension of the quaternion is interpolated on its own. When doing the interpolation this way I observe the strange behavior when the waypoints in the trajectory aren't placed close together, but if the waypoints are placed very close together I don't observe this strange behavior.

I then tried using slerp instead for interpolating the orientation between two waypoints that would result in this strange behavior with the original interpolation method. When using slerp the result was a smooth movement from the one orientation to the other. So, I suspect that issue has something to do with the way the interpolation is done.

Also, I tested it with both the joint_based_cartesian_traj_controller and the pose_based_cartesian_traj_controller and the issue happens in both controllers which could indicate that the issue doesn't have something to do with the inverse kinematics calculation.

[urmahp]

I have added a PR in the cartesian trajectory controller, which should fix this issue. You are more than welcome to test it.

[hidalgoem]

@Hubert51 we split into two small pose_arrays and it seems to work. Thank you

[sebas775]

I have the same problem, when I send Cartesian positions to the robot, it usually makes strange and dangerous movements to reach a nearby point that is easy to traverse.
Robot ur3e

[stefanscherzinger]

@sebas775
One of possible reasons was a sign flip with quaternion interpolation. That has been fixed here. I'm currently not sure if that has been released upstream, though. Could you build this branch from source and check whether the issues persist?

Other than that, this might be a classic problem of singularities that are not directly intuitive. Additionally, you could visualize your commanded motion and check if that's what you expect.

[sebas775]

@stefanscherzinger It worked for me, one more reference for my work, thanks.

[fmauch]

I've just made a release containing the fixes mentioned above, so I will close this issue.