Rename FloatingBaseSystemKinematics to FloatingBaseSystemVelocityKine…

…matics and update related references
ami-iit · Feb 13, 2025 · 9a0daac · 9a0daac
1 parent 9348058
commit 9a0daac
Show file tree

Hide file tree

Showing 16 changed files with 85 additions and 75 deletions.
diff --git a/bindings/python/ContinuousDynamicalSystem/CMakeLists.txt b/bindings/python/ContinuousDynamicalSystem/CMakeLists.txt
@@ -6,8 +6,8 @@ if(TARGET BipedalLocomotion::ContinuousDynamicalSystem)
 
   add_bipedal_locomotion_python_module(
     NAME ContinuousDynamicalSystemBindings
-    SOURCES src/MultiStateWeightProvider.cpp src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/CentroidalDynamics.cpp src/ButterworthLowPassFilter.cpp src/Module.cpp
-    HEADERS ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/LinearTimeInvariantSystem.h ${H_PREFIX}/FloatingBaseSystemKinematics.h ${H_PREFIX}/Integrator.h
+    SOURCES src/MultiStateWeightProvider.cpp src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemVelocityKinematics.cpp src/CentroidalDynamics.cpp src/ButterworthLowPassFilter.cpp src/Module.cpp
+    HEADERS ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/LinearTimeInvariantSystem.h ${H_PREFIX}/FloatingBaseSystemVelocityKinematics.h ${H_PREFIX}/Integrator.h
             ${H_PREFIX}/MultiStateWeightProvider.h ${H_PREFIX}/CentroidalDynamics.h ${H_PREFIX}/ButterworthLowPassFilter.h
             ${H_PREFIX}/Module.h
     LINK_LIBRARIES BipedalLocomotion::ContinuousDynamicalSystem

diff --git a/...icalSystem/FloatingBaseSystemKinematics.h → ...em/FloatingBaseSystemVelocityKinematics.h b/...icalSystem/FloatingBaseSystemKinematics.h → ...em/FloatingBaseSystemVelocityKinematics.h
@@ -1,5 +1,5 @@
 /**
- * @file FloatingBaseSystemKinematics.h
+ * @file FloatingBaseSystemVelocityKinematics.h
  * @authors Giulio Romualdi
  * @copyright 2022 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
@@ -17,7 +17,7 @@ namespace bindings
 namespace ContinuousDynamicalSystem
 {
 
-void CreateFloatingBaseSystemKinematics(pybind11::module& module);
+void CreateFloatingBaseSystemVelocityKinematics(pybind11::module& module);
 
 } // namespace ContinuousDynamicalSystem
 } // namespace bindings

diff --git a/...stem/src/FloatingBaseSystemKinematics.cpp → .../FloatingBaseSystemVelocityKinematics.cpp b/...stem/src/FloatingBaseSystemKinematics.cpp → .../FloatingBaseSystemVelocityKinematics.cpp
@@ -1,15 +1,15 @@
 /**
- * @file FloatingBaseSystemKinematics.cpp
+ * @file FloatingBaseSystemVelocityKinematics.cpp
  * @authors Giulio Romualdi
  * @copyright 2022 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 
 #include <BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h>
-#include <BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemVelocityKinematics.h>
 
 #include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/DynamicalSystem.h>
-#include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h>
+#include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/FloatingBaseSystemVelocityKinematics.h>
 #include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/Integrator.h>
 
 #include <pybind11/eigen.h>
@@ -22,23 +22,23 @@ namespace bindings
 namespace ContinuousDynamicalSystem
 {
 
-void CreateFloatingBaseSystemKinematics(pybind11::module& module)
+void CreateFloatingBaseSystemVelocityKinematics(pybind11::module& module)
 {
     using namespace BipedalLocomotion::ContinuousDynamicalSystem;
     namespace py = ::pybind11;
 
-    constexpr auto name = "FloatingBaseSystemKinematics";
+    constexpr auto name = "FloatingBaseSystemVelocityKinematics";
 
-    CreateDynamicalSystem<DynamicalSystem<FloatingBaseSystemKinematics>,
-                          std::shared_ptr<DynamicalSystem<FloatingBaseSystemKinematics>>> //
+    CreateDynamicalSystem<DynamicalSystem<FloatingBaseSystemVelocityKinematics>,
+                          std::shared_ptr<DynamicalSystem<FloatingBaseSystemVelocityKinematics>>> //
         (module, name);
 
-    py::class_<FloatingBaseSystemKinematics,
-               DynamicalSystem<FloatingBaseSystemKinematics>,
-               std::shared_ptr<FloatingBaseSystemKinematics>>(module, name)
+    py::class_<FloatingBaseSystemVelocityKinematics,
+               DynamicalSystem<FloatingBaseSystemVelocityKinematics>,
+               std::shared_ptr<FloatingBaseSystemVelocityKinematics>>(module, name)
         .def(py::init());
 
-    CreateForwardEulerIntegrator<FloatingBaseSystemKinematics>(module, name);
+    CreateForwardEulerIntegrator<FloatingBaseSystemVelocityKinematics>(module, name);
 }
 } // namespace ContinuousDynamicalSystem
 } // namespace bindings

diff --git a/bindings/python/ContinuousDynamicalSystem/src/Module.cpp b/bindings/python/ContinuousDynamicalSystem/src/Module.cpp
@@ -8,7 +8,7 @@
 #include <pybind11/pybind11.h>
 
 #include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/CentroidalDynamics.h>
-#include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h>
+#include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/FloatingBaseSystemVelocityKinematics.h>
 #include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h>
 #include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/Module.h>
 #include <BipedalLocomotion/bindings/ContinuousDynamicalSystem/MultiStateWeightProvider.h>
@@ -26,7 +26,7 @@ void CreateModule(pybind11::module& module)
                    "BipedalLocomotion::ContinuousDynamicalSystem";
 
     CreateLinearTimeInvariantSystem(module);
-    CreateFloatingBaseSystemKinematics(module);
+    CreateFloatingBaseSystemVelocityKinematics(module);
     CreateMultiStateWeightProvider(module);
     CreateCentroidalDynamics(module);
     CreateButterworthLowPassFilter(module);

diff --git a/bindings/python/ContinuousDynamicalSystem/tests/test_floating_base_system_kinematics.py b/bindings/python/ContinuousDynamicalSystem/tests/test_floating_base_system_kinematics.py
@@ -27,11 +27,11 @@ def test_floating_base_system_kinematics():
     joint_position0 = np.zeros(23)
 
 
-    system = blf.continuous_dynamical_system.FloatingBaseSystemKinematics()
+    system = blf.continuous_dynamical_system.FloatingBaseSystemVelocityKinematics()
     system.state = position0, rotation0, joint_position0
     assert system.set_control_input((twist, joint_velocity))
 
-    integrator = blf.continuous_dynamical_system.FloatingBaseSystemKinematicsForwardEulerIntegrator()
+    integrator = blf.continuous_dynamical_system.FloatingBaseSystemVelocityKinematicsForwardEulerIntegrator()
     assert integrator.set_dynamical_system(system)
     integrator.integration_step = timedelta(seconds=dt)
 

diff --git a/examples/InverseKinematics/inverse_kinematics.ipynb b/examples/InverseKinematics/inverse_kinematics.ipynb
@@ -77,10 +77,10 @@
    "source": [
     "class Simulator:\n",
     "    def __init__(self, initial_config, dt):\n",
-    "        self.system = blf.continuous_dynamical_system.FloatingBaseSystemKinematics()\n",
+    "        self.system = blf.continuous_dynamical_system.FloatingBaseSystemVelocityKinematics()\n",
     "        self.system.set_state(([0,0,0],  manif.SO3.Identity(), initial_config))\n",
     "        \n",
-    "        self.integrator = blf.continuous_dynamical_system.FloatingBaseSystemKinematicsForwardEulerIntegrator()\n",
+    "        self.integrator = blf.continuous_dynamical_system.FloatingBaseSystemVelocityKinematicsForwardEulerIntegrator()\n",
     "        self.integrator.set_dynamical_system(self.system)\n",
     "        assert self.integrator.set_integration_step(dt)\n",
     "        self.dt = dt\n",

diff --git a/src/ContinuousDynamicalSystem/CMakeLists.txt b/src/ContinuousDynamicalSystem/CMakeLists.txt
@@ -11,15 +11,15 @@ if(FRAMEWORK_COMPILE_ContinuousDynamicalSystem)
   add_bipedal_locomotion_library(
     NAME                   ContinuousDynamicalSystem
     PUBLIC_HEADERS         ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/LinearTimeInvariantSystem.h
-                           ${H_PREFIX}/FloatingBaseSystemKinematics.h ${H_PREFIX}/FloatingBaseDynamicsWithCompliantContacts.h ${H_PREFIX}/FixedBaseDynamics.h ${H_PREFIX}/FirstOrderSmoother.h
+                           ${H_PREFIX}/FloatingBaseSystemVelocityKinematics.h ${H_PREFIX}/FloatingBaseDynamicsWithCompliantContacts.h ${H_PREFIX}/FixedBaseDynamics.h ${H_PREFIX}/FirstOrderSmoother.h
                            ${H_PREFIX}/CentroidalDynamics.h
                            ${H_PREFIX}/LieGroupDynamics.h ${H_PREFIX}/SO3Dynamics.h
                            ${H_PREFIX}/Integrator.h  ${H_PREFIX}/FixedStepIntegrator.h ${H_PREFIX}/ForwardEuler.h ${H_PREFIX}/RK4.h
                            ${H_PREFIX}/CompliantContactWrench.h
                            ${H_PREFIX}/MultiStateWeightProvider.h
                            ${H_PREFIX}/ButterworthLowPassFilter.h
     PRIVATE_HEADERS        ${H_PREFIX}/impl/traits.h
-    SOURCES                src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/CompliantContactWrench.cpp
+    SOURCES                src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemVelocityKinematics.cpp src/CompliantContactWrench.cpp
                            src/FloatingBaseDynamicsWithCompliantContacts.cpp src/FixedBaseDynamics.cpp src/CentroidalDynamics.cpp
                            src/FirstOrderSmoother.cpp src/MultiStateWeightProvider.cpp
                            src/ButterworthLowPassFilter.cpp

diff --git a/...icalSystem/FloatingBaseSystemKinematics.h → ...em/FloatingBaseSystemVelocityKinematics.h b/...icalSystem/FloatingBaseSystemKinematics.h → ...em/FloatingBaseSystemVelocityKinematics.h
@@ -1,5 +1,5 @@
 /**
- * @file FloatingBaseSystemKinematics.h
+ * @file FloatingBaseSystemVelocityKinematics.h
  * @authors Giulio Romualdi
  * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
@@ -25,24 +25,25 @@ namespace ContinuousDynamicalSystem
 {
 
 // Forward declare for type traits specialization
-class FloatingBaseSystemKinematics;
-}
-}
+class FloatingBaseSystemVelocityKinematics;
+} // namespace ContinuousDynamicalSystem
+} // namespace BipedalLocomotion
 
 namespace BipedalLocomotion::ContinuousDynamicalSystem::internal
 {
-template <> struct traits<FloatingBaseSystemKinematics>
+template <> struct traits<FloatingBaseSystemVelocityKinematics>
 {
     using Twist = Eigen::Matrix<double, 6, 1>;
     using State = GenericContainer::named_tuple<BLF_NAMED_PARAM(p, Eigen::Vector3d),
-                                      BLF_NAMED_PARAM(R, manif::SO3d),
-                                      BLF_NAMED_PARAM(s, Eigen::VectorXd)>;
-    using StateDerivative = GenericContainer::named_tuple<BLF_NAMED_PARAM(dp, Eigen::Vector3d),
-                                                BLF_NAMED_PARAM(omega, manif::SO3d::Tangent),
-                                                BLF_NAMED_PARAM(ds, Eigen::VectorXd)>;
+                                                BLF_NAMED_PARAM(R, manif::SO3d),
+                                                BLF_NAMED_PARAM(s, Eigen::VectorXd)>;
+    using StateDerivative
+        = GenericContainer::named_tuple<BLF_NAMED_PARAM(dp, Eigen::Vector3d),
+                                        BLF_NAMED_PARAM(omega, manif::SO3d::Tangent),
+                                        BLF_NAMED_PARAM(ds, Eigen::VectorXd)>;
     using Input = GenericContainer::named_tuple<BLF_NAMED_PARAM(twist, Twist),
-                                      BLF_NAMED_PARAM(ds, Eigen::VectorXd)>;
-    using DynamicalSystem = FloatingBaseSystemKinematics;
+                                                BLF_NAMED_PARAM(ds, Eigen::VectorXd)>;
+    using DynamicalSystem = FloatingBaseSystemVelocityKinematics;
 };
 } // namespace BipedalLocomotion::ContinuousDynamicalSystem::internal
 
@@ -52,29 +53,34 @@ namespace ContinuousDynamicalSystem
 {
 
 /**
- * FloatingBaseSystemKinematics describes a floating base system kinematics.
- * The FloatingBaseSystemKinematics inherits from a generic DynamicalSystem where the State is
- * described by a BipedalLocomotion::GenericContainer::named_tuple
- * | Name |        Type       |                                                                  Description                                                                 |
+ * FloatingBaseSystemVelocityKinematics describes a floating base system kinematics.
+ * The FloatingBaseSystemVelocityKinematics inherits from a generic DynamicalSystem where the State
+ * is described by a BipedalLocomotion::GenericContainer::named_tuple | Name |        Type       |
+ * Description                                                                 |
  * |:----:|:-----------------:|:--------------------------------------------------------------------------------------------------------------------------------------------:|
- * |  `p` | `Eigen::Vector3d` |                                                Position of the base w.r.t. the inertial frame                                                |
- * |  `R` |   `manif::SO3d`   | Rotation matrix \f${} ^ I R _ {b}\f$. Matrix that transform a vector whose coordinates are expressed in the base frame in the inertial frame |
- * |  `s` | `Eigen::VectorXd` |                                                           Joint positions [in rad]                                                           |
+ * |  `p` | `Eigen::Vector3d` |                                                Position of the base
+ * w.r.t. the inertial frame                                                | |  `R` | `manif::SO3d`
+ * | Rotation matrix \f${} ^ I R _ {b}\f$. Matrix that transform a vector whose coordinates are
+ * expressed in the base frame in the inertial frame | |  `s` | `Eigen::VectorXd` | Joint positions
+ * [in rad]                                                           |
  *
  * The `StateDerivative` is described by a BipedalLocomotion::GenericContainer::named_tuple
- * |   Name  |          Type          |                                                         Description                                                         |
+ * |   Name  |          Type          | Description |
  * |:-------:|:----------------------:|:---------------------------------------------------------------------------------------------------------------------------:|
- * |  `dp`   |    `Eigen::Vector3d`   | Linear velocity of the origin of the base link whose coordinates are expressed in the Inertial frame (MIXED RERPESENTATION) |
- * | `omega` | `manif::SO3d::Tangent` |                base angular velocity whose coordinates are expressed in the inertial frame (Left trivialized)               |
- * |   `ds`  |    `Eigen::VectorXd`   |                                                 Joint velocities [in rad/s]                                                 |
+ * |  `dp`   |    `Eigen::Vector3d`   | Linear velocity of the origin of the base link whose
+ * coordinates are expressed in the Inertial frame (MIXED RERPESENTATION) | | `omega` |
+ * `manif::SO3d::Tangent` |                base angular velocity whose coordinates are expressed in
+ * the inertial frame (Left trivialized)               | |   `ds`  |    `Eigen::VectorXd`   | Joint
+ * velocities [in rad/s]                                                 |
  *
  * The `Input` is described by a BipedalLocomotion::GenericContainer::named_tuple
  * |   Name  |              Type             |                  Description                 |
  * |:-------:|:-----------------------------:|:--------------------------------------------:|
  * | `twist` | `Eigen::Matrix<double, 6, 1>` | Base twist expressed in mixed representation |
  * |   `ds`  |       `Eigen::VectorXd`       |          Joint velocities [in rad/s]         |
  */
-class FloatingBaseSystemKinematics : public DynamicalSystem<FloatingBaseSystemKinematics>
+class FloatingBaseSystemVelocityKinematics
+    : public DynamicalSystem<FloatingBaseSystemVelocityKinematics>
 {
     State m_state;
     Input m_controlInput;
@@ -119,6 +125,10 @@ class FloatingBaseSystemKinematics : public DynamicalSystem<FloatingBaseSystemKi
     bool dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative);
 };
 
+using FloatingBaseSystemKinematics [[deprecated("Use FloatingBaseSystemVelocityKinematics "
+                                                "instead.")]]
+= FloatingBaseSystemVelocityKinematics;
+
 } // namespace ContinuousDynamicalSystem
 } // namespace BipedalLocomotion
 

diff --git a/...stem/src/FloatingBaseSystemKinematics.cpp → .../FloatingBaseSystemVelocityKinematics.cpp b/...stem/src/FloatingBaseSystemKinematics.cpp → .../FloatingBaseSystemVelocityKinematics.cpp
@@ -1,19 +1,19 @@
 /**
- * @file FloatingBaseSystemKinematics.cpp
+ * @file FloatingBaseSystemVelocityKinematics.cpp
  * @authors Giulio Romualdi
  * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
  */
 
-#include <BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemVelocityKinematics.h>
 #include <BipedalLocomotion/TextLogging/Logger.h>
 
 using namespace BipedalLocomotion::ContinuousDynamicalSystem;
 using namespace BipedalLocomotion::ParametersHandler;
 
-bool FloatingBaseSystemKinematics::initialize(std::weak_ptr<const IParametersHandler> handler)
+bool FloatingBaseSystemVelocityKinematics::initialize(std::weak_ptr<const IParametersHandler> handler)
 {
-    constexpr auto logPrefix = "[FloatingBaseSystemKinematics::initialize]";
+    constexpr auto logPrefix = "[FloatingBaseSystemVelocityKinematics::initialize]";
 
     auto ptr = handler.lock();
     if (ptr != nullptr)
@@ -30,7 +30,7 @@ bool FloatingBaseSystemKinematics::initialize(std::weak_ptr<const IParametersHan
     return true;
 }
 
-bool FloatingBaseSystemKinematics::dynamics(const std::chrono::nanoseconds& time,
+bool FloatingBaseSystemVelocityKinematics::dynamics(const std::chrono::nanoseconds& time,
                                             StateDerivative& stateDerivative)
 {
     using namespace BipedalLocomotion::GenericContainer::literals;
@@ -51,7 +51,7 @@ bool FloatingBaseSystemKinematics::dynamics(const std::chrono::nanoseconds& time
     // check the size of the vectors
     if (jointVelocity.size() != jointPositions.size())
     {
-        log()->error("[FloatingBaseSystemKinematics::dynamics] Wrong size of the vectors.");
+        log()->error("[FloatingBaseSystemVelocityKinematics::dynamics] Wrong size of the vectors.");
         return false;
     }
 
@@ -66,18 +66,18 @@ bool FloatingBaseSystemKinematics::dynamics(const std::chrono::nanoseconds& time
     return true;
 }
 
-bool FloatingBaseSystemKinematics::setState(const FloatingBaseSystemKinematics::State& state)
+bool FloatingBaseSystemVelocityKinematics::setState(const FloatingBaseSystemVelocityKinematics::State& state)
 {
     m_state = state;
     return true;
 }
 
-const FloatingBaseSystemKinematics::State& FloatingBaseSystemKinematics::getState() const
+const FloatingBaseSystemVelocityKinematics::State& FloatingBaseSystemVelocityKinematics::getState() const
 {
     return m_state;
 }
 
-bool FloatingBaseSystemKinematics::setControlInput(const Input& controlInput)
+bool FloatingBaseSystemVelocityKinematics::setControlInput(const Input& controlInput)
 {
     m_controlInput = controlInput;
     return true;

diff --git a/src/ContinuousDynamicalSystem/tests/CMakeLists.txt b/src/ContinuousDynamicalSystem/tests/CMakeLists.txt
@@ -10,8 +10,8 @@ add_bipedal_test(
 
 
 add_bipedal_test(
-  NAME IntegratorFloatingBaseSystemKinematicsTest
-  SOURCES IntegratorFloatingBaseSystemKinematics.cpp
+  NAME IntegratorFloatingBaseSystemVelocityKinematicsTest
+  SOURCES IntegratorFloatingBaseSystemVelocityKinematics.cpp
   LINKS BipedalLocomotion::ContinuousDynamicalSystem
         BipedalLocomotion::TestUtils Eigen3::Eigen)
 

diff --git a/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp b/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp
@@ -1,5 +1,5 @@
 /**
- * @file IntegratorFloatingBaseSystemKinematics.cpp
+ * @file IntegratorFloatingBaseSystemVelocityKinematics.cpp
  * @authors Giulio Romualdi
  * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
@@ -15,7 +15,7 @@
 
 #include <manif/SO3.h>
 
-#include <BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemVelocityKinematics.h>
 #include <BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h>
 #include <BipedalLocomotion/TestUtils/MemoryAllocationMonitor.h>
 
@@ -29,7 +29,7 @@ TEST_CASE("Integrator - Linear system")
     constexpr double tolerance = 1e-3;
     constexpr std::chrono::nanoseconds simulationTime = 500ms;
 
-    auto system = std::make_shared<FloatingBaseSystemKinematics>();
+    auto system = std::make_shared<FloatingBaseSystemVelocityKinematics>();
 
     Eigen::Matrix<double, 6, 1> twist;
     twist.setRandom();
@@ -58,7 +58,7 @@ TEST_CASE("Integrator - Linear system")
 
     system->setState({position0, rotation0, jointPosition0});
 
-    ForwardEuler<FloatingBaseSystemKinematics> integrator;
+    ForwardEuler<FloatingBaseSystemVelocityKinematics> integrator;
     integrator.setIntegrationStep(dT);
     integrator.setDynamicalSystem(system);