Added number_of_links into the robot class

include/sdu_controllers/models/breeding_blanket_handling_robot_model.hpp

@@ -31,7 +31,7 @@ namespace sdu_controllers::models
 
    private:
     std::vector<double> mass_default_;
-    Eigen::Matrix<double, 7, 3> com_default_;
+    Eigen::Matrix<double, Eigen::Dynamic, 3> com_default_;
     std::vector<Eigen::Matrix3d> link_inertia_default_;
   };
 

include/sdu_controllers/models/parameter_robot_model.hpp

@@ -133,6 +133,8 @@ namespace sdu_controllers::models
 
     uint16_t get_dof() const override;
 
+    uint16_t get_number_of_links() const override;
+
     std::vector<double> get_m() override;
 
     Eigen::Vector3d get_g0() override;
@@ -157,6 +159,8 @@ namespace sdu_controllers::models
     // degrees of freedom
     uint16_t dof_;
 
+    uint16_t number_of_links_;
+
     // forward kinematics model (either based on dh parameters or frames)
     std::shared_ptr<kinematics::ForwardKinematics> fk_model_;
 

include/sdu_controllers/models/robot_model.hpp

@@ -149,6 +149,12 @@ namespace sdu_controllers::models
      */
     virtual uint16_t get_dof() const = 0;
 
+    /**
+     * @brief Get the number of links on the robot.
+     * @returns the number of links
+     */
+    virtual uint16_t get_number_of_links() const = 0;
+
     /**
      * @brief Get the masses of the robot links.
      * @returns vector containing the mass of each link

include/sdu_controllers/models/robot_parameters.hpp

@@ -14,6 +14,8 @@ struct RobotParameters
 {
   // degrees of freedom
   uint16_t dof;
+
+  uint16_t number_of_links;
   // forward kinematics model (either based on dh parameters or frames)
   std::shared_ptr<kinematics::ForwardKinematics> fk_model;
 

src/models/parameter_robot_model.cpp

@@ -64,6 +64,7 @@ namespace sdu_controllers::models
       YAML::Node inertial = root["inertial"];
       // determine number of joints by counting entries
       const std::size_t n_links = inertial.size();
+      params.number_of_links = n_links;
       params.mass.resize(n_links);
       params.com.resize(static_cast<int>(n_links), 3);
       params.link_inertia.clear();
@@ -356,6 +357,7 @@ namespace sdu_controllers::models
   void ParameterRobotModel::set_robot_parameters(const RobotParameters &p)
   {
     dof_ = p.dof;
+    number_of_links_ = p.number_of_links;
     // inertial
     mass_ = p.mass;
     com_ = p.com;
@@ -510,6 +512,11 @@ namespace sdu_controllers::models
     return dof_;
   }
 
+  uint16_t ParameterRobotModel::get_number_of_links() const
+  {
+    return number_of_links_;
+  }
+
   std::vector<double> ParameterRobotModel::get_m()
   {
     return mass_;

tests/tests.cpp

@@ -67,12 +67,13 @@
   REQUIRE(params_opt.has_value());
 
   const RobotParameters &p = *params_opt;
-  REQUIRE(p.dof == 7u);
-  REQUIRE(p.mass.size() == p.dof);
-  REQUIRE(p.com.rows() == static_cast<int>(p.dof));
+  //REQUIRE(p.dof == 7u || 6u);
+  REQUIRE(p.mass.size() == p.number_of_links);
+  REQUIRE(p.com.rows() == static_cast<int>(p.number_of_links));
   REQUIRE(p.com.cols() == 3);
   // joint_1 is prismatic
-  REQUIRE(p.joint_types.size() == p.dof);
+  REQUIRE(p.joint_types.size() == p.number_of_links);
+
   REQUIRE(p.joint_types[0] == ForwardKinematics::PRISMATIC);
 }