support for Katana300 with old firmware

[mintar]

Several people have had problems with executing trajectories on a Katana 300 6m180 with an old firmware. The error log looks similar to this:

process[katana-2]: started with pid [18127]
[ INFO] [1389893296.700950640]: trying to connect to katana (serial port: /dev/ttyS0) ...
[ INFO] [1389893296.701484379]: success: serial connection to Katana opened
[ INFO] [1389893296.701531683]: success: protocol class instantiated
[ INFO] [1389893296.701566842]: success: communication with Katana initialized
[ INFO] [1389893296.789326405]: success: katana initialized
[ INFO] [1389893296.975445650]: no calibration required
[ INFO] [1389893296.975594807]: success: katana calibrated
[ INFO] [1389893297.175685246]: katana gripper grasp query service started on topic /gripper_grasp_status
[ INFO] [1389893297.181666223]: katana gripper grasp hand posture action server started on topic /gripper_grasp_posture_controller
[ INFO] [1389893309.250900985]: Sending trajectory to Katana arm...
[ERROR] [1389893310.946135966]: ReadNotCompleteException: Another program accessed the KNI. Please stop it and restart the Katana node. (exception in executeTrajectory(): Cannot read all data from '')
[ERROR] [1389893310.946395937]: Problem while transferring trajectory to Katana arm, aborting

It fails in the kni->setAndStartPolyMovement() call, at joint_trajectory_action_controller.cpp. (See e.g. this answers.ros.org post ).

[mintar]

This seems to be caused by the fact that the old firmware on the Katana300 isn't fully compatible with the version of the KNI that we're using, and it doesn't support following spline trajectories via setAndStartPolyMovement(). This should be fixed by properly overriding executeTrajectory() in Katana300.cpp.

The story so far:

• @fivef and @BennyRe have written a workaround for the Katana300. The latest version seems to be in the master branch in fivef/katana_driver (for ROS Groovy).
• I have cleaned up their git history and extracted the relevant changes. Since I'm a bit wary of putting them into the master branch, I've put them into the branches fuerte_katana300. groovy_katana300 and hydro_katana300 in my personal fork mintar/katana_driver. Let's use those branches to debug and improve the code.

What (I think that) the commits do

• f6756e9 replaces the unsupported setAndStartPolyMovement() call by a call to moveToEnc() for each trajectory segment. Unfortunately, this results in very shaky behavior, since instead of one smooth trajectory, the arm stops at each waypoint.

• e5ef20c therefore ignores all intermediate waypoints and instead uses moveToEnc4D() to directly go to the end point of the trajectory. This probably results in smoother movement, but will crash into obstacles for nontrivial trajectories.

• b93fc7d directly changes some velocity/acceleration parameters inside the moveToEnc4D() function of the KNI. I won't merge this, since it breaks all other Katana models. There must be a way around changing the KNI.

• 7e61065 further tunes some motor + velocity limits, this time inside Katana300.cpp. Fine by me, if it works for everyone.

• d106c9b switches off the check whether the goal position was actually reached. Do we need this? Why? Can we drop this commit if we comment the following line back in?

  //kni->moveRobotToEnc(encoders, true); // to ensure that the goal position is reached

• 850da06 doesn't hurt; testSpeed() isn't used here.

What you can do to help

If you own a Katana 300 6m180 and would like to get it working with katana_driver, please do the following:

• clone one of the branches fuerte_katana300. groovy_katana300 and hydro_katana300 from my personal fork mintar/katana_driver, depending on your ROS version. This issue is independent of your ROS version, so it doesn't matter which one you pick.
• rosmake katana_driver
• Start the Katana and send a trajectory, as described on the katana_driver wiki page. Keep your hand on the "off" switch in case the arm doesn't move as expected... :)
• report back here with
  • whether it worked
  • as much information for identifying your arm as possible: exact Katana type, firmware version, pics, whatever you can think of
• for bonus points: take a look at the last six commits in that branch (e.g., f6756e9..850da06 for hydro) and try to fix the things I complained about above. E.g., can we still keep the intermediate waypoints? Is there a way not to change the KNI?
• submit pull requests with your changes! :)

Since the Katana we have is a 450 6m90A, this is the only way for me to debug this.

---

NB: I've also extracted all other changes from fivef/katana_driver and sorted them into the following branches, all for Groovy right now. Should not be relevant here, just listing them for completenesses sake.

• pfiffner_gripper_jtac -- probably not relevant to this issue (adds capability of sending arm + gripper joints in one single trajectory)
• pfiffner_gripper_controller -- not required any more, since there is already equivalent code in uos/katana_driver; just for backup
• pfiffner_katana_description -- not relevant for this issue, maybe I'll file a separate one later
• pfiffner_preempt_requested -- obsolete change, just for backup

[JakaCikac]

Hi!
The Katana is: Katana 300 6m180.

I managed to rosmake the groovy_katana300 with some difficulties. For one ar_pose can not be found and I don't know how to install it.
I ran the trajectory command and it didn't work so far. The following error appears:

[DEBUG] [1390928409.894723124]: Sleeping for 0.853293 seconds before start of trajectory
[ INFO] [1390928410.295045957]: Sending trajectory to Katana arm...
[DEBUG] [1390928410.295302887]: Entered executeTrajectory. Spline size: 5, trajectory size: 15, number of motors: 6
[ERROR] [1390928415.481205170]: MotorTimeoutException (exception in executeTrajectory(): Motor timeout)
[ERROR] [1390928415.481445021]: Problem while transferring trajectory to Katana arm, aborting
[DEBUG] [1390928415.481599567]: Setting the current goal as aborted
[DEBUG] [1390928415.481758659]: Setting status to aborted on goal, id: /follow_joint_trajectory_client-1-1390928409.748005632, stamp: 1390928409.75

More testing: I ran speedTest(), everything worked. After the end of speed test the Katana was not left in home position. I re-ran the client trajectory and the Katana moved to the home position quite nicely (multiple motors moved at the same time), however after it got to the home position the same error was displayed.

[ERROR] [1390931793.109036759]: MotorTimeoutException (exception in executeTrajectory(): Motor timeout)
[ERROR] [1390931793.109277482]: Problem while transferring trajectory to Katana arm, aborting

Now I chaged the console to display debug messages, no other changes were made. I ran clien trajectory and the error doesn't appear. But the Katana doesn't move besides a small twitch (for a second). It is supposed to move as it moves in the simulation, right?

Motor status is all 8.

[ INFO] [1390932210.314304721]: Sending trajectory to Katana arm...
[DEBUG] [1390932210.314594991]: Entered executeTrajectory. Spline size: 5, trajectory size: 15, number of motors: 6
[ INFO] [1390932210.580668976]: Waiting until goal reached...
[ INFO] [1390932210.595706118]: Goal reached.

[ INFO] [1390932511.265970542]: Sending trajectory to Katana arm...
[DEBUG] [1390932511.266259655]: Entered executeTrajectory. Spline size: 5, trajectory size: 15, number of motors: 6

[mintar]

Hi @JakaCikac,

For one ar_pose can not be found and I don't know how to install it.

That's only required by katana_kinect_calibration, and you don't need that (it also doesn't work so well at the moment). If you want, you can install the stacks mikeferguson/ar_kinect (branch groovy-devel) and LucidOne/ar_tools (branch master); this should fix the dependency problems.

About the MotorTimeoutException: I can't really help a lot, because I don't have a Katana 300 for testing. So it's more or less up to you guys to find a solution. But I guess this could be fixed by making sure that the kni->unBlock() call is executed:

https://github.com/mintar/katana_driver/blob/groovy_katana300/katana/src/Katana300.cpp#L250

The idea of unBlock() is to reset all motor statuses. It's still weird that they happen at all. @fivef, @BennyRe: have you had this problem?

[JakaCikac]

I am no longer getting this error, I am not able to reproduce it. I don't know why it happened before.

Meanwhile @mintar : Do you have any ideas about no movement when executing trajectory? The weirdest part is that the Katana is able to "trajectory"-itself into home position using multiple motors at once but after it's in home position there is no movement. Also, the nodes print out as if trajectory is successful despite the fact that it wasn't.

[ INFO] [1391180119.556416282]: Sending trajectory to Katana arm...
[DEBUG] [1391180119.556663219]: Entered executeTrajectory. Spline size: 5, trajectory size: 15, number of motors: 6
[ INFO] [1391180119.875455730]: Waiting until goal reached...
[ INFO] [1391180119.892043692]: Goal reached.
[DEBUG] [1391180119.892130024]: Setting the current goal as succeeded

To further explain what happens, I took two videos.
1st: Katana Calibration - https://www.dropbox.com/s/lnqkfa5hugmg6sf/katana300_calib.MOV
2nd: Katana client trajectory - https://www.dropbox.com/s/l47k7f7owpejg2o/katana300_kc.MOV

Is there any documentation for the Katana 300 available online or offline? I haven't been able to find it. I am completely lost when it comes to KNI, because I have nowhere to look for explanation.
I found http://docs.ros.org/groovy/api/kni/html/classCKatana.html but as I understand it is not Katana300 specific?

[BennyRe]

Hey all,

I'll answer inline directly to the individual commits.

2014-01-24 Martin Günther notifications@github.com:

What (I think that) the commits do

• f6756e9 replaces the unsupported setAndStartPolyMovement() call by a call to
  moveToEnc() for each trajectory segment. Unfortunately, this results in
  very shaky behavior, since instead of one smooth trajectory, the arm stops
  at each waypoint.

The solution with moveToEnc() worked reasonably for fivef with his MoveIt
generated trajectories. Optimizing the wait time between the single steps
could improve the smoothness of the movement. Our old Katana Software
(SmartSoft) also uses moveToEnc() and produces very smooth trajectories.
Unfortunately I'm not allowed to share the code.

• e5ef20c therefore ignores all intermediate waypoints and instead uses moveToEnc4D()
  to directly go to the end point of the trajectory. This probably results in
  smoother movement, but will crash into obstacles for nontrivial
  trajectories.

Correct, very smooth but also very risky. This was a dirty hack for a very
special use case, where we had an extremely hard deadline. Don't use this
code.

• b93fc7d directly changes some velocity/acceleration parameters inside the
  moveToEnc4D() function of the KNI. I won't merge this, since it breaks all
  other Katana models. There must be a way around changing the KNI.

This did belong to the same use case as above.

• 7e61065 further tunes some motor + velocity limits, this time inside Katana300.cpp.
  Fine by me, if it works for everyone.

  d106c9b switches off the check whether the goal position was actually reached. Do
  we need this? Why? Can we drop this commit if we comment the following line
  back in?

  //kni->moveRobotToEnc(encoders, true); // to ensure that the goal
  position is reached

I don't remember exactly why I commented out this line in allMotorsReady,
but I think this line caused problems for me. I think it returned false all
the time. But this could also be a special issue by our Katana, which is
quite old now and does very strange things in the last time.

[...]

For my trajectories (generated by a Learning from Demonstration node and
NOT by MoveIt) my latest branch works very good and very smooth. It
uses startSplineMovement
. The branch can be found here:
https://github.com/BennyRe/katana_driver/tree/groovy

I have had the MotorTimeoutExceptions also several times but don't know
anymore in which situation this was or how I solved this.
I wish I could help you more, but atm it's very busy in our lab.

Greetings Benny

[mintar]

Thanks @BennyRe for the heads up! I like the sendSplineToMotor() version much better. @JakaCikac / others: Can you test whether the BennyRe/groovy branch works for you? If yes, I'll set up a new branch with clean commits based on that.

[mintar]

@JakaCikac wrote:

Meanwhile @mintar : Do you have any ideas about no movement when executing trajectory? The weirdest part is that the Katana is able to "trajectory"-itself into home position using multiple motors at once but after it's in home position there is no movement. Also, the nodes print out as if trajectory is successful despite the fact that it wasn't.

Yes, that's weird. Can you try @BennyRe's branch instead (see my last comment)?

Is there any documentation for the Katana 300 available online or offline? I haven't been able to find it. I am completely lost when it comes to KNI, because I have nowhere to look for explanation.
I found http://docs.ros.org/groovy/api/kni/html/classCKatana.html but as I understand it is not Katana300 specific?

Unfortunately, I don't have any (I'd be grateful for anything other people might have!).

All I have is pretty specific to our Katana 450 6M90A. I've uploaded everything I scraped off the Neuronics website before it went offline here.

[JakaCikac]

Hi!

I am happy to report that @BennyRe 's branch works! The katana executes the movement as it should, when launching client trajectory.

I will do further testing but so far everything seems to work.

@mintar Thank you for sharing those files, I hope they will help for further work with katana 300, despite the fact that it's for katana 450.

[mintar]

@JakaCikac: That's great! I've just sorted through the code there and created a nice and tidy branch that should have everything we need: groovy_katana300_v2

Can you test that branch? If it works for you, I'll merge it into the official katana_driver repo.

[JakaCikac]

@mintar Tested. It works nicely. Teleoperation with keyboard also works.

[DEBUG] [1391513264.743757122]: Sleeping 0.468821 seconds until scheduled start of trajectory
[DEBUG] [1391513265.212766371]: Executing step 0
[DEBUG] [1391513265.301540644]: Executing step 1
[DEBUG] [1391513265.697972752]: Executing step 2
[DEBUG] [1391513265.988568433]: Executing step 3
[DEBUG] [1391513266.603072573]: Executing step 4
[DEBUG] [1391513267.175286587]: Executing step 5
[DEBUG] [1391513267.570077944]: Executing step 6
[DEBUG] [1391513267.884971794]: Executing step 7
[DEBUG] [1391513268.336367847]: Executing step 8
[DEBUG] [1391513268.751612083]: Executing step 9
[DEBUG] [1391513269.356899755]: Executing step 10
[DEBUG] [1391513269.821488328]: Executing step 11
[DEBUG] [1391513270.507954133]: Executing step 12
[DEBUG] [1391513271.355333852]: Executing step 13
[DEBUG] [1391513272.048275803]: Executing step 14
[ INFO] [1391513272.355933613]: Waiting until goal reached...
[ INFO] [1391513272.470647843]: Goal reached.
[DEBUG] [1391513272.470844500]: Setting the current goal as succeeded

[mintar]

Great! I've pushed the solution in groovy_katana300_v2 to the fuerte, groovy and hydro branches of the uos repo. Thanks everyone!

(P.S.: This means that the mintar/*_katana300 branches shouldn't be used any more and will be deleted in the future.)