From 28a1d3242bbadcb2413e92816b25a1a3b27287bc Mon Sep 17 00:00:00 2001
From: Ruddick Lawrence <679360+mrjogo@users.noreply.github.com>
Date: Mon, 26 Feb 2024 01:03:19 -0800
Subject: [PATCH] Reset Gazebo with initial joint positions and velocities
 (#241)

(cherry picked from commit c5b0b9049ce75410e75d1828242c1dfd5b19bb80)
---
 gz_ros2_control/src/gz_system.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp
index ecbebf7c..351bfc87 100644
--- a/gz_ros2_control/src/gz_system.cpp
+++ b/gz_ros2_control/src/gz_system.cpp
@@ -32,6 +32,7 @@
 #include <gz/sim/components/JointPositionReset.hh>
 #include <gz/sim/components/JointVelocity.hh>
 #include <gz/sim/components/JointVelocityCmd.hh>
+#include <gz/sim/components/JointVelocityReset.hh>
 #include <gz/sim/components/LinearAcceleration.hh>
 #include <gz/sim/components/Name.hh>
 #include <gz/sim/components/ParentEntity.hh>
@@ -48,8 +49,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>
@@ -397,9 +400,15 @@ bool GazeboSimSystem::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,
+          sim::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,
+          sim::components::JointVelocityReset({initial_velocity}));
       }
     }