[fix] Fixed a bug that caused joints to return to their original posi…

…tion after being moved to the initial position.
PickNikRobotics · Sep 5, 2024 · ffa421f · ffa421f
1 parent 6dccdc0
commit ffa421f
Showing 1 changed file with 18 additions and 14 deletions.
diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp
@@ -294,33 +294,37 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
     return hardware_interface::return_type::ERROR;
   }
 
+  std::vector<std::vector<double>> target_joint_commands;
   if (initial_cmd_reached_ == false)
   {
-    const auto diff = std::transform_reduce(
-        joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(),
-        initial_joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0,
-        [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus<double>{});
-    if (diff < trigger_joint_command_threshold_)
+    double abs_sum = std::accumulate(joint_commands_[POSITION_INTERFACE_INDEX].begin(), joint_commands_[POSITION_INTERFACE_INDEX].end(), 0, [](double sum, double val) {
+        return sum + std::abs(val);
+    });
+    if (abs_sum >= trigger_joint_command_threshold_)
     {
       initial_cmd_reached_ = true;
     }
-    else
-    {
-      joint_commands_ = initial_joint_commands_;
-    }
+  }
+  if (initial_cmd_reached_ == false)
+  {
+    target_joint_commands = initial_joint_commands_;
+  }
+  else
+  {
+    target_joint_commands = joint_commands_;
   }
   // To avoid spamming TopicBased's joint command topic we check the difference between the joint states and
   // the current joint commands, if it's smaller than a threshold we don't publish it.
   const auto diff = std::transform_reduce(
       joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(),
-      joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0,
+      target_joint_commands[POSITION_INTERFACE_INDEX].cbegin(), 0.0,
       [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus<double>{});
 
   bool exist_velocity_command = false;
   static bool exist_velocity_command_old = false; // Use old state to publish zero velocities
   for (std::size_t i = 0; i < info_.joints.size(); ++i)
   {
-    if (fabs(joint_commands_[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_)
+    if (fabs(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_)
     {
       exist_velocity_command = true;
     }
@@ -345,7 +349,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
         if (interface.name == hardware_interface::HW_IF_POSITION)
         {
           joint_state.name.push_back(info_.joints[i].name);
-          joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]);
+          joint_state.position.push_back(target_joint_commands[POSITION_INTERFACE_INDEX][i]);
         }
       }
     }
@@ -386,7 +390,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
         if (interface.name == hardware_interface::HW_IF_VELOCITY)
         {
           joint_state.name.push_back(info_.joints[i].name);
-          joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]);
+          joint_state.velocity.push_back(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]);
         }
       }
     }
@@ -427,7 +431,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
         if (interface.name == hardware_interface::HW_IF_EFFORT)
         {
           joint_state.name.push_back(info_.joints[i].name);
-          joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]);
+          joint_state.effort.push_back(target_joint_commands[EFFORT_INTERFACE_INDEX][i]);
         }
       }
     }