[pre-commit] Run locally pre-commit

cmastalli · Apr 6, 2024 · fd35733 · fd35733
1 parent 154dc78
commit fd35733
Show file tree

Hide file tree

Showing 8 changed files with 109 additions and 75 deletions.
diff --git a/include/crocoddyl_msgs/conversions.h b/include/crocoddyl_msgs/conversions.h
@@ -182,14 +182,14 @@ static inline void toMsg(Control &msg,
  * @param tau[in]    Joint effort
  */
 template <int Options, template <typename, int> class JointCollectionTpl>
-static inline void toMsg(
-    const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
-    pinocchio::DataTpl<double, Options, JointCollectionTpl> &data,
-    WholeBodyState &msg, const double t,
-    const Eigen::Ref<const Eigen::VectorXd> &q,
-    const Eigen::Ref<const Eigen::VectorXd> &v,
-    const Eigen::Ref<const Eigen::VectorXd> &a,
-    const Eigen::Ref<const Eigen::VectorXd> &tau) {
+static inline void
+toMsg(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
+      pinocchio::DataTpl<double, Options, JointCollectionTpl> &data,
+      WholeBodyState &msg, const double t,
+      const Eigen::Ref<const Eigen::VectorXd> &q,
+      const Eigen::Ref<const Eigen::VectorXd> &v,
+      const Eigen::Ref<const Eigen::VectorXd> &a,
+      const Eigen::Ref<const Eigen::VectorXd> &tau) {
   if (q.size() != model.nq) {
     throw std::invalid_argument("Expected q to be " + std::to_string(model.nq) +
                                 " but received " + std::to_string(q.size()));

diff --git a/include/crocoddyl_msgs/whole_body_state_publisher.h b/include/crocoddyl_msgs/whole_body_state_publisher.h
@@ -25,10 +25,12 @@ static std::map<std::string, pinocchio::SE3> DEFAULT_SE3;
 
 static std::map<std::string, pinocchio::Motion> DEFAULT_MOTION;
 
-static std::map<std::string, std::tuple<pinocchio::Force, ContactType,
-                                                 ContactStatus>> DEFAULT_FORCE;
+static std::map<std::string,
+                std::tuple<pinocchio::Force, ContactType, ContactStatus>>
+    DEFAULT_FORCE;
 
-static std::map<std::string, std::pair<Eigen::Vector3d, double>> DEFAULT_FRICTION;
+static std::map<std::string, std::pair<Eigen::Vector3d, double>>
+    DEFAULT_FRICTION;
 
 class WholeBodyStateRosPublisher {
 public:
@@ -81,8 +83,8 @@ class WholeBodyStateRosPublisher {
 #ifdef ROS2
       : node_("whole_body_state_publisher"),
         pub_(node_.create_publisher<WholeBodyState>(topic, 1)), model_(model),
-        data_(model), odom_frame_(frame), a_(model.nv),
-        qref_(qref), is_reduced_model_(true) {
+        data_(model), odom_frame_(frame), a_(model.nv), qref_(qref),
+        is_reduced_model_(true) {
     RCLCPP_INFO_STREAM(node_.get_logger(),
                        "Publishing WholeBodyState messages on "
                            << topic << " (frame: " << frame << ")");
@@ -113,22 +115,23 @@ class WholeBodyStateRosPublisher {
    * @param f[in]    Contact force, type and status
    * @param s[in]    Contact surface and friction coefficient
    */
-  void
-  publish(const double t, const Eigen::Ref<const Eigen::VectorXd> &q,
-          const Eigen::Ref<const Eigen::VectorXd> &v,
-          const Eigen::Ref<const Eigen::VectorXd> &tau,
-          const std::map<std::string, pinocchio::SE3> &p = DEFAULT_SE3,
-          const std::map<std::string, pinocchio::Motion> &pd = DEFAULT_MOTION,
-          const std::map<std::string, std::tuple<pinocchio::Force, ContactType,
-                                                 ContactStatus>> &f = DEFAULT_FORCE,
-          const std::map<std::string, std::pair<Eigen::Vector3d, double>> &s = DEFAULT_FRICTION) {
+  void publish(
+      const double t, const Eigen::Ref<const Eigen::VectorXd> &q,
+      const Eigen::Ref<const Eigen::VectorXd> &v,
+      const Eigen::Ref<const Eigen::VectorXd> &tau,
+      const std::map<std::string, pinocchio::SE3> &p = DEFAULT_SE3,
+      const std::map<std::string, pinocchio::Motion> &pd = DEFAULT_MOTION,
+      const std::map<std::string, std::tuple<pinocchio::Force, ContactType,
+                                             ContactStatus>> &f = DEFAULT_FORCE,
+      const std::map<std::string, std::pair<Eigen::Vector3d, double>> &s =
+          DEFAULT_FRICTION) {
     if (pub_.trylock()) {
       pub_.msg_.header.frame_id = odom_frame_;
       if (is_reduced_model_) {
         fromReduced(model_, reduced_model_, qfull_, vfull_, ufull_, q, v, tau,
                     qref_, joint_ids_);
-        crocoddyl_msgs::toMsg(model_, data_, pub_.msg_, t, qfull_,
-                              vfull_, a_, ufull_, p, pd, f, s);
+        crocoddyl_msgs::toMsg(model_, data_, pub_.msg_, t, qfull_, vfull_, a_,
+                              ufull_, p, pd, f, s);
       } else {
         crocoddyl_msgs::toMsg(model_, data_, pub_.msg_, t, q, v, a_, tau, p, pd,
                               f, s);
@@ -161,7 +164,10 @@ class WholeBodyStateRosPublisher {
       // Check the size of the reference configuration
       if (qref_.size() != model_.nq) {
 #ifdef ROS2
-        RCLCPP_ERROR_STREAM(node_.get_logger(), "Invalid argument: qref has wrong dimension (it should be " << std::to_string(model_.nq) << ")");
+        RCLCPP_ERROR_STREAM(
+            node_.get_logger(),
+            "Invalid argument: qref has wrong dimension (it should be "
+                << std::to_string(model_.nq) << ")");
 #else
         ROS_ERROR_STREAM(
             "Invalid argument: qref has wrong dimension (it should be "

diff --git a/include/crocoddyl_msgs/whole_body_state_subscriber.h b/include/crocoddyl_msgs/whole_body_state_subscriber.h
@@ -227,7 +227,10 @@ class WholeBodyStateRosSubscriber {
       // Check the size of the reference configuration
       if (qref_.size() != model_.nq) {
 #ifdef ROS2
-        RCLCPP_ERROR_STREAM(node_->get_logger(), "Invalid argument: qref has wrong dimension (it should be " << std::to_string(model_.nq) << ")");
+        RCLCPP_ERROR_STREAM(
+            node_->get_logger(),
+            "Invalid argument: qref has wrong dimension (it should be "
+                << std::to_string(model_.nq) << ")");
 #else
         ROS_ERROR_STREAM(
             "Invalid argument: qref has wrong dimension (it should be "

diff --git a/include/crocoddyl_msgs/whole_body_trajectory_publisher.h b/include/crocoddyl_msgs/whole_body_trajectory_publisher.h
@@ -23,14 +23,15 @@ namespace crocoddyl_msgs {
 
 static std::vector<std::map<std::string, pinocchio::SE3>> DEFAULT_SE3_VECTOR;
 
-static std::vector<std::map<std::string, pinocchio::Motion>> DEFAULT_MOTION_VECTOR;
+static std::vector<std::map<std::string, pinocchio::Motion>>
+    DEFAULT_MOTION_VECTOR;
 
-static std::vector<
-              std::map<std::string, std::tuple<pinocchio::Force, ContactType,
-                                               ContactStatus>>> DEFAULT_FORCE_VECTOR;
+static std::vector<std::map<
+    std::string, std::tuple<pinocchio::Force, ContactType, ContactStatus>>>
+    DEFAULT_FORCE_VECTOR;
 
-static std::vector<
-              std::map<std::string, std::pair<Eigen::Vector3d, double>>> DEFAULT_FRICTION_VECTOR;
+static std::vector<std::map<std::string, std::pair<Eigen::Vector3d, double>>>
+    DEFAULT_FRICTION_VECTOR;
 
 class WholeBodyTrajectoryRosPublisher {
 public:
@@ -117,16 +118,20 @@ class WholeBodyTrajectoryRosPublisher {
    * @param fs[in]   Vector of contact forces, types and statuses
    * @param ss[in]   Vector of contact surfaces and friction coefficients
    */
-  void
-  publish(const std::vector<double> &ts, const std::vector<Eigen::VectorXd> &xs,
-          const std::vector<Eigen::VectorXd> &us,
-          const std::vector<std::map<std::string, pinocchio::SE3>> &ps = DEFAULT_SE3_VECTOR,
-          const std::vector<std::map<std::string, pinocchio::Motion>> &pds = DEFAULT_MOTION_VECTOR,
-          const std::vector<
-              std::map<std::string, std::tuple<pinocchio::Force, ContactType,
-                                               ContactStatus>>> &fs = DEFAULT_FORCE_VECTOR,
-          const std::vector<
-              std::map<std::string, std::pair<Eigen::Vector3d, double>>> &ss = DEFAULT_FRICTION_VECTOR) {
+  void publish(
+      const std::vector<double> &ts, const std::vector<Eigen::VectorXd> &xs,
+      const std::vector<Eigen::VectorXd> &us,
+      const std::vector<std::map<std::string, pinocchio::SE3>> &ps =
+          DEFAULT_SE3_VECTOR,
+      const std::vector<std::map<std::string, pinocchio::Motion>> &pds =
+          DEFAULT_MOTION_VECTOR,
+      const std::vector<
+          std::map<std::string,
+                   std::tuple<pinocchio::Force, ContactType, ContactStatus>>>
+          &fs = DEFAULT_FORCE_VECTOR,
+      const std::vector<
+          std::map<std::string, std::pair<Eigen::Vector3d, double>>> &ss =
+          DEFAULT_FRICTION_VECTOR) {
     if (pub_.trylock()) {
       if (ts.size() != xs.size()) {
         throw std::invalid_argument("The size of the ts vector needs to equal "
@@ -139,24 +144,24 @@ class WholeBodyTrajectoryRosPublisher {
       }
       if (ps.size() != 0) {
         if (ts.size() != ps.size()) {
-          throw std::invalid_argument(
-              "If provided, the size of the ps vector needs to equal the size of "
-              "the ts vector.");
+          throw std::invalid_argument("If provided, the size of the ps vector "
+                                      "needs to equal the size of "
+                                      "the ts vector.");
         }
         if (ts.size() != pds.size()) {
           throw std::invalid_argument("If provided, the size of the pds vector "
                                       "needs to equal the size of "
                                       "the ts vector.");
         }
         if (ts.size() != fs.size()) {
-          throw std::invalid_argument(
-              "If provided, the size of the fs vector needs to equal the size of "
-              "the ts vector.");
+          throw std::invalid_argument("If provided, the size of the fs vector "
+                                      "needs to equal the size of "
+                                      "the ts vector.");
         }
         if (ts.size() != ss.size()) {
-          throw std::invalid_argument(
-              "If provided, the size of the ss vector needs to equal the size of "
-              "the ts vector.");
+          throw std::invalid_argument("If provided, the size of the ss vector "
+                                      "needs to equal the size of "
+                                      "the ts vector.");
         }
       }
       pub_.msg_.header.frame_id = odom_frame_;
@@ -173,12 +178,12 @@ class WholeBodyTrajectoryRosPublisher {
                       xs[i].head(reduced_model_.nq),
                       xs[i].tail(reduced_model_.nv), us[i], qref_, joint_ids_);
           if (ps.size() != 0) {
-            crocoddyl_msgs::toMsg(model_, data_, pub_.msg_.trajectory[i],
-                                  ts[i], qfull_, vfull_, a_, ufull_, ps[i],
-                                  pds[i], fs[i], ss[i]);
+            crocoddyl_msgs::toMsg(model_, data_, pub_.msg_.trajectory[i], ts[i],
+                                  qfull_, vfull_, a_, ufull_, ps[i], pds[i],
+                                  fs[i], ss[i]);
           } else {
-            crocoddyl_msgs::toMsg(model_, data_, pub_.msg_.trajectory[i],
-                                  ts[i], qfull_, vfull_, a_, ufull_);            
+            crocoddyl_msgs::toMsg(model_, data_, pub_.msg_.trajectory[i], ts[i],
+                                  qfull_, vfull_, a_, ufull_);
           }
         } else {
           if (ps.size() != 0) {
@@ -229,7 +234,10 @@ class WholeBodyTrajectoryRosPublisher {
       // Check the size of the reference configuration
       if (qref_.size() != model_.nq) {
 #ifdef ROS2
-        RCLCPP_ERROR_STREAM(node_.get_logger(), "Invalid argument: qref has wrong dimension (it should be " << std::to_string(model_.nq) << ")");
+        RCLCPP_ERROR_STREAM(
+            node_.get_logger(),
+            "Invalid argument: qref has wrong dimension (it should be "
+                << std::to_string(model_.nq) << ")");
 #else
         ROS_ERROR_STREAM(
             "Invalid argument: qref has wrong dimension (it should be "

diff --git a/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h b/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h
@@ -100,8 +100,8 @@ class WholeBodyTrajectoryRosSubscriber {
             topic, 1, &WholeBodyTrajectoryRosSubscriber::callback, this,
             ros::TransportHints().tcpNoDelay())),
         has_new_msg_(false), is_processing_msg_(false), last_msg_time_(0.),
-        odom_frame_(frame), model_(model), data_(model),
-        a_(model.nv), qref_(qref), is_reduced_model_(true) {
+        odom_frame_(frame), model_(model), data_(model), a_(model.nv),
+        qref_(qref), is_reduced_model_(true) {
     ROS_INFO_STREAM("Subscribing WholeBodyTrajectory messages on " << topic);
 #endif
     init(locked_joints);
@@ -260,7 +260,10 @@ class WholeBodyTrajectoryRosSubscriber {
       // Check the size of the reference configuration
       if (qref_.size() != model_.nq) {
 #ifdef ROS2
-        RCLCPP_ERROR_STREAM(node_->get_logger(), "Invalid argument: qref has wrong dimension (it should be " << std::to_string(model_.nq) << ")");
+        RCLCPP_ERROR_STREAM(
+            node_->get_logger(),
+            "Invalid argument: qref has wrong dimension (it should be "
+                << std::to_string(model_.nq) << ")");
 #else
         ROS_ERROR_STREAM(
             "Invalid argument: qref has wrong dimension (it should be "

diff --git a/src/crocoddyl_ros.cpp b/src/crocoddyl_ros.cpp
@@ -169,7 +169,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) {
            ":param f: contact force, type and status\n"
            ":param s: contact surface and friction coefficient",
            py::arg("t"), py::arg("q"), py::arg("v"), py::arg("tau"),
-           py::arg("p") = DEFAULT_SE3, py::arg("pd") = DEFAULT_MOTION, py::arg("f") = DEFAULT_FORCE, py::arg("s") = DEFAULT_FRICTION);
+           py::arg("p") = DEFAULT_SE3, py::arg("pd") = DEFAULT_MOTION,
+           py::arg("f") = DEFAULT_FORCE, py::arg("s") = DEFAULT_FRICTION);
 
   py::class_<WholeBodyStateRosSubscriber,
              std::unique_ptr<WholeBodyStateRosSubscriber, py::nodelete>>(
@@ -179,8 +180,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) {
            py::arg("model"), py::arg("topic") = "/crocoddyl/whole_body_state",
            py::arg("frame") = "odom")
       .def(py::init<pinocchio::Model &, const std::vector<std::string> &,
-                    const Eigen::Ref<const Eigen::VectorXd> &, const std::string &,
-                    const std::string &>(),
+                    const Eigen::Ref<const Eigen::VectorXd> &,
+                    const std::string &, const std::string &>(),
            py::arg("model"), py::arg("locked_joints"), py::arg("qref"),
            py::arg("topic") = "/crocoddyl/whole_body_state",
            py::arg("frame") = "odom")
@@ -203,8 +204,8 @@ PYBIND11_MODULE(crocoddyl_ros, m) {
            py::arg("topic") = "/crocoddyl/whole_body_trajectory",
            py::arg("frame") = "odom", py::arg("queue") = 10)
       .def(py::init<pinocchio::Model &, const std::vector<std::string> &,
-                    const Eigen::Ref<const Eigen::VectorXd> &, const std::string &,
-                    const std::string &, int>(),
+                    const Eigen::Ref<const Eigen::VectorXd> &,
+                    const std::string &, const std::string &, int>(),
            py::arg("model"), py::arg("locked_joints"), py::arg("qref"),
            py::arg("topic") = "/crocoddyl/whole_body_state",
            py::arg("frame") = "odom", py::arg("queue") = 10)
@@ -218,8 +219,11 @@ PYBIND11_MODULE(crocoddyl_ros, m) {
            ":param pds: list of contact velocities\n"
            ":param fs: list of contact forces, types and statuses\n"
            ":param ss: list of contact surfaces and friction coefficients",
-           py::arg("ts"), py::arg("xs"), py::arg("us"), py::arg("ps") = DEFAULT_SE3_VECTOR,
-           py::arg("pds") = DEFAULT_MOTION_VECTOR, py::arg("fs") = DEFAULT_FORCE_VECTOR, py::arg("ss") = DEFAULT_FRICTION_VECTOR);
+           py::arg("ts"), py::arg("xs"), py::arg("us"),
+           py::arg("ps") = DEFAULT_SE3_VECTOR,
+           py::arg("pds") = DEFAULT_MOTION_VECTOR,
+           py::arg("fs") = DEFAULT_FORCE_VECTOR,
+           py::arg("ss") = DEFAULT_FRICTION_VECTOR);
 
   py::class_<WholeBodyTrajectoryRosSubscriber,
              std::unique_ptr<WholeBodyTrajectoryRosSubscriber, py::nodelete>>(
@@ -245,10 +249,11 @@ PYBIND11_MODULE(crocoddyl_ros, m) {
            "coefficients.")
       .def("has_new_msg", &WholeBodyTrajectoryRosSubscriber::has_new_msg);
 
-  m.def("getRootJointId", &getRootJointId<0, pinocchio::JointCollectionDefaultTpl>,
-           "Return the root joint id.\n\n"
-           ":param model: Pinocchio model\n"
-           ":return root joint id");
+  m.def("getRootJointId",
+        &getRootJointId<0, pinocchio::JointCollectionDefaultTpl>,
+        "Return the root joint id.\n\n"
+        ":param model: Pinocchio model\n"
+        ":return root joint id");
 
   m.def(
       "fromReduced",

diff --git a/unittest/test_whole_body_state.py b/unittest/test_whole_body_state.py
@@ -161,7 +161,9 @@ def test_communication_with_reduced_model(self):
         sub = WholeBodyStateRosSubscriber(
             model, locked_joints, qref, "reduced_whole_body_state"
         )
-        pub = WholeBodyStateRosPublisher(model, locked_joints, qref, "reduced_whole_body_state")
+        pub = WholeBodyStateRosPublisher(
+            model, locked_joints, qref, "reduced_whole_body_state"
+        )
         time.sleep(1)
         # publish whole-body state messages
         q = pinocchio.randomConfiguration(model)
@@ -219,7 +221,9 @@ def test_communication_with_non_locked_joints(self):
         sub = WholeBodyStateRosSubscriber(
             model, locked_joints, qref, "non_locked_whole_body_state"
         )
-        pub = WholeBodyStateRosPublisher(model, locked_joints, qref, "non_locked_whole_body_state")
+        pub = WholeBodyStateRosPublisher(
+            model, locked_joints, qref, "non_locked_whole_body_state"
+        )
         time.sleep(1)
         # publish whole-body state messages
         q = pinocchio.randomConfiguration(model)
@@ -267,6 +271,7 @@ def test_communication_with_non_locked_joints(self):
                 S[1], _S[1], "Wrong contact friction coefficient at " + name
             )
 
+
 if __name__ == "__main__":
     if ROS_VERSION == 2:
         unittest.main()

diff --git a/unittest/test_whole_body_trajectory.py b/unittest/test_whole_body_trajectory.py
@@ -94,8 +94,12 @@ def setUp(self) -> None:
 
     def test_publisher_without_contact(self):
         model = pinocchio.buildSampleModelHumanoid()
-        sub = WholeBodyTrajectoryRosSubscriber(model, "whole_body_trajectory_without_contact")
-        pub = WholeBodyTrajectoryRosPublisher(model, "whole_body_trajectory_without_contact")
+        sub = WholeBodyTrajectoryRosSubscriber(
+            model, "whole_body_trajectory_without_contact"
+        )
+        pub = WholeBodyTrajectoryRosPublisher(
+            model, "whole_body_trajectory_without_contact"
+        )
         time.sleep(1)
         # publish whole-body trajectory messages
         N = len(self.ts)