FIX: gravity computation using the robot state

frankaemika · Mar 25, 2022 · 248c544 · 248c544
1 parent cac09ea
commit 248c544
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 4 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,6 +1,6 @@
 # CHANGELOG
 
-## 0.9.0 - 2022-04-24
+## 0.9.0 - UNRELEASED
 
 Requires Panda system version >= 4.2.1
 

diff --git a/include/franka/model.h b/include/franka/model.h
@@ -263,13 +263,20 @@ class Model {
    *
    * @param[in] robot_state State from which the gravity vector should be calculated.
    * @param[in] gravity_earth Earth's gravity vector. Unit: \f$\frac{m}{s^2}\f$.
-   * Default to {0.0, 0.0, -9.81}.
    *
    * @return Gravity vector.
    */
   std::array<double, 7> gravity(const franka::RobotState& robot_state,
-                                const std::array<double, 3>& gravity_earth = {
-                                    {0., 0., -9.81}}) const noexcept;
+                                const std::array<double, 3>& gravity_earth) const noexcept;
+
+  /**
+   * Calculates the gravity vector using the robot state. Unit: \f$[Nm]\f$.
+   *
+   * @param[in] robot_state State from which the gravity vector should be calculated.
+   *
+   * @return Gravity vector.
+   */
+  std::array<double, 7> gravity(const franka::RobotState& robot_state) const noexcept;
 
   /// @cond DO_NOT_DOCUMENT
   Model(const Model&) = delete;

diff --git a/src/model.cpp b/src/model.cpp
@@ -228,6 +228,10 @@ std::array<double, 7> franka::Model::gravity(const franka::RobotState& robot_sta
   return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, gravity_earth);
 };
 
+std::array<double, 7> franka::Model::gravity(const franka::RobotState& robot_state) const noexcept {
+  return gravity(robot_state, robot_state.O_ddP_O);
+};
+
 std::array<double, 7> franka::Model::gravity(
     const std::array<double, 7>& q,
     double m_total,