Enhance impedance control

[nickswalker]

Expand the impedance motion stack for both joint-space and Cartesian torque control. Adds tracking/reference-handle based impedance motions, introduces nullspace posture control for Cartesian impedance, adds joint-limit repulsion and shared torque safety utilities, and updates the Python API and docs accordingly.

Joint-space impedance

• Add joint impedance motions with joint-limit repulsion.
• Add joint impedance tracking via live reference updates.
  • A natural solution for trajectory-tracking impedance motions (closes Franky and Torque control and Libfranka? #53).
    This was the primary motivation for this PR.
• Add optional joint friction compensation (off by default) for more transparent
  zero-stiffness kinesthetic teaching.
• Add configuration-space error clipping and safety hardening.

Cartesian impedance

• Refactor the Cartesian impedance API; rename the base class ImpedanceMotion
  → CartesianImpedanceBase, now that there is a distinct JointImpedanceMotion.
• Add tracking/moving Cartesian references.
  • A more natural interface for teleoperation uses (closes Smooth 30 Hz joint position control (jerky motion and gripper interference) #76).
• Move target_type into fixed-target Cartesian impedance motions.
• Add Cartesian nullspace posture control (with dynamic nullspace gains).
• Add Cartesian impedance acceleration feedforward support.
• Apply the same joint-limit repulsion, friction compensation as joint impedance, and task space error clipping.

Shared / API

• Runtime-adjustable impedance gains and damping via reference handles.
• Add TorqueStop for cleanly ending impedance motions.
• Expand README/API docs for the impedance motion interfaces (previously the
  ImpedanceMotion types weren't documented at all).

Examples

• joint_impedance_tracker.py, cartesian_impedance_tracker.py

Opening to see if anyone has feedback.

[TimSchneider42]

Hi,

I think this PR adds some really important functionality, but I will need some time to review it. I will get back to you once I have taken a look at it. Thank you very much for the PR, though!

Best,
Tim

[nickswalker]

Thanks for your willingness to take a look! Here are the two new Python examples running on an FR3.

manual_guidance_joint_impedance.py demonstrates setting joint 6 to be very stiff while leaving the others compliant (a la franka_interactive_controllers), which can sometimes be useful when doing kinesthetic teaching

trim.B82E2E43-2B00-4CC8-8B18-B52AE9AD1165.MOV

cartesian_virtual_fixture.py demonstrates a Python-side loop updating the cartesian reference for the cartesian impedance controller, allowing the user to push the robot along a vertical axis, and resisting all other motion.

trim.247AD8D4-340B-481A-AF0C-A6783A8653C9.MOV

[Asunatan]

Hello, thank you for sharing this work—the impedance control shown in the video looks very smooth and promising.

I'm relatively new to robotics and have been evaluating open-source Python libraries for controlling the Franka Emika Panda. Currently, I'm using Deoxys, but I've observed noticeable jitter when performing impedance control, which affects trajectory tracking and force adjustment. I've also looked at FrankaPy, but it doesn't seem to provide a dedicated impedance control interface. Given the performance your library demonstrates, I'm very much looking forward to it…

[Brunch-Life]

It's quite a good feature! I'm using it to use gello control franka use joint implance control!

[Brunch-Life]

waiting for this to be approved!

[nickswalker]

I set the fork to compile and release wheels in the meantime: https://github.com/nickswalker/franky/releases
Also added 0.21.1 libfranka builds, as those are what the videos were recorded with.

[TimSchneider42]

Hey guys,

Sorry for the delay in merging this. I am currently super busy as I am pushing for NeurIPS (deadline is on the 6th of May). I cannot promise to merge this before the deadline, as I would like to review it thoroughly. However, I will try my best to do it as soon as I can.

Best,
Tim

[hugohadfield]

Really looking forward to seeing this land!

[TimSchneider42]

Sorry to keep you waiting for so long. I will have some time next week to review the code and merge the PR.

[nickswalker]

We've been using the JointImpedenceTracker in 0 stiffness for collecting kinesthetic demonstrations lately and noticed that the joints aren't quite transparent. Went ahead and incorporated a simple friction compensation feedforward term (off by default), as I expect this will be a common use case.

[nickswalker]

I've done some characterization of the tracking error for a simple Lissajous:

trim.8134BB07-6CD7-4ED5-A20F-9CD1B2D1E48B.MOV

Tracking error at different stiffnesses, trajectory speeds (as percentage of max cartesian or joint vel), with and without friction compensation:

These controllers have quite some surface area, which might make it hard to be confident that they're ready to go. I think some further examples that exercise them might help with this? This tracker example covers the basic python usage of the trackers with stiffness gains, friction params, inertial feedforward terms. But there's currently nothing that exercises the cartesian nullspace tasks or the simpler non-tracker motions...

[TimSchneider42]

Hey @nickswalker, just a quick heads-up. I merged your changes into the master branch yesterday and added some refactoring. Unfortunately, I don't have time today to give you a full summary of my changes. Just want to let you know that with your recent edits, we are running danger to diverge. I will have more time to look into this on Monday and give you a full account of my refactoring.

Btw, would you consider becoming a co-maintainer of franky? I think I could use some support, and I really appreciate the effort you have already put into this project!

[nickswalker]

Thanks for doing the merge! Happy to help maintain.

I rebased on your refactoring. The remaining items are not as important as what has already been merged:

• Changed the cartesian interface to accept full stiffness/damping matrices, as I see that most comparable implementations support this
• Add cartesian gain shaping to the joint impedance controller. Fairly close copy of the implementation in fairo's Polymetis. This lets one eek out slightly better trajectory tracking performance by running IK before hand then running with both q and cartesian reference (second set of plots from above).
• Manipulability nullspace task
• A motion for stopping under torque control (parallel to the other stopmotions)
• A raft of ~one line fixes in the last few commits

[nickswalker]

It seems that the new TorqueStopMotion hangs forever in the sim tests. Perhaps the semantics for motion termination are different for torque motions than position/velocity motions in the robot/libfranka protocol.

[TimSchneider42]

Maybe you already saw, but here is a brief summary of my changes:

• Replaced the double-buffered lock-free handles with a triple-buffered scheme, because the former had a race condition (I saw later that you also fixed that in a separate commit)
• Made these buffers private and changed the API to just have a method on the motion for setting new gains/references
• Removed ExponentialImpedanceMotion because I think it does not really serve a purpose anymore, and aligned the implementation of Cartesian and joint impedance controllers a bit.
• Dropped EIGEN_MAKE_ALIGNED_OPERATOR_NEW because c++17 and upwards do not need it anymore.
• Added type hints to tracker.py
• Moved the tutorial into a separate file as the README was getting very long by this point. Though I think we need to improve the documentation format at some point.

I will look into the TorqueStopMotion issue now.

[TimSchneider42]

@nickswalker, I just merged the remaining changes. The TorqueStopMotion issue was caused by a problem inside franky-sim which is now fixed.

If you are happy with the state of the code as it is, I think we can soon release it to PyPi.

Again, thanks a lot for the PR!

[nickswalker]

Great! The library users in our lab will be exercising these controllers and the desk API changes throughout the week, so I'd deem it releasable if they don't run into problems.

[TimSchneider42]

Sounds good to me

[TimSchneider42]

@nickswalker, bit of a random question, but would you be fine with switching franky to an MIT license? I would like to lower the legal barriers to using this library as much as possible, and I think now would be a good time for updating the license.

[nickswalker]

Yes, I would endorse the next version being released under a permissive license. A quick look at git blame and it seems that the only other substantial contribution came from the original author. My understanding is we'd need their permission to change the license of code that they own the copyright to. Perhaps you could tag him in a new issue to obtain his consent?

[hugohadfield]

It would be extremely helpful to release this package under a more permissive license mostly because legal review inside companies (even for research only code) can sometimes be extremely slow for anything other than MIT licensed code