Impedance controller implementation real

Goal: Have first implementation of the baseline impedance controller on the franka robot. This will be used to validate the observers and as the base for future controllers.

• Look at the zoomed in contact transitions, especially possible phase delay between franka and your approach

• Could also check observer synchronization by publishing observer results, even when re-playing rosbag

• Save desired/measured pose, twist, accel of (real) FR3's end-effector

  • NOTE: O_T_EE_d is not saved in torque control interface.

Hello @hanikevi , thanks to Dingyuan and Ben, I implemented Cartesian impedance controller connected with spacenav joystick, and tested that impedance controller is working for real robot with desired twist command from the joystick. It looks working fine, but needs to further job (especially for orientation control since the gain was too low).

I implemented it at local franka_ros package at cartesian-impedance-control-with-spacenav-teleop branch here, and now I will implement it in nix environment to integrate with my observers.

Nice, hope you could get some first data collection. For the nix packaging, I suggest we can discuss on Monday - it's not a priority and I can help a lot there (otherwise it'll be quite difficult).

Oh I got it. Then I will focus on data collection first. Thank you for your feedback =)

Hello @hanikevi , I tested an impedance controller of real robot manipulated by joystick making a contact with the FT sensor. From the recorded data, I compared three data;

• DOB estimation : obtained with
  $F_{imp} = (J^T)^+ \ \times$
  tau_J_d
• FR3 estimation : F_ext_hat_filtered =
  $(J^T)^+ \ \times$
  tau_ext_hat_filtered
• FT sensor data

I tested twice and the results are as below (area colored as cyan represents contact with FT sensor):

• Test 01
  • for Test 01, data after around
    $t=40$
    are lost, due to error of franka (maybe I commanded downward motion of the robot too hard)
• Test 02

From the results, it seems that:

• Control performance during free-space motion could be improved by applying negative-feedback of disturbance estimation.
• Both DOB and FR3's state can be used to estimate contact force, but since normal direction is dominant compared to other axes, some idea would be required how to handle these contact force/torque in a smart way.
  • Until now, I represented end-effector's wrench in the world frame, but it would be better to represent it in body frame or contact frame. I will think about it and bring some topics to discuss on the next Monday.

Here, I just wrote some TODOs for further implementation and experiments.

• Save desired/measured pose, twist, accel of (real) FR3's end-effector
  • NOTE: O_T_EE_d is not saved in torque control interface.
• Add homing feature to improve repeatability of experiment (+ sync w/ Simulator)
  • Twist command by joystick -> record pose trajectory -> reproduce pose to validate controller online/offline
• It would be better if user can feel an interaction while robot making contact with environment by teleopration.
• Refactor MATLAB code to save & analysis data efficiently.
• Sync recorded data (especially time) btwn FT sensor and FR3 states
• Experiment with FT sensor mounted on slope

• The teleop, reproduceable trajectory are lower priority (teleop needs different hardware, reproduceable trajectories not necessary as we can evaluate all approaches in parallel).
• FT on slope just if needed to explore some interesting aspects of performance. Different task space inertia may be interesting.

changed the description

Hello @hanikevi , I made data sync by publishing observer states online, and compared between DOB estimation, Franka state (O_F_ext_hat_K), and FT sensor.

• I mentioned last time that there was NaN value is DOB estimation, but I guess there was just temporal issue in Franka state (inertia matrix or other). I didn't change anything, but it worked again.

• Time-domain result

  • It seems that DOB has less phase lag compared to Franka's internal state.
  • I will check if applying SOSML (or MOB) would bring some additional benefits in phase lag during contact.

• Frequency-domain result

  • I guess Franka's internal state estimation includes a specific filter such that high frequency contact force is filtered. (which results in phase lag compared to DOB.)

Results seem good, update to todos above

marked the checklist item Look at the zoomed in contact transitions, especially possible phase delay between franka and your approach as completed

marked the checklist item Could also check observer synchronization by publishing observer results, even when re-playing rosbag as completed

marked the checklist item Save desired/measured pose, twist, accel of (real) FR3's end-effector as completed

mentioned in merge request !3 (merged)

mentioned in commit 5c87d398

closed with merge request !3 (merged)

changed the description

Issue closed as impedance controller working, observer tuning to occur in #8

mentioned in issue #7 (closed)

mentioned in commit f63141a5

mentioned in commit 2f63ab99