Skip to content
Snippets Groups Projects

Package fr3_impedance_controller_real in nix env

Merged Kevin Haninger requested to merge 7-add-local-package-to-nix-flake-new into main
1 unresolved thread
Compare and Show latest version
87 files
+ 659
532
Compare changes
  • Side-by-side
  • Inline
Files
87
@@ -5,9 +5,6 @@
#ifndef DISTURBANCE_OBSERVER_HPP_
#define DISTURBANCE_OBSERVER_HPP_
/* C++ STL */
#include <iostream>
/* Custom Headers */
#include "eigen_types.hpp"
@@ -17,19 +14,26 @@ class DisturbanceObserver
private:
T dt_; // timestep
VecX<T> fq_; // Q-filter cutoff-frequency [Hz]
VecX<T> Fd_hat_; // estimated force disturbance [N]
VecX<T> Fd_hat_; // estimated force disturbance [N, Nm]
VecX<T> F_lhs_, F_lhs_prev_; // left-hand-side state of DOB
VecX<T> F_rhs_, F_rhs_prev_; // right-hand-side state of DOB
public:
DisturbanceObserver(const T & dt, const VecX<T> & fq);
DisturbanceObserver(const VecX<T> & fq);
//* ----- SETTERS --------------------------------------------------------------------------------
void set_parameters(const T & dt, const VecX<T> & fq)
{
dt_ = dt;
fq_ = fq;
}
//* ----- METHODS --------------------------------------------------------------------------------
// void update_previous_states()
// {
// F_lhs_prev_ = F_lhs_;
// F_rhs_prev_ = F_rhs_;
// };
void update_previous_states()
{
F_lhs_prev_ = F_lhs_;
F_rhs_prev_ = F_rhs_;
};
//* ----- GETTERS --------------------------------------------------------------------------------
/**
Loading