diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index d5aa0b8a..a60a4eb3 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -48,8 +49,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -427,9 +430,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})); } }