[pre-commit] Run pre-commit locally

CHANGELOG.md

@@ -4,32 +4,32 @@ All notable changes to this project will be documented in this file.
 
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
-## [Unreleased]
+## \[Unreleased\]
 
-* Supported fixed-based robots in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/17
-* Add inertial parameters publisher/subscriber and extend whole-body state publisher with accelerations in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/16
+- Supported fixed-based robots in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/17
+- Add inertial parameters publisher/subscriber and extend whole-body state publisher with accelerations in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/16
 
-## [1.3.1] - 2024-01-27
+## \[1.3.1\] - 2024-01-27
 
-* Enabled to publish whole-body state and trajectory without the need of passing contact info in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/15
+- Enabled to publish whole-body state and trajectory without the need of passing contact info in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/15
 
-## [1.2.1] - 2023-12-13
+## \[1.2.1\] - 2023-12-13
 
-* Extended CI jobs in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/12
+- Extended CI jobs in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/12
 
-## [1.2.0] - 2023-09-22
+## \[1.2.0\] - 2023-09-22
 
-* Fixed bug in init functions for empty locked joints in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/11
-* Introduced locked joints in publishers and subscribers in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/10
+- Fixed bug in init functions for empty locked joints in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/11
+- Introduced locked joints in publishers and subscribers in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/10
 
-## [1.0.1] - 2023-08-24
+## \[1.0.1\] - 2023-08-24
 
-* Used ROS to print starting message and fixed bug when using reduced models in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/9
-* Developed other unit tests with Pinocchio in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/8
+- Used ROS to print starting message and fixed bug when using reduced models in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/9
+- Developed other unit tests with Pinocchio in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/8
 
-## [1.0.0] - 2023-07-07
+## \[1.0.0\] - 2023-07-07
 
-* Supported ROS 2 in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/7
-* Integrated pre-commit in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/6
-* Replaced nv_root finding via frames to using the first joint in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/4
-* Integrated CI in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/3
+- Supported ROS 2 in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/7
+- Integrated pre-commit in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/6
+- Replaced nv_root finding via frames to using the first joint in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/4
+- Integrated CI in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/3

include/crocoddyl_msgs/conversions.h

@@ -107,7 +107,9 @@ template <int Options, template <typename, int> class JointCollectionTpl>
 static inline std::size_t getRootNq(
     const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model) {
   const std::size_t root_joint_id = getRootJointId(model);
-  return model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nq() : 0;
+  return model.frames[root_joint_id].name != "universe"
+             ? model.joints[root_joint_id].nq()
+             : 0;
 }
 
 /**
@@ -119,7 +121,9 @@ template <int Options, template <typename, int> class JointCollectionTpl>
 static inline std::size_t getRootNv(
     const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model) {
   const std::size_t root_joint_id = getRootJointId(model);
-  return model.frames[root_joint_id].name != "universe" ? model.joints[root_joint_id].nv() : 0;
+  return model.frames[root_joint_id].name != "universe"
+             ? model.joints[root_joint_id].nv()
+             : 0;
 }
 
 /**
@@ -139,48 +143,58 @@ static inline std::size_t getRootNv(
 template <int Options, template <typename, int> class JointCollectionTpl>
 void updateBodyInertialParameters(
     pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
-    const std::string &frame_name,
-    const Eigen::Ref<const Vector10d> &psi) {
+    const std::string &frame_name, const Eigen::Ref<const Vector10d> &psi) {
   if (model.existFrame(frame_name)) {
     const std::size_t frame_id = model.getFrameId(frame_name);
     switch (model.frames[frame_id].type) {
-      case pinocchio::FrameType::JOINT: {
+    case pinocchio::FrameType::JOINT: {
       const std::size_t joint_id = model.getJointId(frame_name);
       model.inertias[joint_id] = pinocchio::Inertia::FromDynamicParameters(psi);
-      } break;
-      case pinocchio::FrameType::BODY: {
-//TODO: Update Pinocchio version after releasing https://github.com/stack-of-tasks/pinocchio/pull/2204
-#if (PINOCCHIO_MAJOR_VERSION >= 2 && PINOCCHIO_MINOR_VERSION >= 7 && PINOCCHIO_PATCH_VERSION >= 1)
+    } break;
+    case pinocchio::FrameType::BODY: {
+// TODO: Update Pinocchio version after releasing
+// https://github.com/stack-of-tasks/pinocchio/pull/2204
+#if (PINOCCHIO_MAJOR_VERSION >= 2 && PINOCCHIO_MINOR_VERSION >= 7 &&           \
+     PINOCCHIO_PATCH_VERSION >= 1)
       const std::size_t fixed_joint_id = model.frames[frame_id].previousFrame;
       const std::size_t joint_id = model.frames[fixed_joint_id].parent;
-      const pinocchio::SE3& jMb = model.frames[fixed_joint_id].placement;
-      const pinocchio::Inertia& I_updated = pinocchio::Inertia::FromDynamicParameters(psi);
-      const pinocchio::Inertia& dI = I_updated - model.frames[fixed_joint_id].inertia;
+      const pinocchio::SE3 &jMb = model.frames[fixed_joint_id].placement;
+      const pinocchio::Inertia &I_updated =
+          pinocchio::Inertia::FromDynamicParameters(psi);
+      const pinocchio::Inertia &dI =
+          I_updated - model.frames[fixed_joint_id].inertia;
       model.frames[fixed_joint_id].inertia = I_updated;
       model.inertias[joint_id] += jMb.act(dI);
 #else
-      std::invalid_argument("The installed Pinocchio version doesn't support minus operators in inertia needed for " + frame_name);
+      std::invalid_argument("The installed Pinocchio version doesn't support "
+                            "minus operators in inertia needed for " +
+                            frame_name);
 #endif
-      } break;
-      case pinocchio::FrameType::FIXED_JOINT: {
-#if (PINOCCHIO_MAJOR_VERSION >= 2 && PINOCCHIO_MINOR_VERSION >= 7 && PINOCCHIO_PATCH_VERSION >= 1)
+    } break;
+    case pinocchio::FrameType::FIXED_JOINT: {
+#if (PINOCCHIO_MAJOR_VERSION >= 2 && PINOCCHIO_MINOR_VERSION >= 7 &&           \
+     PINOCCHIO_PATCH_VERSION >= 1)
       const std::size_t joint_id = model.frames[frame_id].parent;
-      const pinocchio::SE3& jMb = model.frames[frame_id].placement;
-      const pinocchio::Inertia& I_updated = pinocchio::Inertia::FromDynamicParameters(psi);
-      const pinocchio::Inertia& dI = I_updated - model.frames[frame_id].inertia;
+      const pinocchio::SE3 &jMb = model.frames[frame_id].placement;
+      const pinocchio::Inertia &I_updated =
+          pinocchio::Inertia::FromDynamicParameters(psi);
+      const pinocchio::Inertia &dI = I_updated - model.frames[frame_id].inertia;
       model.frames[frame_id].inertia = I_updated;
       model.inertias[joint_id] += jMb.act(dI);
 #else
-      std::invalid_argument("The installed Pinocchio version doesn't support minus operators in inertia needed for " + frame_name);
+      std::invalid_argument("The installed Pinocchio version doesn't support "
+                            "minus operators in inertia needed for " +
+                            frame_name);
 #endif
-      } break;
-      default: {
-        std::invalid_argument("The type of frame " + frame_name + " is not supported");
-        break;
-      }
+    } break;
+    default: {
+      std::invalid_argument("The type of frame " + frame_name +
+                            " is not supported");
+      break;
+    }
     }
   } else {
-    std::invalid_argument("Doesn't exist " + frame_name + " frame");     
+    std::invalid_argument("Doesn't exist " + frame_name + " frame");
   }
 }
 
@@ -193,37 +207,37 @@ void updateBodyInertialParameters(
  * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the
  * barycenter. Additionally, the type of frame supported are joints,
  * fixed joints, and bodies
- * 
+ *
  * @param model[in]      Pinocchio model
  * @param frame_name[in] Frame name
  * @return inertial parameters.
  */
 template <int Options, template <typename, int> class JointCollectionTpl>
-const Vector10d
-getBodyInertialParameters(
+const Vector10d getBodyInertialParameters(
     const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
     const std::string &frame_name) {
   if (model.existFrame(frame_name)) {
     const std::size_t frame_id = model.getFrameId(frame_name);
     switch (model.frames[frame_id].type) {
-      case pinocchio::FrameType::JOINT: {
+    case pinocchio::FrameType::JOINT: {
       const std::size_t joint_id = model.getJointId(frame_name);
       return model.inertias[joint_id].toDynamicParameters();
-      } break;
-      case pinocchio::FrameType::BODY: {
+    } break;
+    case pinocchio::FrameType::BODY: {
       const std::size_t fixed_joint_id = model.frames[frame_id].previousFrame;
       return model.frames[fixed_joint_id].inertia.toDynamicParameters();
-      } break;
-      case pinocchio::FrameType::FIXED_JOINT: {
+    } break;
+    case pinocchio::FrameType::FIXED_JOINT: {
       return model.frames[frame_id].inertia.toDynamicParameters();
-      } break;
-      default: {
-        std::invalid_argument("The type of frame " + frame_name + " is not supported");
-        break;
-      }
+    } break;
+    default: {
+      std::invalid_argument("The type of frame " + frame_name +
+                            " is not supported");
+      break;
+    }
     }
   } else {
-    std::invalid_argument("Doesn't exist " + frame_name + " frame");     
+    std::invalid_argument("Doesn't exist " + frame_name + " frame");
   }
 }
 
@@ -656,7 +670,7 @@ static inline void fromMsg(const Control &msg, Eigen::Ref<Eigen::VectorXd> u,
 }
 
 /**
- * @brief Conversion of inertial parameters from a 
+ * @brief Conversion of inertial parameters from a
  * crocoddyl_msgs::BodyInertia message to Eigen
  *
  * @param[in] msg   ROS message that contains the inertial parameters
@@ -880,9 +894,9 @@ static inline void fromReduced(
                                 " but received " + std::to_string(v_in.size()));
   }
   if (static_cast<std::size_t>(tau_out.size()) != model.nv - nv_root) {
-    throw std::invalid_argument(
-        "Expected tau_out to be " + std::to_string(njoints) +
-        " but received " + std::to_string(tau_out.size()));
+    throw std::invalid_argument("Expected tau_out to be " +
+                                std::to_string(njoints) + " but received " +
+                                std::to_string(tau_out.size()));
   }
   if (static_cast<std::size_t>(tau_in.size()) != njoints_reduced) {
     throw std::invalid_argument(
@@ -979,9 +993,9 @@ static inline void fromReduced(
                                 " but received " + std::to_string(a_in.size()));
   }
   if (static_cast<std::size_t>(tau_out.size()) != njoints) {
-    throw std::invalid_argument(
-        "Expected tau_out to be " + std::to_string(njoints) +
-        " but received " + std::to_string(tau_out.size()));
+    throw std::invalid_argument("Expected tau_out to be " +
+                                std::to_string(njoints) + " but received " +
+                                std::to_string(tau_out.size()));
   }
   if (static_cast<std::size_t>(tau_in.size()) != njoints_reduced) {
     throw std::invalid_argument(
@@ -1069,9 +1083,9 @@ toReduced(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
         " but received " + std::to_string(tau_out.size()));
   }
   if (static_cast<std::size_t>(tau_in.size()) != njoints) {
-    throw std::invalid_argument(
-        "Expected tau_in to be " + std::to_string(njoints) +
-        " but received " + std::to_string(tau_in.size()));
+    throw std::invalid_argument("Expected tau_in to be " +
+                                std::to_string(njoints) + " but received " +
+                                std::to_string(tau_in.size()));
   }
 
   typedef pinocchio::ModelTpl<double, Options, JointCollectionTpl> Model;
@@ -1160,9 +1174,9 @@ toReduced(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
         " but received " + std::to_string(tau_out.size()));
   }
   if (static_cast<std::size_t>(tau_in.size()) != njoints) {
-    throw std::invalid_argument(
-        "Expected tau_in to be " + std::to_string(njoints) +
-        " but received " + std::to_string(tau_in.size()));
+    throw std::invalid_argument("Expected tau_in to be " +
+                                std::to_string(njoints) + " but received " +
+                                std::to_string(tau_in.size()));
   }
 
   typedef pinocchio::ModelTpl<double, Options, JointCollectionTpl> Model;

include/crocoddyl_msgs/multibody_inertia_publisher.h

@@ -36,15 +36,13 @@ class MultibodyInertiaRosPublisher {
       : node_("inertial_parameters_publisher"),
         pub_(node_.create_publisher<MultibodyInertia>(topic, 1)) {
     RCLCPP_INFO_STREAM(node_.get_logger(),
-                       "Publishing MultibodyInertia messages on "
-                           << topic);
+                       "Publishing MultibodyInertia messages on " << topic);
   }
 #else
   {
     ros::NodeHandle n;
     pub_.init(n, topic, 1);
-    ROS_INFO_STREAM("Publishing MultibodyInertia messages on "
-                    << topic);
+    ROS_INFO_STREAM("Publishing MultibodyInertia messages on " << topic);
   }
 #endif
 
@@ -58,7 +56,7 @@ class MultibodyInertiaRosPublisher {
    * the first moment of inertial (mass * barycenter) and the rotational
    * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the
    * barycenter.
-   * 
+   *
    * @param parameters[in]  Multibody inertial parameters.
    */
   void publish(const std::map<std::string, const Eigen::Ref<const Vector10d>>

include/crocoddyl_msgs/multibody_inertia_subscriber.h

@@ -21,9 +21,11 @@
 namespace crocoddyl_msgs {
 
 #ifdef ROS2
-typedef const crocoddyl_msgs::msg::MultibodyInertia::SharedPtr MultibodyInertiaSharedPtr;
+typedef const crocoddyl_msgs::msg::MultibodyInertia::SharedPtr
+    MultibodyInertiaSharedPtr;
 #else
-typedef const crocoddyl_msgs::MultibodyInertia::ConstPtr &MultibodyInertiaSharedPtr;
+typedef const crocoddyl_msgs::MultibodyInertia::ConstPtr
+    &MultibodyInertiaSharedPtr;
 #endif
 
 class MultibodyInertiaRosSubscriber {
@@ -48,8 +50,7 @@ class MultibodyInertiaRosSubscriber {
     thread_ = std::thread([this]() { this->spin(); });
     thread_.detach();
     RCLCPP_INFO_STREAM(node_->get_logger(),
-                       "Subscribing MultibodyInertia messages on "
-                           << topic);
+                       "Subscribing MultibodyInertia messages on " << topic);
 #else
       : spinner_(2), has_new_msg_(false), is_processing_msg_(false),
         last_msg_time_(0.) {
@@ -58,8 +59,7 @@ class MultibodyInertiaRosSubscriber {
         topic, 1, &MultibodyInertiaRosSubscriber::callback, this,
         ros::TransportHints().tcpNoDelay());
     spinner_.start();
-    ROS_INFO_STREAM("Subscribing MultibodyInertia messages on "
-                    << topic);
+    ROS_INFO_STREAM("Subscribing MultibodyInertia messages on " << topic);
 #endif
   }
 
@@ -73,7 +73,7 @@ class MultibodyInertiaRosSubscriber {
    * the first moment of inertial (mass * barycenter) and the rotational
    * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the
    * barycenter.
-   * 
+   *
    * @return  A map from body names to inertial parameters.   *
    */
   std::map<std::string, Vector10d> get_parameters() {
@@ -104,14 +104,13 @@ class MultibodyInertiaRosSubscriber {
   rclcpp::executors::SingleThreadedExecutor spinner_;
   std::thread thread_;
   void spin() { spinner_.spin(); }
-  rclcpp::Subscription<MultibodyInertia>::SharedPtr
-      sub_; //!< ROS subscriber
+  rclcpp::Subscription<MultibodyInertia>::SharedPtr sub_; //!< ROS subscriber
 #else
   ros::NodeHandle node_;
   ros::AsyncSpinner spinner_;
   ros::Subscriber sub_; //!< ROS subscriber
 #endif
-  std::mutex mutex_; ///< Mutex to prevent race condition on callback
+  std::mutex mutex_;     ///< Mutex to prevent race condition on callback
   MultibodyInertia msg_; //!< ROS message
 
   bool has_new_msg_;       //!< Indcate when a new message has been received

include/crocoddyl_msgs/whole_body_state_publisher.h

@@ -180,7 +180,8 @@ class WholeBodyStateRosPublisher {
   }
 
   /**
-   * @brief Update the Pinocchio model's inertial parameters of a given body frame
+   * @brief Update the Pinocchio model's inertial parameters of a given body
+   * frame
    *
    * The inertial parameters vector is defined as [m, h_x, h_y, h_z,
    * I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is
@@ -193,21 +194,21 @@ class WholeBodyStateRosPublisher {
    * @param body_name[in] Body name
    * @param psi[in]        Inertial parameters
    */
-  void update_body_inertial_parameters(
-      const std::string &body_name,
-      const Eigen::Ref<const Vector10d> &psi) {
+  void update_body_inertial_parameters(const std::string &body_name,
+                                       const Eigen::Ref<const Vector10d> &psi) {
     updateBodyInertialParameters(model_, body_name, psi);
   }
 
   /**
-   * @brief Return the Pinocchio model's inertial parameters of a given body frame
+   * @brief Return the Pinocchio model's inertial parameters of a given body
+   * frame
    *
    * The inertial parameters vector is defined as [m, h_x, h_y, h_z,
    * I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T, where h=mc is
    * the first moment of inertial (mass * barycenter) and the rotational
    * inertia I = I_C + mS^T(c)S(c) where I_C has its origin at the
    * barycenter.
-   * 
+   *
    * @param body_name[in]  Body name
    */
   const Vector10d