Fixed for loop issiue, effort commands are working!

KNR-PW · Nov 17, 2024 · 7b99fda · 7b99fda
1 parent b59477b
commit 7b99fda
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 3 deletions.
diff --git a/joint_controller_msgs/msg/JointCommand.msg b/joint_controller_msgs/msg/JointCommand.msg
@@ -5,8 +5,9 @@
 # constants for a PD controller.
 #
 # Applied joint torque is calculated as follows:
-# effort = feedforward_effort + kp_scale * kp * position_error + kd_scale * kd * velocity_error
-# where kp and kd are constants in joint controller
+# effort = feedforward_effort + kp_scale * kp * position_error 
+# + kd_scale * kd * velocity_error + ki * integration_error 
+# where kp, kd and ki are constants in joint controller parameters
 
 std_msgs/Header header
 

diff --git a/joint_controller_ros2_control/src/joint_controller.cpp b/joint_controller_ros2_control/src/joint_controller.cpp
@@ -198,12 +198,13 @@ controller_interface::return_type JointController::update_reference_from_subscri
 controller_interface::return_type JointController::update_and_write_commands(
       const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-    for(size_t i; i < joint_num_; ++i)
+    for(size_t i = 0; i < joint_num_; ++i)
     {
         joint_states_[i].position_ = position_state_interfaces_[i].get().get_value();
         joint_states_[i].velocity_ = velocity_state_interfaces_[i].get().get_value();
 
         double torque = joint_controllers_[i].calculateEffort(joint_commands_[i], joint_states_[i]);
+        std::cout << "Torque: " << torque << std::endl;
         effort_command_interfaces_[i].get().set_value(torque);
     }
     return controller_interface::return_type::OK;