Change dt for pid to appropriate measure.

ros-controls · Sep 21, 2021 · 9e695fb · 9e695fb
1 parent 09e6954
commit 9e695fb
Showing 1 changed file with 5 additions and 5 deletions.
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -205,11 +205,11 @@ controller_interface::return_type JointTrajectoryController::update(
           // Update PIDs
           for (auto i = 0ul; i < joint_num; ++i)
           {
-            tmp_command_[i] =
-              (state_desired.velocities[i] * ff_velocity_scale_[i]) +
-              pids_[i]->computeCommand(
-                state_desired.positions[i] - state_current.positions[i],
-                state_desired.velocities[i] - state_current.velocities[i], period.seconds());
+            tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) +
+                              pids_[i]->computeCommand(
+                                state_desired.positions[i] - state_current.positions[i],
+                                state_desired.velocities[i] - state_current.velocities[i],
+                                (uint64_t)period.nanoseconds());
           }
 
           assign_interface_from_point(joint_command_interface_[1], tmp_command_);