diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 73f424a7..09410963 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -282,16 +282,10 @@ bool IgnitionSystem::initSim( } // independently of existence of command interface set initial value if defined if (!std::isnan(initial_position)) { - const auto * jointPositions = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[j].sim_joint); this->dataPtr->joints_[j].joint_position = initial_position; } if (!std::isnan(initial_velocity)) { - const auto * jointVelocity = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[j].sim_joint); - this->dataPtr->joints_[j].joint_velocity = jointVelocity->Data()[0]; + this->dataPtr->joints_[j].joint_velocity = initial_velocity; } } }