Optional robot state initialization

PickNikRobotics · Mar 16, 2023 · bf34094 · bf34094
1 parent 0f9cd01
commit bf34094
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 0 deletions.
diff --git a/include/ros_control_boilerplate/generic_hw_interface.h b/include/ros_control_boilerplate/generic_hw_interface.h
@@ -180,6 +180,7 @@ class GenericHWInterface : public hardware_interface::RobotHW
 
   // Configuration
   std::vector<std::string> joint_names_;
+  std::vector<double> initial_state_;
   std::size_t num_joints_;
   urdf::Model* urdf_model_;
 

diff --git a/src/generic_hw_interface.cpp b/src/generic_hw_interface.cpp
@@ -58,6 +58,15 @@ GenericHWInterface::GenericHWInterface(const ros::NodeHandle& nh, urdf::Model* u
       nh_, "hardware_interface");  // TODO(davetcoleman): change the namespace to "generic_hw_interface" aka name_
   std::size_t error = 0;
   error += !rosparam_shortcuts::get(name_, rpnh, "joints", joint_names_);
+
+  if (rpnh.hasParam("initial_state"))
+  {
+    error += !rosparam_shortcuts::get(name_, rpnh, "initial_state", initial_state_);
+
+    if (initial_state_.size() != joint_names_.size())
+      ROS_WARN("skipping initial_state, does not match joints size");
+  }
+
   rosparam_shortcuts::shutdownIfError(name_, error);
 }
 
@@ -81,6 +90,16 @@ void GenericHWInterface::init()
   joint_velocity_limits_.resize(num_joints_, 0.0);
   joint_effort_limits_.resize(num_joints_, 0.0);
 
+  // Initialize robot state
+  if (initial_state_.size() != num_joints_)
+  {
+    initial_state_.clear();
+    initial_state_.resize(joint_names_.size(), 0.0);
+  }
+
+  for (std::size_t i = 0; i < num_joints_; ++i)
+    joint_position_[i] = initial_state_[i];
+
   // Initialize interfaces for each joint
   for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
   {