[effort_controllers] JointGroupPositionController: Set to current pos…

…ition in starting

Currently, the target position upon starting is all zeros. This is not
great if a command is not issued immediately. To be safer, set the goal
position to the current sensed position and also reset the PID internal
state upon starting.

effort_controllers/include/effort_controllers/joint_group_position_controller.h

@@ -72,6 +72,7 @@ class JointGroupPositionController : public controller_interface::Controller<har
 
   bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n);
   void update(const ros::Time& /*time*/, const ros::Duration& /*period*/);
+  void starting(const ros::Time& /*time*/);
 
   std::vector< std::string > joint_names_;
   std::vector< hardware_interface::JointHandle > joints_;

effort_controllers/src/joint_group_position_controller.cpp

@@ -120,14 +120,26 @@ namespace effort_controllers
     return true;
   }
 
+  void JointGroupPositionController::starting(const ros::Time& time)
+  {
+    std::vector<double> current_positions(n_joints_, 0.0);
+    for (std::size_t i = 0; i < n_joints_; ++i)
+    {
+      current_positions[i] = joints_[i].getPosition();
+      enforceJointLimits(current_positions[i], i);
+      pid_controllers_[i].reset();
+    }
+    commands_buffer_.writeFromNonRT(current_positions);
+  }
+
   void JointGroupPositionController::update(const ros::Time& time, const ros::Duration& period)
   {
     std::vector<double> & commands = *commands_buffer_.readFromRT();
     for(unsigned int i=0; i<n_joints_; i++)
     {
         double command_position = commands[i];
 
-        double error; //, vel_error;
+        double error;
         double commanded_effort;
 
         double current_position = joints_[i].getPosition();