Forced zero vel in position mode to avoid sagging (#213)

(cherry picked from commit 3e95061)
ros-controls · Jun 23, 2023 · 2f7349c · 2f7349c
1 parent cbaa8ae
commit 2f7349c
Showing 1 changed file with 1 addition and 0 deletions.
diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -603,6 +603,7 @@ hardware_interface::return_type GazeboSystem::write(
     if (this->dataPtr->sim_joints_[j]) {
       if (this->dataPtr->joint_control_methods_[j] & POSITION) {
         this->dataPtr->sim_joints_[j]->SetPosition(0, this->dataPtr->joint_position_cmd_[j], true);
+        this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
       } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { // NOLINT
         this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]);
       } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT