feat: restructure joint getters and support continuous joints

CHANGELOG.md

@@ -28,6 +28,7 @@ Release Versions
 - feat: expose pinocchio::Data through getter (#238)
 - feat: add functionality to create a robot model from string (#200)
 - feat: add joint types & limit some uses to supported types only (#243)
+- feat: restructure joint getters and support continuous joints (#245, #246)
 
 ## 9.2.0
 

source/controllers/README.md

@@ -150,7 +150,7 @@ auto ctrl = CartesianControllerFactory::create_controller(CONTROLLER_TYPE::IMPED
 auto robot = robot_model::Model("my_robot", "/path/to/robot.urdf");
 
 // create some state variables
-auto current_joints = JointState::Random(robot.get_robot_name(), robot.get_number_of_joints());
+auto current_joints = JointState::Random(robot.get_robot_name(), robot.get_configuration_dimension());
 auto command_state = CartesianState::Random("command");
 auto feedback_state = CartesianState::Random("feedback");
 

source/controllers/src/ControllerFactory.cpp

@@ -88,7 +88,8 @@ std::shared_ptr<IController<JointState>> ControllerFactory<JointState>::create_c
     CONTROLLER_TYPE type, const std::list<std::shared_ptr<ParameterInterface>>& parameters,
     const robot_model::Model& robot_model
 ) {
-  auto ctrl = ControllerFactory<JointState>::create_controller(type, parameters, robot_model.get_number_of_joints());
+  auto ctrl =
+      ControllerFactory<JointState>::create_controller(type, parameters, robot_model.get_configuration_dimension());
   if (ctrl == nullptr) {
     throw exceptions::InvalidControllerException("Cannot assign robot model to this controller!");
   }

source/controllers/test/tests/test_controller_factory.cpp

@@ -180,7 +180,7 @@ TEST(ControllerFactoryTest, CreateControllerWithRobot) {
 
   auto command_state = CartesianState::Identity(robot.get_frames().back(), robot.get_base_frame());
   auto feedback_state = CartesianState::Identity(robot.get_frames().back(), robot.get_base_frame());
-  auto joint_state = JointState::Zero(robot_name, robot.get_number_of_joints());
+  auto joint_state = JointState::Zero(robot_name, robot.get_configuration_dimension());
 
   EXPECT_THROW(
       CartesianControllerFactory::create_controller(CONTROLLER_TYPE::NONE, robot),
@@ -203,7 +203,7 @@ TEST(ControllerFactoryTest, CreateControllerWithRobot) {
   EXPECT_NO_THROW(joint_ctrl->compute_command(joint_state, joint_state));
   EXPECT_EQ(
       joint_ctrl->get_parameter_value<Eigen::MatrixXd>("stiffness").size(),
-      robot.get_number_of_joints() * robot.get_number_of_joints()
+      robot.get_configuration_dimension() * robot.get_configuration_dimension()
   );
 }
 
@@ -221,8 +221,8 @@ TEST(ControllerFactoryTest, CreateControllerWithRobotAndParams) {
   ASSERT_NE(ctrl, nullptr);
   EXPECT_EQ(
       ctrl->get_parameter_value<Eigen::MatrixXd>("damping").size(),
-      robot.get_number_of_joints() * robot.get_number_of_joints()
+      robot.get_configuration_dimension() * robot.get_configuration_dimension()
   );
 
-  EXPECT_EQ(ctrl->get_parameter_value<Eigen::MatrixXd>("damping").sum(), 5.0 * robot.get_number_of_joints());
+  EXPECT_EQ(ctrl->get_parameter_value<Eigen::MatrixXd>("damping").sum(), 5.0 * robot.get_configuration_dimension());
 }

source/robot_model/include/robot_model/Model.hpp

@@ -196,6 +196,10 @@ class Model {
   */
   std::vector<pinocchio::CollisionPair> generate_joint_exclusion_list();
 
+protected:
+  Eigen::VectorXd
+  expand_joint_positions_to_full_configuration(const state_representation::JointPositions& joint_positions) const;
+
 public:
   /**
    * @brief Construct with robot name and path to URDF file
@@ -307,10 +311,22 @@ class Model {
 
   /**
    * @brief Getter of the number of joints
-   * @return the number of joints
+   * @return the number of joints (minus pinocchio's universe root joint and fixed joints)
    */
   unsigned int get_number_of_joints() const;
 
+  /**
+   * @brief Getter of the degrees of freedom (dof) of the model
+   * @return the number of degrees of freedom (dof) of the model
+   */
+  unsigned int get_dof() const;
+
+  /**
+   * @brief Getter of the configuration dimension of the model
+   * @return the number of configuration dimensions of the model
+   */
+  unsigned int get_configuration_dimension() const;
+
   /**
    * @brief Getter of the joint frames from the model
    * @return the joint frames
@@ -321,7 +337,7 @@ class Model {
    * @brief Getter of the types of the current model's joints
    * @return a vector of joint types
    */
-  std::vector<JointType> get_joint_types();
+  std::vector<JointType> get_joint_types() const;
 
   /**
    * @brief Getter of the frames from the model
@@ -651,7 +667,15 @@ inline std::optional<std::reference_wrapper<const std::string>> Model::get_urdf_
 }
 
 inline unsigned int Model::get_number_of_joints() const {
-  return this->robot_model_.nq;// ! todo: this is actually incorrect, that gives dofs
+  return this->robot_model_.njoints - 1;
+}
+
+inline unsigned int Model::get_dof() const {
+  return this->robot_model_.nv;
+}
+
+inline unsigned int Model::get_configuration_dimension() const {
+  return this->robot_model_.nq;
 }
 
 inline std::vector<std::string> Model::get_joint_frames() const {
@@ -726,7 +750,7 @@ struct JointTypeVisitor : public boost::static_visitor<JointType> {
   }
 };
 
-inline std::vector<JointType> Model::get_joint_types() {
+inline std::vector<JointType> Model::get_joint_types() const {
   std::vector<JointType> joint_types;
   JointTypeVisitor classifier;
   for (int j = 1; j < this->robot_model_.njoints; ++j) {// skips the first joint (universe)
@@ -736,4 +760,34 @@ inline std::vector<JointType> Model::get_joint_types() {
   }
   return joint_types;
 }
+
+inline Eigen::VectorXd
+Model::expand_joint_positions_to_full_configuration(const state_representation::JointPositions& joint_positions) const {
+  if (this->get_number_of_joints() == this->get_configuration_dimension()) {
+    return joint_positions.get_positions();
+  }
+  Eigen::VectorXd q(this->get_configuration_dimension());
+  const auto& joint_types = this->get_joint_types();
+  const Eigen::VectorXd& positions = joint_positions.get_positions();
+  std::size_t joint_input_idx = 0;
+  for (int i = 1; i < this->robot_model_.njoints; ++i) {
+    const auto& joint = this->robot_model_.joints[i];
+    std::size_t idx_q = joint.idx_q();
+    switch (joint_types[i - 1]) {
+      case JointType::CONTINUOUS: {
+        double angle = positions[joint_input_idx++];
+        q[idx_q + 0] = std::cos(angle);
+        q[idx_q + 1] = std::sin(angle);
+        break;
+      }
+      default: {
+        for (int j = 0; j < joint.nq(); ++j) {
+          q[idx_q + j] = positions[joint_input_idx++];
+        }
+        break;
+      }
+    }
+  }
+  return q;
+}
 }// namespace robot_model

source/robot_model/src/Model.cpp

@@ -277,16 +277,50 @@ Model::compute_jacobian(const state_representation::JointPositions& joint_positi
     throw exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints());
   }
   // compute the Jacobian from the joint state
-  pinocchio::Data::Matrix6x J(6, this->get_number_of_joints());
+  pinocchio::Data::Matrix6x J(6, this->get_configuration_dimension());
   J.setZero();
-  pinocchio::computeFrameJacobian(
-      this->robot_model_, this->robot_data_, joint_positions.data(), frame_id, pinocchio::LOCAL_WORLD_ALIGNED, J
-  );
-  // the model does not have any reference frame
-  return state_representation::Jacobian(
-      this->get_robot_name(), this->get_joint_frames(), this->robot_model_.frames[frame_id].name, J,
-      this->get_base_frame()
-  );
+  if (this->get_configuration_dimension() != this->get_number_of_joints()) [[unlikely]] {
+    Eigen::VectorXd q = this->expand_joint_positions_to_full_configuration(joint_positions);
+    Eigen::MatrixXd P(this->robot_model_.nq, this->robot_model_.nv);
+    P.setZero();
+    static const auto& joint_types = this->get_joint_types();
+    for (int joint_input_idx = 0, i = 1; i < this->robot_model_.njoints; ++i) {
+      const auto& joint = this->robot_model_.joints[i];
+      std::size_t idx_q = joint.idx_q();
+      std::size_t idx_v = joint.idx_v();
+      switch (joint_types[i - 1]) {
+        case JointType::CONTINUOUS: {
+          double angle = joint_positions.get_positions()[joint_input_idx++];
+          P(idx_q, idx_v) = -std::sin(angle);
+          P(idx_q + 1, idx_v) = std::cos(angle);
+          break;
+        }
+        default: {
+          for (int j = 0; j < joint.nq(); ++j) {
+            P(idx_q + j, idx_v + j) = 1.0;
+          }
+          joint_input_idx += joint.nq();
+          break;
+        }
+      }
+    }
+    Eigen::MatrixXd Jv = J * P;
+    pinocchio::computeFrameJacobian(
+        this->robot_model_, this->robot_data_, q, frame_id, pinocchio::LOCAL_WORLD_ALIGNED, Jv
+    );
+    return state_representation::Jacobian(
+        this->get_robot_name(), this->get_joint_frames(), this->robot_model_.frames[frame_id].name, Jv,
+        this->get_base_frame()
+    );
+  } else [[likely]] {
+    pinocchio::computeFrameJacobian(
+        this->robot_model_, this->robot_data_, joint_positions.data(), frame_id, pinocchio::LOCAL_WORLD_ALIGNED, J
+    );
+    return state_representation::Jacobian(
+        this->get_robot_name(), this->get_joint_frames(), this->robot_model_.frames[frame_id].name, J,
+        this->get_base_frame()
+    );
+  }
 }
 
 state_representation::Jacobian
@@ -373,8 +407,10 @@ std::vector<state_representation::CartesianPose> Model::forward_kinematics(
   if (joint_positions.get_size() != this->get_number_of_joints()) {
     throw exceptions::InvalidJointStateSizeException(joint_positions.get_size(), this->get_number_of_joints());
   }
+
+  Eigen::VectorXd q = this->expand_joint_positions_to_full_configuration(joint_positions);
   std::vector<state_representation::CartesianPose> pose_vector;
-  pinocchio::forwardKinematics(this->robot_model_, this->robot_data_, joint_positions.data());
+  pinocchio::forwardKinematics(this->robot_model_, this->robot_data_, q);
   for (unsigned int id : frame_ids) {
     if (id >= static_cast<unsigned int>(this->robot_model_.nframes)) {
       throw exceptions::FrameNotFoundException(std::to_string(id));

source/robot_model/test/tests/test_joint_types.cpp

@@ -1,6 +1,7 @@
 #include "robot_model/Model.hpp"
 
 #include <cassert>
+#include <cstddef>
 #include <state_representation/space/joint/JointState.hpp>
 
 #include <gtest/gtest.h>
@@ -21,24 +22,59 @@ class JointTypeTesting : public testing::Test {
 
 TEST_F(JointTypeTesting, CheckContinuousJoint) {
   // clang-format off
-  auto urdf =
-  "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
-  "<robot name=\"continuous\">"
-    "<link name=\"world\"/>"
-    "<joint name=\"base_joint\" type=\"continuous\">"
-      "<parent link=\"world\"/>"
-      "<child link=\"base\"/>"
-      "<axis xyz=\"0 0 1\"/>"
-    "</joint>"
-    "<link name=\"base\"/>"
-  "</robot>";
+    auto urdf =
+    "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
+    "<robot name=\"continuous\">"
+      "<link name=\"world\"/>"
+      "<joint name=\"base_joint\" type=\"continuous\">"
+        "<parent link=\"world\"/>"
+        "<child link=\"base\"/>"
+        "<axis xyz=\"0 0 1\"/>"
+      "</joint>"
+      "<link name=\"base\"/>"
+    "</robot>";
   // clang-format on
+
   this->robot_ = std::make_unique<Model>("continuous", urdf);
   EXPECT_STREQ(this->robot_->get_robot_name().c_str(), "continuous");
   auto joint_types = this->robot_->get_joint_types();
   EXPECT_EQ(joint_types.size(), 1);
   EXPECT_EQ(joint_types.at(0), JointType::CONTINUOUS);
-  EXPECT_EQ(this->robot_->get_number_of_joints(), 2);
+  EXPECT_EQ(this->robot_->get_number_of_joints(), 1);
+  EXPECT_EQ(this->robot_->get_dof(), 1);
+  EXPECT_EQ(this->robot_->get_configuration_dimension(), 2);
+
+  auto& data = this->robot_->get_pinocchio_data();
+
+  const double angle = M_PI / 3.0;
+  Eigen::VectorXd q = Eigen::VectorXd::Zero(this->robot_->get_number_of_joints());
+  for (size_t i = 0; i < this->robot_->get_joint_types().size(); ++i) {
+    if (this->robot_->get_joint_types().at(i) == JointType::CONTINUOUS) {
+      q[i] = angle;
+    }
+  }
+  state_representation::JointState joint_state(this->robot_->get_robot_name(), this->robot_->get_joint_frames());
+  joint_state.set_positions(q);
+  this->robot_->forward_kinematics(joint_state);
+
+  const auto& M = data.oMi[1];
+  Eigen::Matrix3d R_exp = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()).toRotationMatrix();
+  EXPECT_TRUE(M.rotation().isApprox(R_exp, 1e-9)) << "Rotation should be a pure Rz(pi/3)";
+
+  Eigen::VectorXd v(1);
+  v << 1.0;
+  joint_state.set_velocities(v);
+
+  auto twist = this->robot_->forward_velocity(joint_state);
+  EXPECT_EQ(twist.get_reference_frame(), this->robot_->get_base_frame());
+  EXPECT_EQ(twist.get_name(), "base");
+
+  Eigen::Vector3d expected_angular(0.0, 0.0, 1.0);
+  EXPECT_TRUE(twist.get_angular_velocity().isApprox(expected_angular, 1e-9))
+      << "Expected angular velocity [0, 0, 1], got: " << twist.get_angular_velocity().transpose();
+
+  EXPECT_TRUE(twist.get_linear_velocity().isZero(1e-9))
+      << "Expected zero linear velocity, got: " << twist.get_linear_velocity().transpose();
 }
 
 TEST_F(JointTypeTesting, CheckPrismaticJoint) {
@@ -84,7 +120,7 @@ TEST_F(JointTypeTesting, CheckFloatingJoint) {
   // clang-format on
   this->robot_ = std::make_unique<Model>("floating", urdf);
   EXPECT_STREQ(this->robot_->get_robot_name().c_str(), "floating");
-  EXPECT_EQ(this->robot_->get_number_of_joints(), 7);
+  EXPECT_EQ(this->robot_->get_number_of_joints(), 1);
   auto joint_types = this->robot_->get_joint_types();
   EXPECT_EQ(joint_types.size(), 1);
   EXPECT_EQ(joint_types.at(0), JointType::FLOATING);
@@ -128,7 +164,7 @@ TEST_F(JointTypeTesting, CheckPlanarJoint) {
   // clang-format on
   this->robot_ = std::make_unique<Model>("planar", urdf);
   EXPECT_STREQ(this->robot_->get_robot_name().c_str(), "planar");
-  EXPECT_EQ(this->robot_->get_number_of_joints(), 4);
+  EXPECT_EQ(this->robot_->get_number_of_joints(), 1);
   auto joint_types = this->robot_->get_joint_types();
   EXPECT_EQ(joint_types.size(), 1);
   EXPECT_EQ(joint_types.at(0), JointType::PLANAR);