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})); } }