Merge pull request #17 from cmastalli/topic/fixed-base-robots

Support fixed-based robots
RobotMotorIntelligence · Apr 4, 2024 · 4c10eaf · 4c10eaf
2 parents ee1e67a + 7f04744
commit 4c10eaf
Show file tree

Hide file tree

Showing 12 changed files with 286 additions and 112 deletions.
diff --git a/.github/dependabot.yml b/.github/dependabot.yml
@@ -0,0 +1,7 @@
+version: 2
+updates:
+- package-ecosystem: "github-actions"
+  directory: "/"
+  target-branch: "devel"
+  schedule:
+      interval: "weekly"
diff --git a/.github/workflows/changelog.yml b/.github/workflows/changelog.yml
@@ -0,0 +1,16 @@
+name: CHANGELOG
+
+on:
+  pull_request:
+    types: [assigned, opened, synchronize, reopened, labeled, unlabeled]
+    branches:
+      - devel
+
+jobs:
+  check-changelog:
+    name: Check changelog action
+    runs-on: ubuntu-latest
+    steps:
+      - uses: tarides/changelog-check-action@v2
+        with:
+          changelog: CHANGELOG.md
diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml
@@ -1,5 +1,24 @@
 name: ROS
-on: [push, pull_request]
+on:
+  push:
+    paths-ignore:
+      - '.gitignore'
+      - '.pre-commit-config.yaml'
+      - '*.md'
+      - 'LICENSE'
+      - 'package.xml'
+      - 'dependencies.rosintall'
+  pull_request:
+    paths-ignore:
+      - '.gitignore'
+      - '.pre-commit-config.yaml'
+      - '*.md'
+      - 'LICENSE'
+      - 'package.xml'
+      - 'dependencies.rosintall'
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
 
 jobs:
   CI:
@@ -17,10 +36,16 @@ jobs:
           # - {name: "(iron, Debug)", ROS_DISTRO: iron, CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"}
     name: ${{ matrix.env.name }}
     env:
+      CCACHE_DIR: /github/home/.ccache
       UPSTREAM_WORKSPACE: dependencies.rosinstall
       CTEST_OUTPUT_ON_FAILURE: 1
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
+      - uses: actions/cache@v4
+        with:
+          path: ${{ env.CCACHE_DIR }}
+          key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}-${{ github.sha }}
+          restore-keys: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}-
       - uses: 'ros-industrial/industrial_ci@master'
         env: ${{ matrix.env }}
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -0,0 +1,34 @@
+# Changelog
+
+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]
+
+* Supported fixed-based robots in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/17
+
+## [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
+
+## [1.2.1] - 2023-12-13
+
+* Extended CI jobs in https://github.com/RobotMotorIntelligence/crocoddyl_msgs/pull/12
+
+## [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
+
+## [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
+
+## [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
diff --git a/include/crocoddyl_msgs/conversions.h b/include/crocoddyl_msgs/conversions.h
@@ -89,6 +89,30 @@ static inline std::size_t getRootJointId(
                     : 0);
 }
 
+/**
+ * @brief Return the root nq dimension
+ *
+ * @param return  Root joint nq dimension
+ */
+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;
+}
+
+/**
+ * @brief Return the root nv dimension
+ *
+ * @param return  Root joint nv dimension
+ */
+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;
+}
+
 /**
  * @brief Conversion of Eigen to message for a given
  * crocoddyl_msgs::FeedbackGain message reference
@@ -203,7 +227,8 @@ static inline void toMsg(
                                 " but received " + std::to_string(a.size()));
   }
   const std::size_t root_joint_id = getRootJointId(model);
-  const std::size_t nv_root = model.joints[root_joint_id].nv();
+  const std::size_t nq_root = getRootNq(model);
+  const std::size_t nv_root = getRootNv(model);
   const std::size_t njoints = model.nv - nv_root;
   if (tau.size() != static_cast<int>(njoints) && tau.size() != 0) {
     throw std::invalid_argument("Expected tau to be 0 or " +
@@ -236,7 +261,7 @@ static inline void toMsg(
     msg.centroidal.base_angular_velocity.x = v(3);
     msg.centroidal.base_angular_velocity.y = v(4);
     msg.centroidal.base_angular_velocity.z = v(5);
-  } else if (nv_root != 0) {
+  } else if (nv_root > 0) {
     std::cerr
         << "Warning: toMsg conversion does not yet support root joints "
            "different to a floating base. We cannot publish base information."
@@ -262,10 +287,10 @@ static inline void toMsg(
   // Filling the joint state
   msg.joints.resize(njoints);
   for (std::size_t j = 0; j < njoints; ++j) {
-    msg.joints[j].name = model.names[2 + j];
-    msg.joints[j].position = q(model.joints[1].nq() + j);
-    msg.joints[j].velocity = v(model.joints[1].nv() + j);
-    msg.joints[j].acceleration = a(model.joints[1].nv() + j);
+    msg.joints[j].name = model.names[root_joint_id + j + 1];
+    msg.joints[j].position = q(nq_root + j);
+    msg.joints[j].velocity = v(nv_root + j);
+    msg.joints[j].acceleration = a(nv_root + j);
     if (tau.size() != 0) {
       msg.joints[j].effort = tau(j);
     }
@@ -537,7 +562,7 @@ fromMsg(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
                                 " but received " + std::to_string(v.size()));
   }
   const std::size_t root_joint_id = getRootJointId(model);
-  const std::size_t nv_root = model.joints[root_joint_id].nv();
+  const std::size_t nv_root = getRootNv(model);
   const std::size_t njoints = model.nv - nv_root;
   if (tau.size() != static_cast<int>(njoints)) {
     throw std::invalid_argument("Expected tau to be " +
@@ -587,7 +612,7 @@ fromMsg(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
                       .toRotationMatrix()
                       .transpose() *
                   v.head<3>(); // local frame
-  } else if (nv_root != 0) {
+  } else if (nv_root > 0) {
     std::cerr
         << "Warning: fromMsg conversion does not yet support root joints "
            "different to a floating base. We cannot publish base information."
@@ -676,8 +701,10 @@ static inline void fromReduced(
     const Eigen::Ref<const Eigen::VectorXd> &qref,
     const std::vector<pinocchio::JointIndex> &locked_joint_ids) {
   const std::size_t root_joint_id = getRootJointId(model);
-  const std::size_t nq_root = model.joints[root_joint_id].nq();
-  const std::size_t nv_root = model.joints[root_joint_id].nv();
+  const std::size_t nq_root = getRootNq(model);
+  const std::size_t nv_root = getRootNv(model);
+  const std::size_t njoints = model.nv - nv_root;
+  const std::size_t njoints_reduced = reduced_model.nv - nv_root;
   if (q_out.size() != model.nq) {
     throw std::invalid_argument("Expected q_out to be " +
                                 std::to_string(model.nq) + " but received " +
@@ -698,14 +725,14 @@ static inline void fromReduced(
                                 std::to_string(reduced_model.nv) +
                                 " but received " + std::to_string(v_in.size()));
   }
-  if (static_cast<std::size_t>(tau_out.size()) != model.nv - nv_root) {
+  if (static_cast<std::size_t>(tau_out.size()) != njoints) {
     throw std::invalid_argument(
-        "Expected tau_out to be " + std::to_string(model.nv - nv_root) +
+        "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()) != reduced_model.nv - nv_root) {
+  if (static_cast<std::size_t>(tau_in.size()) != njoints_reduced) {
     throw std::invalid_argument(
-        "Expected tau_in to be " + std::to_string(reduced_model.nv - nv_root) +
+        "Expected tau_in to be " + std::to_string(njoints_reduced) +
         " but received " + std::to_string(tau_in.size()));
   }
 
@@ -755,8 +782,10 @@ toReduced(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
           const Eigen::Ref<const Eigen::VectorXd> &v_in,
           const Eigen::Ref<const Eigen::VectorXd> &tau_in) {
   const std::size_t root_joint_id = getRootJointId(model);
-  const std::size_t nq_root = model.joints[root_joint_id].nq();
-  const std::size_t nv_root = model.joints[root_joint_id].nv();
+  const std::size_t nq_root = getRootNq(model);
+  const std::size_t nv_root = getRootNv(model);
+  const std::size_t njoints = model.nv - nv_root;
+  const std::size_t njoints_reduced = reduced_model.nv - nv_root;
   if (q_out.size() != reduced_model.nq) {
     throw std::invalid_argument(
         "Expected q_out to be " + std::to_string(reduced_model.nq) +
@@ -777,14 +806,14 @@ toReduced(const pinocchio::ModelTpl<double, Options, JointCollectionTpl> &model,
                                 std::to_string(model.nv) + " but received " +
                                 std::to_string(v_in.size()));
   }
-  if (static_cast<std::size_t>(tau_out.size()) != reduced_model.nv - nv_root) {
+  if (static_cast<std::size_t>(tau_out.size()) != njoints_reduced) {
     throw std::invalid_argument(
-        "Expected tau_out to be " + std::to_string(reduced_model.nv - nv_root) +
+        "Expected tau_out to be " + std::to_string(njoints_reduced) +
         " but received " + std::to_string(tau_out.size()));
   }
-  if (static_cast<std::size_t>(tau_in.size()) != model.nv - nv_root) {
+  if (static_cast<std::size_t>(tau_in.size()) != njoints) {
     throw std::invalid_argument(
-        "Expected tau_in to be " + std::to_string(model.nv - nv_root) +
+        "Expected tau_in to be " + std::to_string(njoints) +
         " but received " + std::to_string(tau_in.size()));
   }
 
@@ -835,10 +864,10 @@ toReduced_return(
     const Eigen::Ref<const Eigen::VectorXd> &v_in,
     const Eigen::Ref<const Eigen::VectorXd> &tau_in) {
   const std::size_t root_joint_id = getRootJointId(model);
+  const std::size_t nv_root = getRootNv(model);
   Eigen::VectorXd q_out = Eigen::VectorXd::Zero(reduced_model.nq);
   Eigen::VectorXd v_out = Eigen::VectorXd::Zero(reduced_model.nv);
-  Eigen::VectorXd tau_out = Eigen::VectorXd::Zero(
-      reduced_model.nv - model.joints[root_joint_id].nv());
+  Eigen::VectorXd tau_out = Eigen::VectorXd::Zero(reduced_model.nv - nv_root);
   toReduced(model, reduced_model, q_out, v_out, tau_out, q_in, v_in, tau_in);
   return {q_out, v_out, tau_out};
 }

diff --git a/include/crocoddyl_msgs/whole_body_state_publisher.h b/include/crocoddyl_msgs/whole_body_state_publisher.h
@@ -183,8 +183,7 @@ class WholeBodyStateRosPublisher {
       }
       pinocchio::buildReducedModel(model_, joint_ids_, qref_, reduced_model_);
       // Initialize the vectors and dimensions
-      const std::size_t root_joint_id = getRootJointId(model_);
-      const std::size_t nv_root = model_.joints[root_joint_id].nv();
+      const std::size_t nv_root = getRootNv(model_);
       qfull_ = Eigen::VectorXd::Zero(model_.nq);
       vfull_ = Eigen::VectorXd::Zero(model_.nv);
       ufull_ = Eigen::VectorXd::Zero(model_.nv - nv_root);

diff --git a/include/crocoddyl_msgs/whole_body_state_subscriber.h b/include/crocoddyl_msgs/whole_body_state_subscriber.h
@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2020-2023, Heriot-Watt University, University of Oxford
+// Copyright (C) 2020-2024, Heriot-Watt University, University of Oxford
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -221,8 +221,7 @@ class WholeBodyStateRosSubscriber {
       f_tmp_;
 
   void init(const std::vector<std::string> &locked_joints = DEFAULT_VECTOR) {
-    const std::size_t root_joint_id = getRootJointId(model_);
-    const std::size_t nv_root = model_.joints[root_joint_id].nv();
+    const std::size_t nv_root = getRootNv(model_);
     if (locked_joints.size() != 0) {
       // Check the size of the reference configuration
       if (qref_.size() != model_.nq) {
@@ -235,6 +234,7 @@ class WholeBodyStateRosSubscriber {
 #endif
       }
       // Build the reduced model
+      std::size_t inexistent_joints = 0;
       for (std::string name : locked_joints) {
         if (model_.existJointName(name)) {
           joint_ids_.push_back(model_.getJointId(name));
@@ -245,8 +245,14 @@ class WholeBodyStateRosSubscriber {
 #else
           ROS_ERROR_STREAM("Doesn't exist " << name << " joint");
 #endif
+          inexistent_joints += 1;
         }
       }
+      // Check that locked joint exists to update q and v appropriately
+      if (inexistent_joints != 0) {
+        q_ = Eigen::VectorXd::Zero(model_.nq - locked_joints.size() + inexistent_joints);
+        v_ = Eigen::VectorXd::Zero(model_.nv - locked_joints.size() + inexistent_joints);
+      }
       pinocchio::buildReducedModel(model_, joint_ids_, qref_, reduced_model_);
 
       // Initialize the vectors and dimensions

diff --git a/include/crocoddyl_msgs/whole_body_trajectory_publisher.h b/include/crocoddyl_msgs/whole_body_trajectory_publisher.h
@@ -253,7 +253,7 @@ class WholeBodyTrajectoryRosPublisher {
 
       // Initialize the vectors and dimensions
       const std::size_t root_joint_id = getRootJointId(model_);
-      const std::size_t nv_root = model_.joints[root_joint_id].nv();
+      const std::size_t nv_root = getRootNv(model_);
       qfull_ = Eigen::VectorXd::Zero(model_.nq);
       vfull_ = Eigen::VectorXd::Zero(model_.nv);
       ufull_ = Eigen::VectorXd::Zero(model_.nv - nv_root);

diff --git a/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h b/include/crocoddyl_msgs/whole_body_trajectory_subscriber.h
@@ -1,7 +1,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2023-2023, Heriot-Watt University
+// Copyright (C) 2023-2024, Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -254,8 +254,7 @@ class WholeBodyTrajectoryRosSubscriber {
     spinner_.start();
 #endif
 
-    const std::size_t root_joint_id = getRootJointId(model_);
-    const std::size_t nv_root = model_.joints[root_joint_id].nv();
+    const std::size_t nv_root = getRootNv(model_);
     if (locked_joints.size() != 0) {
       // Check the size of the reference configuration
       if (qref_.size() != model_.nq) {

diff --git a/src/crocoddyl_ros.cpp b/src/crocoddyl_ros.cpp
@@ -250,6 +250,16 @@ PYBIND11_MODULE(crocoddyl_ros, m) {
            ":param model: Pinocchio model\n"
            ":return root joint id");
 
+  m.def("getRootNq", &getRootNq<0, pinocchio::JointCollectionDefaultTpl>,
+           "Return the root joint nq dimension.\n\n"
+           ":param model: Pinocchio model\n"
+           ":return root joint nq dimension");
+
+  m.def("getRootNv", &getRootNv<0, pinocchio::JointCollectionDefaultTpl>,
+           "Return the root joint nv dimension.\n\n"
+           ":param model: Pinocchio model\n"
+           ":return root joint nv dimension");
+
   m.def(
       "fromReduced",
       &fromReduced_return<0, pinocchio::JointCollectionDefaultTpl>,