From ad76ddccc4b1b78f110202879bded5146670d109 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 27 Feb 2024 09:58:41 +0100 Subject: [PATCH] Fix `initial_value` not working (backport #241) (#243) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Reset Gazebo with initial joint positions and velocities (#241) (cherry picked from commit c5b0b9049ce75410e75d1828242c1dfd5b19bb80) # Conflicts: # ign_ros2_control/src/ign_system.cpp * Fixed merge Signed-off-by: Alejandro Hernández Cordero * Fix Signed-off-by: Alejandro Hernández Cordero --------- Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Ruddick Lawrence <679360+mrjogo@users.noreply.github.com> Co-authored-by: Alejandro Hernández Cordero --- ign_ros2_control/src/ign_system.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index c4623d49..b949ca0a 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -28,8 +28,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -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})); } }