Observer Implementation Real

C++ Implementation Plan

Dev environment [Kevin]

• [Kevin] Set up C++ deps: rosclient, and pinocchio. We use libfranka / PREEMPT_RT on converging PC
• [Kevin] Add CasADi in C++
• [Jeong] set up nix either on another OS on your laptop or look at WSL
• Have a look at symbolic variables in casadi, first section here

Observer program

• Observer program sketch (functions, signatures, etc)
• ROS interface
• Move symbolic function creation to the constructor to speed up the loop
• Debugging:
  • Add publisher for the observer estimate
  • Get joint torques with pinv to compare
• [optional] Load in parameters for gains from rosparam or a yaml file

Test data collection on real robot

• Test bag with free space motion to check that observer converges. Get help from Dingyuan, try to keep resulting bag(s) smaller than 30mb so it's ok in git
  • Position-controlled with sinusuoidal motion
  • Check that franka_state/tau_J_d is non-zero when the impedance controller is running
  • Impedance-controlled with external force application. start_franka_imp should start the impedance controller interface
  • Impedance control with external force, hold in a static pose so there's minimal friction compensation on the franka estimates.
  • Benchmark SOSML, standard momentum observer, and numerical differentiation for joint acceleration. Tune gains on SOSML to see if can get less noise w/ minor delay compared to numerical differentiation
• Connect F/T sensor - discussion with Philip Hotz
• Set up validation experiments, collect data
  • Add rod instead of hand at end-effector, so that end-effector has a point contact
  • Motion generation
    • Sinusoidal motion for each axis : x, y, z, \psi_x, \psi_y, \psi_z
    • Circular motion (or 3D motion) in xy-, yz-, zx- plane
    • Contact scenario :
      • vertical down to impact in z direction
      • circular motion in xy- plane during contact
      • (+optional) : rotate in \psi_x, \psi_y, \psi_z directions
    • Cartesian Impedance Control → refer libfranka
    • Only implement TFDOB first, without SOSML

Simulation as needed to get acceleration estimates

• Sim if needed, especially for true acceleration estimates