Added three integration methods

examples/simulation/general/cpp/admittance_controller.cpp

@@ -55,7 +55,10 @@ int main()
 
   // Compute admittance trajectory
   VectorXd u;
-  controllers::AdmittanceControllerPosition adm_controller;
+  double freq = 500.;
+  integrator::IntegrationMethod intg_method = integrator::IntegrationMethod::Euler;
+  controllers::AdmittanceControllerPosition adm_controller(freq, intg_method);
+
   adm_controller.set_mass_matrix_position(Vector3d(22.5, 22.5, 22.5).asDiagonal());
   adm_controller.set_stiffness_matrix_position(Vector3d(54, 54, 54).asDiagonal());
   adm_controller.set_damping_matrix_position(Vector3d(160, 160, 160).asDiagonal());
@@ -75,11 +78,15 @@ int main()
     x_desired = get_circle_target(start_position, t * dt);
     if (adm_pos[0] > 0.29 && adm_pos[0] < 0.31 && adm_pos[1] > 0.39 && adm_pos[1] < 0.41)
     {
+      f[0] = 40;
       f[1] = 40;
+      f[2] = 40;
     }
     else
     {
+      f[0] = 0;
       f[1] = 0;
+      f[2] = 0;
     }
 
     // Step controller

examples/simulation/ur/python/admittance_controller.py

@@ -20,7 +20,10 @@ def main():
         x_desired = get_circle_target(start_position, t*dt)
         ref_traj.append(x_desired)
 
-    adm_controller = sdu_controllers.controllers.AdmittanceControllerPosition(frequency)
+    integration_method = sdu_controllers.integrator.IntegrationMethod.RK4
+
+    adm_controller = sdu_controllers.controllers.AdmittanceControllerPosition(frequency, integration_method)
+
     adm_controller.set_mass_matrix_position(np.identity(3) * 22.5)
     adm_controller.set_stiffness_matrix_position(np.identity(3) * 54)
     adm_controller.set_damping_matrix_position(np.identity(3) * 160)
@@ -39,9 +42,13 @@ def main():
     for t in range(int(steps)):
         x_desired = get_circle_target(start_position, t * dt)
         if 0.29 < adm_pos[0] < 0.31 and 0.39 < adm_pos[1] < 0.41:
+            f[0] = 40
             f[1] = 40
+            f[2] = 40
         else:
+            f[0] = 0
             f[1] = 0
+            f[2] = 0
 
         # Step controller
         adm_controller.step(f, mu, x_desired, quat_init)
@@ -54,10 +61,14 @@ def main():
     ref_traj = np.array(ref_traj)
     adm_traj = np.array(adm_traj)
 
-    plt.figure(figsize=(8, 8))
-    plt.plot(ref_traj[:, 0], ref_traj[:, 1], label="desired trajectory", color='red')
-    plt.plot(adm_traj[:, 0], adm_traj[:, 1], label="admittance trajectory", color='blue')
-    plt.legend()
+    ax = plt.figure(figsize=(8, 8)).add_subplot(projection='3d')
+    ax.plot(ref_traj[:, 0], ref_traj[:, 1], ref_traj[:, 2], label="desired trajectory", color='red')
+    ax.plot(adm_traj[:, 0], adm_traj[:, 1], adm_traj[:, 2], label="admittance trajectory", color='blue')
+    ax.legend()
+    ax.set_aspect('equal')
+
+    plt.grid()
+
     plt.show()
 
 if __name__ == "__main__":

include/sdu_controllers/controllers/admittance_controller_position.hpp

@@ -4,6 +4,7 @@
 
 #include <Eigen/Dense>
 #include <sdu_controllers/controllers/controller.hpp>
+#include <sdu_controllers/integrator/integrator.hpp>
 
 namespace sdu_controllers::controllers
 {
@@ -52,7 +53,8 @@ namespace sdu_controllers::controllers
      * Initialize the admittance controller
      * @param frequency controller frequency, defaults to 500 Hz
      */
-    explicit AdmittanceControllerPosition(double frequency=500.0);
+    explicit AdmittanceControllerPosition(double frequency=500.0, 
+      integrator::IntegrationMethod intg_method = integrator::IntegrationMethod::RK4);
 
     /**
      * @brief
@@ -133,6 +135,8 @@ namespace sdu_controllers::controllers
 
     // Helper variables
     Eigen::Matrix3d rot_identity_;
+
+    integrator::IntegrationMethod intg_method;
   };
 }  // namespace sdu_controllers::controllers
 

include/sdu_controllers/integrator/integrator.hpp

@@ -0,0 +1,107 @@
+#pragma once
+
+#ifndef INTEGRATOR_HPP
+#define INTEGRATOR_HPP
+
+#include <Eigen/Core>
+#include <functional>
+#include <iostream>
+
+
+namespace sdu_controllers::integrator 
+{
+    /**
+     * @brief Utility functions for integrating different methods.
+     *  
+     * Inspired by the implementation of the integrators in 
+     * https://github.com/sfwa/ukf/blob/master/include/UKF/Integrator.h
+     *
+     * This file is directly pulled from sdu_estimators
+     * https://github.com/SDU-Robotics/sdu_estimators/blob/main/include/sdu_estimators/integrator/integrator.hpp
+     */
+    enum class IntegrationMethod
+    {
+      Euler,
+      RK2,
+      RK4
+    };
+
+    template <typename T, int32_t DIM_N, int32_t DIM_P>
+    class Integrator
+    {
+        public:
+            using State = Eigen::Matrix<T, DIM_N, DIM_P>;
+
+            static State integrate(
+                const State & y,
+                const std::function<State(
+                    const State&
+                )> & get_dydt,
+                float delta,
+                IntegrationMethod method = IntegrationMethod::RK2
+            )
+            {
+                State outval;
+
+                switch (method) 
+                {
+                    case IntegrationMethod::Euler:
+                        outval = integrate_euler(y, get_dydt, delta);
+                        // std::cout << "euler" << std::endl;
+                        break;
+
+                    case IntegrationMethod::RK2:
+                        outval = integrate_rk2(y, get_dydt, delta);
+                        // std::cout << "rk2" << std::endl;
+                        break;
+
+                    case IntegrationMethod::RK4:
+                        outval = integrate_rk4(y, get_dydt, delta);
+                        // std::cout << "rk4" << std::endl;
+                        break;
+                }
+
+                return outval;
+            }
+
+        private:
+            static State integrate_euler(
+                const State & y,
+                const std::function<State(
+                    const State&
+                )> & get_dydt,
+                float delta)
+            {
+                return y + delta * get_dydt(y);
+            }
+
+            static State integrate_rk2(
+                const State & y,
+                const std::function<State(
+                    const State&
+                )> & get_dydt,
+                float delta)
+            {
+                State k1 = delta * get_dydt(y);
+                State k2 = delta * get_dydt(y + 0.5 * k1);
+                return y + k2;
+            }
+
+            static State integrate_rk4(
+                const State & y,
+                const std::function<State(
+                    const State&
+                )> & get_dydt,
+                float delta)
+            {
+                State k1 = delta * get_dydt(y);
+                State k2 = delta * get_dydt(y + 0.5 * k1);
+                State k3 = delta * get_dydt(y + 0.5 * k2);
+                State k4 = delta * get_dydt(y + k3);
+
+                return y + (k1 + 2 * k2 + 2 * k3 + k4) / 6.;
+            }
+    };
+}
+
+#endif  //INTEGRATOR_HPP

python/CMakeLists.txt

@@ -1,8 +1,10 @@
 # Compile the nanobind module
 nanobind_add_module(
   _sdu_controllers sdu_controllers/_sdu_controllers.cpp
-  sdu_controllers/_robot_models.cpp sdu_controllers/_math.cpp
-  sdu_controllers/_kinematics.cpp)
+  sdu_controllers/_robot_models.cpp 
+  sdu_controllers/_math.cpp
+  sdu_controllers/_kinematics.cpp
+  sdu_controllers/_integrator.cpp)
 
 target_link_libraries(_sdu_controllers PUBLIC sdu_controllers)
 

python/sdu_controllers/__init__.py

@@ -3,7 +3,8 @@
     "models",
     "controllers",
     "math",
-    "kinematics"
+    "kinematics",
+    "integrator"
 )
 
 import numpy as _np
@@ -17,6 +18,7 @@
 from ._sdu_controllers import kinematics
 from ._sdu_controllers import math
 from ._sdu_controllers import utils
+from ._sdu_controllers import integrator
 
 # Add the module's config directory to ConfigFolder search paths
 import pathlib as _pathlib
@@ -51,7 +53,7 @@
 #     return PIDController_(Kp, Ki, Kd, N, dt)
 
 class _PIDController(_original_PIDController):
     def __init__(self, Kp, Ki, Kd, N, dt):
         args = [Kp, Ki, Kd, N]
 
         for i, arg in enumerate(args):

python/sdu_controllers/_integrator.cpp

@@ -0,0 +1,48 @@
+// Python bindings for sdu_controller integrator functions
+
+#include <nanobind/nanobind.h>
+#include <nanobind/ndarray.h>
+#include <nanobind/eigen/dense.h>
+#include <nanobind/stl/function.h> 
+#include <nanobind/stl/vector.h>
+
+#include <sdu_controllers/integrator/integrator.hpp>
+
+namespace nb = nanobind;
+
+namespace sdu_controllers
+{
+    template <typename T, int32_t DIM_N, int32_t DIM_P>
+    void nb_IntegratorClass(nb::module_ & m)
+    {
+        std::string typestr;
+        typestr = "_" + std::to_string(DIM_N) + "x" + std::to_string(DIM_P);
+
+        using Class = integrator::Integrator<T, DIM_N, DIM_P>;
+        using State = Eigen::Matrix<T, DIM_N, DIM_P>;
+
+        std::string nbclass_name = std::string("integrate") + typestr;
+
+        m.def(nbclass_name.c_str(),
+            &Class::integrate,
+            nb::arg("state"),
+            nb::arg("get_dydt"),
+            nb::arg("delta"),
+            nb::arg("method"));
+    }
+
+    nb::module_ create_integrator_module(nb::module_ & main_module)
+    {
+        nb::module_ m = main_module.def_submodule("integrator", "Submodule containing definitions for integrators.");
+        m.doc() = "Python bindings for sdu_controllers integrator utilities.";
+
+        nb::enum_<integrator::IntegrationMethod>(m, "IntegrationMethod")
+            .value("Euler", integrator::IntegrationMethod::Euler)
+            .value("RK2", integrator::IntegrationMethod::RK2)
+            .value("RK4", integrator::IntegrationMethod::RK4);
+
+        nb_IntegratorClass<double, 3, 1>(m);
+
+        return m;
+    }
+}

python/sdu_controllers/_sdu_controllers.cpp

@@ -18,13 +18,18 @@
 #include <sdu_controllers/kinematics/forward_kinematics.hpp>
 #include <sdu_controllers/sdu_controllers.hpp>
 
+// integrator
+#include <sdu_controllers/integrator/integrator.hpp>
+
+
 namespace nb = nanobind;
 
 namespace sdu_controllers
 {
   nb::module_ create_robot_models_module(nb::module_ &main_module);
   nb::module_ create_math_module(nb::module_ &main_module);
   nb::module_ create_kinematics_module(nb::module_ &main_module);
+  nb::module_ create_integrator_module(nb::module_ &main_module);
 
   NB_MODULE(_sdu_controllers, m)
   {
@@ -35,6 +40,7 @@ namespace sdu_controllers
     nb::module_ m_math = create_math_module(m);
     nb::module_ m_kinematics = create_kinematics_module(m);
     nb::module_ m_utils = m.def_submodule("utils", "Submodule containing utility functions.");
+    nb::module_ m_integrator = create_integrator_module(m);
 
     // utils
     nb::class_<utils::ConfigFolder>(m_utils, "ConfigFolder")
@@ -88,6 +94,7 @@ namespace sdu_controllers
 
     nb::class_<controllers::AdmittanceControllerPosition>(m_controllers, "AdmittanceControllerPosition")
         .def(nb::init<const double>(), nb::arg("frequency"))
+        .def(nb::init<const double, integrator::IntegrationMethod>(), nb::arg("frequency"), nb::arg("integration_method"))
         .def("step", &controllers::AdmittanceControllerPosition::step)
         .def("get_output", &controllers::AdmittanceControllerPosition::get_output)
         .def("reset", &controllers::AdmittanceControllerPosition::reset)

src/controllers/admittance_controller_position.cpp

@@ -1,10 +1,11 @@
 #include <sdu_controllers/controllers/admittance_controller_position.hpp>
 #include <sdu_controllers/math/math.hpp>
 
-
 namespace sdu_controllers::controllers
 {
-  AdmittanceControllerPosition::AdmittanceControllerPosition(const double frequency)
+  AdmittanceControllerPosition::AdmittanceControllerPosition(const double frequency, 
+    const integrator::IntegrationMethod intg_method)
+    : intg_method(intg_method)
   {
     // Specification of impedance-parameters
     const Eigen::Vector3d M_vec{ 22.5, 22.5, 22.5 };  // Positional mass
@@ -31,12 +32,43 @@
   {
     // -- Compute positional error --
 
+    /* Old code
     // Acceleration error
     Eigen::Vector3d ddx_e_ = M_.inverse() * (input_force - K_ * x_e_ - D_ * dx_e_);
 
     // Integrate velocity and position error
     dx_e_ += ddx_e_ * dt_;
     x_e_ += dx_e_ * dt_;
+    */
+
+    // Integrate using integrator.hpp
+    // Define get_dydt function
+    auto get_dydt = [=](Eigen::Vector<double, 6> x_e_dx_e_)
+    {
+      Eigen::Vector3d _x_e = x_e_dx_e_(Eigen::seqN(0, 3));
+      Eigen::Vector3d _dx_e = x_e_dx_e_(Eigen::seqN(3, 3));
+
+      Eigen::Vector3d _ddx_e = M_.inverse() * (input_force - K_ * _x_e - D_ * _dx_e);
+
+      Eigen::Vector<double, 6> return_value;
+      return_value << _dx_e, _ddx_e;
+
+      return return_value;
+    };
+
+    Eigen::Vector<double, 6> current_state;
+    current_state << x_e_, dx_e_;
+
+    Eigen::Vector<double, 6> new_state = integrator::Integrator<double, 6, 1>::integrate(
+      current_state,
+      get_dydt,
+      dt_,
+      intg_method
+    );
+
+    x_e_ = new_state(Eigen::seqN(0, 3));
+    dx_e_ = new_state(Eigen::seqN(3, 3));
+    // End of integration
 
     // -- Compute rotational error --
     Eigen::Matrix3d E = (quat_e_.w() * rot_identity_) - math::skew(quat_e_.vec());