search

RobotLocomotion / drake-iiwa-driver

BSD 3-Clause "New" or "Revised" License
24 stars 30 forks source link

Unexpected violent jerk during motion #38

Closed y8han closed 3 years ago

y8han commented 3 years ago

Hi, when I was running kuka_driver.cc and send the goal pose info via lcm, the robot arm (Kuka IIWA) may sometimes return to zero position in a violent jerk. After I went deep into codes, it seems to me that the reason is something wrong with the fri communication. I saw a flag (restart_fri) and if I set it to be true, the robot will first return to zero position in a jerk and then come back in another jerk... Does someone observe the same thing happens? Is it possible that we solve the fri communication issue, or at least when it breaks, the robot arm keeps still? Thanks in advance for any suggestions.

sammy-tri commented 3 years ago

From email, my first guess is that there are multiple controllers sending IIWA_COMMAND messages to the driver, rather than it being an issue on the FRI side.

y8han commented 3 years ago

Hi, I think when I was manipulating the robot arm, there was indeed only one PC sending IIWA_COMMAND. Also, I prined out the message on terminal when this happened: Re-entering command state. Holding position and ignoring LCM commands. Do you have any ideas? Thanks in advance.

sammy-tri commented 3 years ago

So, it sounds like the move to zero positions is associated with the FRI link to the robot having issues (and then recovering). I'd recommend not using --restart_fri (which it looks like you weren't in this most recent report) as it sounds like that causes the robot to go back to the old position (after zeroing!), which is probably even worse.

Here are some ideas we can try to attempt to narrow things down...

I'll let you know if I think of anything else.

y8han commented 3 years ago

Hi Sam, a quick update: I add exit(EXIT_FAILURE) after line 505. The program was indeed terminated but the robot arm still went back to the zero position. Also, it seems to me that the code:memcpy(pos, joint_position_when_command_entered_.data(), kNumJoints * sizeof(double)); is supposed to overwrite whatever the current position value is with the joint_position_when_command_entered_. But in fact, in the raw code, joint_position_when_command_entered_ is only assigned with values at the first time when the robot arm is activated. I tried manually to update its value as the current position and the final result was the zeroing and returning back. Added all the terminal output as follow:

Memory is not locked Running without realtime priority Joint torque limits: 60, 60, 60, 30, 12, 12, 6 Listening for robot 0 port 30200 LBRiiwaClient state changed from 0 to 1 onStateChange ( 1634640392163000): old 0 new 1 onStateChange ( 1634640392163000): quality 0 safety 0 oper 2 drive 0 control 3 command 0 overlay 0 Something weird happens! LBRiiwaClient state changed from 1 to 2 onStateChange ( 1634640392361000): old 1 new 2 onStateChange ( 1634640392361000): quality 2 safety 0 oper 2 drive 0 control 3 command 0 overlay 0 LBRiiwaClient state changed from 2 to 3 onStateChange ( 1634640392846000): old 2 new 3 onStateChange ( 1634640392846000): quality 3 safety 0 oper 2 drive 2 control 0 command 1 overlay 1 LBRiiwaClient state changed from 3 to 4 onStateChange ( 1634640392856000): old 3 new 4 onStateChange ( 1634640392856000): quality 3 safety 0 oper 2 drive 2 control 2 command 1 overlay 1 LBRiiwaClient state changed from 4 to 1 onStateChange ( 1634640598907000): old 4 new 1 onStateChange ( 1634640598907000): quality 0 safety 0 oper 2 drive 2 control 2 command 1 overlay 1 LBRiiwaClient state changed from 1 to 0 onStateChange ( 1634640598924000): old 1 new 0 onStateChange ( 1634640598924000): quality 0 safety 0 oper 2 drive 0 control 3 command 0 overlay 0 LBRiiwaClient state changed from 0 to 1 onStateChange ( 1634640598946000): old 0 new 1 onStateChange ( 1634640598946000): quality 0 safety 0 oper 2 drive 0 control 3 command 0 overlay 0 LBRiiwaClient state changed from 1 to 2 onStateChange ( 1634640599143000): old 1 new 2 onStateChange ( 1634640599143000): quality 2 safety 0 oper 2 drive 0 control 3 command 0 overlay 0 LBRiiwaClient state changed from 2 to 3 onStateChange ( 1634640600338000): old 2 new 3 onStateChange ( 1634640600338000): quality 3 safety 0 oper 2 drive 2 control 0 command 1 overlay 1 LBRiiwaClient state changed from 3 to 4 onStateChange ( 1634640600348000): old 3 new 4 onStateChange ( 1634640600348000): quality 3 safety 0 oper 2 drive 2 control 2 command 1 overlay 1 Re-entering command state. Holding position and ignoring LCM commands.

sammy-tri commented 3 years ago

joint_position_when_command_entered_ is updated whenever the robot enters the COMMANDING_ACTIVE state, not just the first time. https://github.com/RobotLocomotion/drake-iiwa-driver/blob/master/kuka-driver/kuka_driver.cc#L485

If the driver is exiting inside of onStateChange, then I would expect that command is never called, and so the driver shouldn't have a chance to command anything to the robot over FRI at all, so I'm not sure where the motion is coming from (maybe command is being called anyway? But the position still shouldn't be zeros). I don't know why we would call setJointPosition https://github.com/RobotLocomotion/drake-iiwa-driver/blob/master/kuka-driver/kuka_driver.cc#L561 with the zero position, and I don't know why the robot would move to zero if we didn't. Maybe add a check at the call to setJointPosition to print (or exit?) if it's ever called with zeros?

LCM logs would probably help.

What version of the Sunrise FRI package are you using? And what software version is running on the robot? What Java application is running on the robot? (I'm guessing sunrise_1.11/DrakeFRITorqueDriver.java from drake-iiwa-driver?)

y8han commented 3 years ago

I printed out the pos values before robotCommand().setJointPosition(pos); and if I remembered correctly, there was no zero pos on the terminal. That's why at first I thought there should be some communication issue on the FRI side. I will print pos again to ensure it. I am not quite sure what exact version we are using now. Under drake-iiwa-driver, there are sunrise_1.7 and sunrise_1.11. Under each of them, there are DrakeFRIPositionDriver.java and DrakeFRITorqueDriver.java.

sammy-tri commented 3 years ago

Normally, FRI communication problems (which you are having) just cause the robot to stop moving (and the brakes to activate). I've never seen (using the java scripts from drake-iiwa-driver, anyway) the robot move in response to FRI issues. You could probably write a Java program for the robot which does move the robot when FRI exits, but the programs included with this driver don't do that.

I've noticed you're not running the driver with realtime priority. If you pass --realtime (you'll need to run as root or configure your system to let user accounts run as realtime) does that help to avoid issues with FRI?

y8han commented 3 years ago

Hi Sammy, Sorry for the late reply. We just met with a problem of the robot hardware, which made it impossible to do more tests. I will see what's going on when we solve the hardware issue.

y8han commented 3 years ago

Hi Sammy, We found that the issue may be caused by the robot hardware issues (maybe the communication drive inside the robot arm). The programming codes are good and we tested it on the other Kuka IIWA arm and there was no weird zeroing. Sorry for bothering you and I really appreciate your efforts on building this reliable tools for the community.

sammy-tri commented 3 years ago

Glad to hear it's working for you now.

y8han commented 3 years ago

Hi Sam, Thank you for your help! Indeed, I have a new comment for another issue and it has been added on Github. Best, Yunhai

On Fri, Nov 19, 2021 at 3:10 PM Sam Creasey @.***> wrote:

Glad to hear it's working for you now.

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/RobotLocomotion/drake-iiwa-driver/issues/38#issuecomment-974400923, or unsubscribe https://github.com/notifications/unsubscribe-auth/ANOWLFYSQOU2WIM7RY3WPW3UM2VMRANCNFSM5GAUUCJQ . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.