Fix initial_value not working (backport #241) (#243)

* Reset Gazebo with initial joint positions and velocities (#241) (cherry picked from commit c5b0b90) # Conflicts: # ign_ros2_control/src/ign_system.cpp * Fixed merge Signed-off-by: Alejandro Hernández Cordero <[email protected]> * Fix Signed-off-by: Alejandro Hernández Cordero <[email protected]> --------- Signed-off-by: Alejandro Hernández Cordero <[email protected]> Co-authored-by: Ruddick Lawrence <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]>
ros-controls · Feb 27, 2024 · ad76ddc · ad76ddc
1 parent 62e7663
commit ad76ddc
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp
@@ -28,8 +28,10 @@
 #include <ignition/gazebo/components/JointForce.hh>
 #include <ignition/gazebo/components/JointForceCmd.hh>
 #include <ignition/gazebo/components/JointPosition.hh>
+#include <ignition/gazebo/components/JointPositionReset.hh>
 #include <ignition/gazebo/components/JointVelocity.hh>
 #include <ignition/gazebo/components/JointVelocityCmd.hh>
+#include <ignition/gazebo/components/JointVelocityReset.hh>
 #include <ignition/gazebo/components/LinearAcceleration.hh>
 #include <ignition/gazebo/components/Name.hh>
 #include <ignition/gazebo/components/ParentEntity.hh>
@@ -376,9 +378,15 @@ bool IgnitionSystem::initSim(
       // independently of existence of command interface set initial value if defined
       if (!std::isnan(initial_position)) {
         this->dataPtr->joints_[j].joint_position = initial_position;
+        this->dataPtr->ecm->CreateComponent(
+          this->dataPtr->joints_[j].sim_joint,
+          ignition::gazebo::components::JointPositionReset({initial_position}));
       }
       if (!std::isnan(initial_velocity)) {
         this->dataPtr->joints_[j].joint_velocity = initial_velocity;
+        this->dataPtr->ecm->CreateComponent(
+          this->dataPtr->joints_[j].sim_joint,
+          ignition::gazebo::components::JointVelocityReset({initial_velocity}));
       }
     }