Add velocity feedforward term to HardwareInterfaceAdapter for velocit…

…y_controllers::JointTrajectoryController.
ros-controls · Jun 15, 2016 · d02dd6d · d02dd6d
1 parent b4dc152
commit d02dd6d
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h b/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h
@@ -199,7 +199,7 @@ class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State
 
   void updateCommand(const ros::Time&     /*time*/,
                      const ros::Duration& period,
-                     const State&         /*desired_state*/,
+                     const State&         desired_state,
                      const State&         state_error)
   {
     const unsigned int n_joints = joint_handles_ptr_->size();
@@ -213,7 +213,7 @@ class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State
     // Update PIDs
     for (unsigned int i = 0; i < n_joints; ++i)
     {
-      const double command = pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
+      const double command = desired_state.velocity[i] + pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
       (*joint_handles_ptr_)[i].setCommand(command);
     }
   }