SOSML Parameter tuning

Tune the observers so they are working, compare with Frnaka estimates. SOSML is working as of #5 (closed), but is sensitive to certain parameter settings and not working very robust.

Goal: Have SOSML working, and maybe have less phase lag than the low-pass filter from standard momentum observer.

SOSML

• Test on two link in simulation
• Test on data from Franka Emika, either rosbag play or live data on Franka
  • Look at the contact transitions in detail

Observer, general

• Can do experiments with a higher-speed contact transition
• Ploting results:
  • Check timestamps on DOB estimate, could use the timestamp from the rosbag if needed
  • Frequency plot could help us see if there's internal filtering on the Franka estimate, but just need positive frequencies and log magnitude
• Check using SOSML intead of numerical differentiation once working
• Add UI to visualize observer results and robot motion

Experiment To Do Lists

• Test SOSML-based acceleration estimation to Task-space DOB and check its effect.
• Test with various velocity range for FRF analysis.
• Test using a compliance between robot and f/t sensor to stabilize the contact loss
• Online SOSML gain tuning by watching plots
  • Check during sinusoidal motion
• Plot velocity and evaluate if implementing friction model would be helpful
• Apply DOB feedback (carefully) to see if it is beneficial for improving motion control performance.
• Apply external force with f/t sensor in the hand to see about the impedance rendering

Troubleshooting

• Acquiring data
• Abrupt jump when controller starts (occasionally)

added backlog label

Hello @hanikevi , while I setting up a two-link model for optimization approach, I could check and debug about SOSML @ 2136f7e6, but still it seems that it is too sensitive to parameter tuning. I will check if it works with the FR3 robot similarly, or has some benefits in phase lag during contact.

• NOTE: Euler method and MOB results are filtered by 1st-order low-pass filter

mentioned in issue #9 (closed)

changed the description

mentioned in merge request !3 (merged)

changed the description

changed the description

changed the description

Hello @hanikevi , I merged 7-...-new branch into main branch after refactoring fr3_impedance_controller_real, and I collected some data with higher velocity (0.15m/s at most) command by joystick.

Reference Tracking

Overall, the robot follows both pose and twist pretty fine, but I need to check if applying DOB feedback can improve tracking performance in free-space.

• Pose Tracking

• Twist Tracking

Acceleration Estimation

I compared btwn (1) Euler method with LPF (considered it as a ground truth), (2) MOB-based estimation, and (3) SOSML-based estimation. Looks like SOSML has similar tendency to Euler method compared to MOB-based one, but additional gain tuning is required. I set SOSML parameters very low, but when I increased a little, noise effect was amplified very sensitively.

• Joint acceleration

• Task acceleration

Disturbance Estimation during Contact

I set cutoff frequency of DOB and MOB as a similar level, and checked

\hat{\tau}_d

of MOB and DOB for validation.

• Joint torque disturbance

The result of contact response was actually not very impressive compared to last time. I think I was to careful for experiment.

• DOB seems to show a little bit faster response, but I forgot to apply SOSML-based acceleration estimation for DOB to see its effect (I am gonna check this today).

• Also, contact force estimation of DOB was not that good, but I think this is obvious because DOB estimates a lumped disturbance not only the contact force with environment.

• Task-force disturbance (only in

  z
  direction)

• FRF Frequency response was not pretty actually. I need further experiment to analyze it.

Issues

• ROS time issue: I made custom topic message for controller states and published it while rosbag recording. I converted bag file to csv file, and I read time information from std_msgs/Header of the message, however, there was multiple data on the same timestamp. I will fix it.
• spacenav was not running all of sudden, so I used my PS3 joystick controller for a moment.

TODO

• Apply DOB feedback, and check its effect in improving motion control performance during free-space motion.
• Apply SOSML-based acceleration estimation for DOB, and check its effect in disturbance response during contact (without DOB feedback)
• Fix ROS timestamp issue
• Test with various velocity range for FRF analysis
• Convert quaternion to RPY for intuitive plotting
• Online SOSML parameter tuning
• Make periodic contact, not by myself

marked the checklist item Test on data from Franka Emika, either rosbag play or live data on Franka as completed

marked the checklist item Can do experiments with a higher-speed contact transition as completed

marked the checklist item Frequency plot could help us see if there's internal filtering on the Franka estimate, but just need positive frequencies and log magnitude as completed

marked the checklist item Apply MOB-based acceleration estimation to Task-space DOB and check its effect. as completed

changed the description

marked the checklist item Test SOSML-based acceleration estimation to Task-space DOB and check its effect. as completed

marked the checklist item Test SOSML-based acceleration estimation to Task-space DOB and check its effect. as incomplete

Hello @hanikevi , I tuned SOSML observer gains (with some tuning in inertia matrix) roughly, and it shows better results in acceleration/disturbance estimation.

• Joint acceleration

• Task acceleration

Although there's some unsatisfactory estimation in some DoFs, I think this can be made better with further fine tuning of parameters, since I just set the same values for all DoF.

Additionally, when I compared contact information btwn (1) DOB with Euler-based acceleration, and (2) DOB with SOSMl-based acceleration, I could see some better results.

• Time-domain : less peak & noise in DOB using SOSML-based estimation

  • DOB using acceleration from Euler method

  • DOB using SOSML-based acceleration

• Frequency-domain : DOB using SOSML-based estimation has lower-level over frequency range compared to franka estimation

  • DOB using acceleration from Euler method
  • DOB using SOSML-based acceleration

Logging issue

• I didn't use rosbag timestamp yet, but it seems that there're data loss during rosbag, which results in discontinuous data plotting. About 0.04 sec time jump happened occasionally. Possibly, by increasing buffer size of rosbag would solve this issue I guess?
• To remember this issue,
  • rosbag delay and time jump

• Rosbag: it might also be that the bandwidth is saturating. Are you collecting all topics or just the specific topics you need? If all (this is default) you could try just collecting a few topics to reduce the bandwidth needs. Like you mention, buffer might help too? I haven't used that parameter before.
• Acceleration Results:
  • I find it interesting that q1 and q3 oscillate more with SOSML (5-15 sec range), but then seems like Euler generally has more white noise.
  • Is the data for acceleration and force the same? I.e. the jumps in acceleration I see for Euler around 22, 27, etc are 'real'?
• Force results:
  • Good to see that the SOSML improves the force estimates
  • I find the overshoot on Franka to be much higher than the overshoot on the DOB. That looks to match the external force better and shows that the inertia modelling is working.
  • Please add the zoom of the contact transitions, at least for the meeting with Sehoon, I'm sure he'll want to see those too

• rosbag: yes, I will reduce published topics, cause I don't need all of them. Additionally I will check if setting large buffer would resolve this issue.
• acceleration:
  • For the results of q1, q3, maybe I should do further gain tuning for each DoF, and diagonal term of inertia matrix.
  • Since acceleration is just for observation currently, without applied as a feedback control, I couldn't see any specific issue from those peak. Also, maybe it was not observed in DOB due to Q-filter I guess.
• Force results: Yes I will prepare for the zoom of them. Currently it is quite ugly due to data loss during logging.

I will share the results after I fix some issues, and get further results. Thank you for your comments ! =)

changed the description

marked the checklist item Check timestamps on DOB estimate, could use the timestamp from the rosbag if needed as completed

marked the checklist item Ploting results: as completed

marked the checklist item Check using SOSML intead of numerical differentiation once working as completed

changed the description

removed backlog label

changed the description