Use size_t for index variables

moveit_core/robot_model/include/moveit/robot_model/fixed_joint_model.h

@@ -46,7 +46,7 @@ namespace core
 class FixedJointModel : public JointModel
 {
 public:
-  FixedJointModel(const std::string& name);
+  FixedJointModel(const std::string& name, size_t joint_index, size_t first_variable_index);
 
   void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override;
   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,

moveit_core/robot_model/include/moveit/robot_model/floating_joint_model.h

@@ -46,7 +46,7 @@ namespace core
 class FloatingJointModel : public JointModel
 {
 public:
-  FloatingJointModel(const std::string& name);
+  FloatingJointModel(const std::string& name, size_t joint_index, size_t first_variable_index);
 
   void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override;
   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,

moveit_core/robot_model/include/moveit/robot_model/joint_model.h

@@ -90,7 +90,7 @@ class LinkModel;
 class JointModel;
 
 /** \brief Data type for holding mappings from variable names to their position in a state vector */
-typedef std::map<std::string, int> VariableIndexMap;
+typedef std::map<std::string, size_t> VariableIndexMap;
 
 /** \brief Data type for holding mappings from variable names to their bounds */
 using VariableBoundsMap = std::map<std::string, VariableBounds>;
@@ -130,8 +130,14 @@ class JointModel
   /** \brief The datatype for the joint bounds */
   using Bounds = std::vector<VariableBounds>;
 
-  /** \brief Construct a joint named \e name */
-  JointModel(const std::string& name);
+  /**
+   * @brief      Constructs a joint named \e name
+   *
+   * @param[in]  name                   The joint name
+   * @param[in]  index                  The index of the joint in the RobotModel
+   * @param[in]  first_variable_index   The index of the first variable in the RobotModel
+   */
+  JointModel(const std::string& name, size_t joint_index, size_t first_variable_index);
 
   virtual ~JointModel();
 
@@ -207,31 +213,19 @@ class JointModel
   }
 
   /** \brief Get the index of this joint's first variable within the full robot state */
-  int getFirstVariableIndex() const
+  size_t getFirstVariableIndex() const
   {
     return first_variable_index_;
   }
 
-  /** \brief Set the index of this joint's first variable within the full robot state */
-  void setFirstVariableIndex(int index)
-  {
-    first_variable_index_ = index;
-  }
-
   /** \brief Get the index of this joint within the robot model */
-  int getJointIndex() const
+  size_t getJointIndex() const
   {
     return joint_index_;
   }
 
-  /** \brief Set the index of this joint within the robot model */
-  void setJointIndex(int index)
-  {
-    joint_index_ = index;
-  }
-
   /** \brief Get the index of the variable within this joint */
-  int getLocalVariableIndex(const std::string& variable) const;
+  size_t getLocalVariableIndex(const std::string& variable) const;
 
   /** @} */
 
@@ -462,12 +456,19 @@ class JointModel
 
   /** @} */
 
-protected:
-  void computeVariableBoundsMsg();
-
+private:
   /** \brief Name of the joint */
   std::string name_;
 
+  /** \brief Index for this joint in the array of joints of the complete model */
+  size_t joint_index_;
+
+  /** \brief The index of this joint's first variable, in the complete robot state */
+  size_t first_variable_index_;
+
+protected:
+  void computeVariableBoundsMsg();
+
   /** \brief The type of joint */
   JointType type_;
 
@@ -519,12 +520,6 @@ class JointModel
 
   /** \brief The factor applied to the distance between two joint states */
   double distance_factor_;
-
-  /** \brief The index of this joint's first variable, in the complete robot state */
-  int first_variable_index_;
-
-  /** \brief Index for this joint in the array of joints of the complete model */
-  int joint_index_;
 };
 
 /** \brief Operator overload for printing variable bounds to a stream */

moveit_core/robot_model/include/moveit/robot_model/link_model.h

@@ -73,7 +73,13 @@ class LinkModel
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  LinkModel(const std::string& name);
+  /**
+   * @brief      Construct a link model named \e name
+   *
+   * @param[in]  name        The name of the link
+   * @param[in]  link_index  The link index in the RobotModel
+   */
+  LinkModel(const std::string& name, size_t link_index);
   ~LinkModel();
 
   /** \brief The name of this link */
@@ -83,16 +89,11 @@ class LinkModel
   }
 
   /** \brief The index of this joint when traversing the kinematic tree in depth first fashion */
-  int getLinkIndex() const
+  size_t getLinkIndex() const
   {
     return link_index_;
   }
 
-  void setLinkIndex(int index)
-  {
-    link_index_ = index;
-  }
-
   int getFirstCollisionBodyTransformIndex() const
   {
     return first_collision_body_transform_index_;
@@ -231,6 +232,9 @@ class LinkModel
   /** \brief Name of the link */
   std::string name_;
 
+  /** \brief Index of the transform for this link in the full robot frame */
+  size_t link_index_;
+
   /** \brief JointModel that connects this link to the parent link */
   const JointModel* parent_joint_model_;
 
@@ -280,9 +284,6 @@ class LinkModel
   /** \brief Index of the transform for the first shape that makes up the geometry of this link in the full robot state
    */
   int first_collision_body_transform_index_;
-
-  /** \brief Index of the transform for this link in the full robot frame */
-  int link_index_;
 };
 }  // namespace core
 }  // namespace moveit

moveit_core/robot_model/include/moveit/robot_model/planar_joint_model.h

@@ -53,7 +53,7 @@ class PlanarJointModel : public JointModel
     DIFF_DRIVE
   };
 
-  PlanarJointModel(const std::string& name);
+  PlanarJointModel(const std::string& name, size_t joint_index, size_t first_variable_index);
 
   void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override;
   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,

moveit_core/robot_model/include/moveit/robot_model/prismatic_joint_model.h

@@ -48,7 +48,7 @@ class PrismaticJointModel : public JointModel
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  PrismaticJointModel(const std::string& name);
+  PrismaticJointModel(const std::string& name, size_t joint_index, size_t first_variable_index);
 
   void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override;
   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,

moveit_core/robot_model/include/moveit/robot_model/revolute_joint_model.h

@@ -48,7 +48,7 @@ class RevoluteJointModel : public JointModel
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  RevoluteJointModel(const std::string& name);
+  RevoluteJointModel(const std::string& name, size_t joint_index, size_t first_variable_index);
   void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const override;
   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
                                   const Bounds& other_bounds) const override;

moveit_core/robot_model/include/moveit/robot_model/robot_model.h

@@ -139,7 +139,7 @@ class RobotModel
   const JointModel* getJointModel(const std::string& joint) const;
 
   /** \brief Get a joint by its index. Output error and return nullptr when the link is missing. */
-  const JointModel* getJointModel(int index) const;
+  const JointModel* getJointModel(size_t index) const;
 
   /** \brief Get a joint by its name. Output error and return nullptr when the joint is missing. */
   JointModel* getJointModel(const std::string& joint);
@@ -248,7 +248,7 @@ class RobotModel
   const LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr) const;
 
   /** \brief Get a link by its index. Output error and return nullptr when the link is missing. */
-  const LinkModel* getLinkModel(int index) const;
+  const LinkModel* getLinkModel(size_t index) const;
 
   /** \brief Get a link by its name. Output error and return nullptr when the link is missing. */
   LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr);
@@ -444,7 +444,7 @@ class RobotModel
                                std::vector<std::string>& missing_variables) const;
 
   /** \brief Get the index of a variable in the robot state */
-  int getVariableIndex(const std::string& variable) const;
+  size_t getVariableIndex(const std::string& variable) const;
 
   /** \brief Get the deepest joint in the kinematic tree that is a common parent of both joints passed as argument */
   const JointModel* getCommonRoot(const JointModel* a, const JointModel* b) const
@@ -596,6 +596,7 @@ class RobotModel
   /** \brief The array of end-effectors, in alphabetical order */
   std::vector<const JointModelGroup*> end_effectors_;
 
+private:
   /** \brief Given an URDF model and a SRDF model, build a full kinematic model */
   void buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model);
 
@@ -630,10 +631,9 @@ class RobotModel
   /** \brief Construct a JointModelGroup given a SRDF description \e group */
   bool addJointModelGroup(const srdf::Model::Group& group);
 
-  /** \brief Given a urdf joint model, a child link and a set of virtual joints,
+  /** \brief Given a child link and a srdf model,
       build up the corresponding JointModel object*/
-  JointModel* constructJointModel(const urdf::Joint* urdf_joint_model, const urdf::Link* child_link,
-                                  const srdf::Model& srdf_model);
+  JointModel* constructJointModel(const urdf::Link* child_link, const srdf::Model& srdf_model);
 
   /** \brief Given a urdf link, build the corresponding LinkModel object*/
   LinkModel* constructLinkModel(const urdf::Link* urdf_link);

moveit_core/robot_model/src/fixed_joint_model.cpp

@@ -40,7 +40,8 @@ namespace moveit
 {
 namespace core
 {
-FixedJointModel::FixedJointModel(const std::string& name) : JointModel(name)
+FixedJointModel::FixedJointModel(const std::string& name, size_t joint_index, size_t first_variable_index)
+  : JointModel(name, joint_index, first_variable_index)
 {
   type_ = FIXED;
 }

moveit_core/robot_model/src/floating_joint_model.cpp

@@ -53,7 +53,8 @@ constexpr size_t STATE_SPACE_DIMENSION = 7;
 
 }  // namespace
 
-FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0)
+FloatingJointModel::FloatingJointModel(const std::string& name, size_t joint_index, size_t first_variable_index)
+  : JointModel(name, joint_index, first_variable_index), angular_distance_weight_(1.0)
 {
   type_ = FLOATING;
   local_variable_names_.push_back("trans_x");
@@ -65,7 +66,7 @@ FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(nam
   local_variable_names_.push_back("rot_w");
   for (size_t i = 0; i < STATE_SPACE_DIMENSION; ++i)
   {
-    variable_names_.push_back(name_ + "/" + local_variable_names_[i]);
+    variable_names_.push_back(getName() + "/" + local_variable_names_[i]);
     variable_index_map_[variable_names_.back()] = i;
   }
 

moveit_core/robot_model/src/joint_model.cpp

@@ -44,8 +44,10 @@ namespace moveit
 {
 namespace core
 {
-JointModel::JointModel(const std::string& name)
+JointModel::JointModel(const std::string& name, size_t joint_index, size_t first_variable_index)
   : name_(name)
+  , joint_index_(joint_index)
+  , first_variable_index_(first_variable_index)
   , type_(UNKNOWN)
   , parent_link_model_(nullptr)
   , child_link_model_(nullptr)
@@ -54,8 +56,6 @@ JointModel::JointModel(const std::string& name)
   , mimic_offset_(0.0)
   , passive_(false)
   , distance_factor_(1.0)
-  , first_variable_index_(-1)
-  , joint_index_(-1)
 {
 }
 
@@ -82,7 +82,7 @@ std::string JointModel::getTypeName() const
   }
 }
 
-int JointModel::getLocalVariableIndex(const std::string& variable) const
+size_t JointModel::getLocalVariableIndex(const std::string& variable) const
 {
   VariableIndexMap::const_iterator it = variable_index_map_.find(variable);
   if (it == variable_index_map_.end())

moveit_core/robot_model/src/link_model.cpp

@@ -44,14 +44,14 @@ namespace moveit
 {
 namespace core
 {
-LinkModel::LinkModel(const std::string& name)
+LinkModel::LinkModel(const std::string& name, size_t link_index)
   : name_(name)
+  , link_index_(link_index)
   , parent_joint_model_(nullptr)
   , parent_link_model_(nullptr)
   , is_parent_joint_fixed_(false)
   , joint_origin_transform_is_identity_(true)
   , first_collision_body_transform_index_(-1)
-  , link_index_(-1)
 {
   joint_origin_transform_.setIdentity();
 }

moveit_core/robot_model/src/planar_joint_model.cpp

@@ -46,8 +46,11 @@ namespace moveit
 {
 namespace core
 {
-PlanarJointModel::PlanarJointModel(const std::string& name)
-  : JointModel(name), angular_distance_weight_(1.0), motion_model_(HOLONOMIC), min_translational_distance_(1e-5)
+PlanarJointModel::PlanarJointModel(const std::string& name, size_t joint_index, size_t first_variable_index)
+  : JointModel(name, joint_index, first_variable_index)
+  , angular_distance_weight_(1.0)
+  , motion_model_(HOLONOMIC)
+  , min_translational_distance_(1e-5)
 {
   type_ = PLANAR;
 
@@ -56,7 +59,7 @@ PlanarJointModel::PlanarJointModel(const std::string& name)
   local_variable_names_.push_back("theta");
   for (int i = 0; i < 3; ++i)
   {
-    variable_names_.push_back(name_ + "/" + local_variable_names_[i]);
+    variable_names_.push_back(getName() + "/" + local_variable_names_[i]);
     variable_index_map_[variable_names_.back()] = i;
   }
 

moveit_core/robot_model/src/prismatic_joint_model.cpp

@@ -41,15 +41,16 @@ namespace moveit
 {
 namespace core
 {
-PrismaticJointModel::PrismaticJointModel(const std::string& name) : JointModel(name), axis_(0.0, 0.0, 0.0)
+PrismaticJointModel::PrismaticJointModel(const std::string& name, size_t joint_index, size_t first_variable_index)
+  : JointModel(name, joint_index, first_variable_index), axis_(0.0, 0.0, 0.0)
 {
   type_ = PRISMATIC;
-  variable_names_.push_back(name_);
+  variable_names_.push_back(getName());
   variable_bounds_.resize(1);
   variable_bounds_[0].position_bounded_ = true;
   variable_bounds_[0].min_position_ = -std::numeric_limits<double>::max();
   variable_bounds_[0].max_position_ = std::numeric_limits<double>::max();
-  variable_index_map_[name_] = 0;
+  variable_index_map_[getName()] = 0;
   computeVariableBoundsMsg();
 }
 

moveit_core/robot_model/src/revolute_joint_model.cpp

@@ -45,16 +45,24 @@ namespace moveit
 {
 namespace core
 {
-RevoluteJointModel::RevoluteJointModel(const std::string& name)
-  : JointModel(name), axis_(0.0, 0.0, 0.0), continuous_(false), x2_(0.0), y2_(0.0), z2_(0.0), xy_(0.0), xz_(0.0), yz_(0.0)
+RevoluteJointModel::RevoluteJointModel(const std::string& name, size_t joint_index, size_t first_variable_index)
+  : JointModel(name, joint_index, first_variable_index)
+  , axis_(0.0, 0.0, 0.0)
+  , continuous_(false)
+  , x2_(0.0)
+  , y2_(0.0)
+  , z2_(0.0)
+  , xy_(0.0)
+  , xz_(0.0)
+  , yz_(0.0)
 {
   type_ = REVOLUTE;
-  variable_names_.push_back(name_);
+  variable_names_.push_back(getName());
   variable_bounds_.resize(1);
   variable_bounds_[0].position_bounded_ = true;
   variable_bounds_[0].min_position_ = -boost::math::constants::pi<double>();
   variable_bounds_[0].max_position_ = boost::math::constants::pi<double>();
-  variable_index_map_[name_] = 0;
+  variable_index_map_[getName()] = 0;
   computeVariableBoundsMsg();
 }