[jog_arm] cartesian jog one linear axis adds rotation as well

[rfeistenauer]

Description

By Following the tutorial of jog_arm with current moveit master branch (ad88e18ae604e0533) and then publishing twists with 1 and -1 in y axis, the robot gets high angular twists as well. Depending on the chosen robot (ur like in tutorial or prbt) there are different rotations in size and magnitude.

for example, achieved with jogging only linear y axis!

Your environment

• ROS Distro: Melodic
• OS Version: Ubuntu 18.04
• Source build
• ad88e18ae604e0533

Steps to reproduce

1. Run the steps of the ReadMe
2. Move to a position that is not near a singularity.
3. Send delta_jogs for a linear axis for example by using a script like the following:

   delta_jog_cmds_publisher = rospy.Publisher("jog_server/delta_jog_cmds", TwistStamped, queue_size=10)
   rospy.sleep(1)
   for i in range(10):
       for p in range(100):
           delta_jog_cmds_publisher.publish(TwistStamped(header=Header(stamp=rospy.Time.now()),
                                                         twist=Twist(linear=Vector3(y=1))))
           rospy.sleep(.01)
       for m in range(100):
           delta_jog_cmds_publisher.publish(TwistStamped(header=Header(stamp=rospy.Time.now()),
                                                         twist=Twist(linear=Vector3(y=-1))))
           rospy.sleep(.01)

Expected behaviour

robot moves on y axis and ends somewhere near the starting position, without rotating the tcp

Actual behaviour

unexpected rotations and movements increasing over the time

Comment

Its hard to believe, a bug like this has gone unnoticed, so if you can not reproduce this, please tell me. I tested it on a second PC with newly setup environment, still maybe there is something that i missed here.

[AndyZe]

I don't see this behavior :/

You haven't changed anything in the yaml file, right?

Can you try publishing via command line instead of with your Python script?

rostopic pub -r 100 /jog_server/delta_jog_cmds geometry_msgs/TwistStamped "header: auto twist: linear: x: 0.0 y: -0.1 z: 0 angular: x: 0.0 y: 0.0 z: 0.0"

Also, you're asking for a 1m/s velocity. That's a very fast ramp from 0 to 1 m/s, it might be part of the problem. It should work in an ideal world but the low-pass filters are screwing with it.

[rfeistenauer]

retried with head master and your proposed msg, repeating that one (y: -0.1) and with y: 0.1 shows the same result. In fact my script was only meant to clarify, what i did.

moveit: acc94505b23cb28 universal_robot: c8c27c15579fa

You haven't changed anything in the yaml file, right?

correct

In fact i noticed that the main jitter appears to be on the change from - to + and when enclosing on singularities.

[rfeistenauer]

Just wanted to state, that I still can reproduce this with the newest melodic-devel branch
commit: ff552bf861609f99ca97a7e173fcbeb0 just repeated your msg with positive and negative y value. As soon as the arm comes close to the outer arm extend it starts adding unwanted transition or rotation

[AdamPettinger]

I pulled the ff552bf commit from the most recent comment and have a few comments.

I was unable to reproduce the exact problem shown. This issue was opened a few months before #2151 which fixed a bug where the rotation about the z-axis was always considered a "drift dimension". Thus, I believe that could have led to the image above (deviation only appears to be around the z-axis).

Regardless, I did get weird and unexpected behavior while doing this. I launched per the README and used rqt to publish messages, and DID see the manipulator deviating from the command. See attached.

A few thoughts for causes:

1. @AndyZe mentioned the low pass filters could be un-linearizing the output
2. Something in the acceleration limits (since removed) or velocity limits could be scaling. Unlikely I think, as those scale the joint velocities together (linearly), see #2471
3. The robot gets close enough to singularity so the Jacobian inversion isn't good but the velocity limits are working well to stop the robot from spazzing and clearly misbehaving.
4. The sim uses position controllers and updates on the current position: over time the arm can "droop" with gravity pulling the joints slightly away from the target then the target getting updated to match the new positions each cycle. I have seen this before, and could explain why it is more pronounced at full arm reach. Not really sure it could account for the dramatic onset of deviation in the video

[AndyZe]

@AdamPettinger if it is a slow sag due to gravity, I would suggest pausing Servo when possible. servo_-setPaused(true);