From a7793201917d28c459700db489f28a27172db9b2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 7 Apr 2024 17:50:07 +0000 Subject: [PATCH] Set initial position before simulation starts --- gazebo_ros2_control/src/gazebo_system.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index b8621fbf..4d596db7 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -314,6 +314,15 @@ void GazeboSystem::registerJoints( // check if joint is actuated (has command interfaces) or passive this->dataPtr->is_joint_actuated_[j] = (joint_info.command_interfaces.size() > 0); } + + // set initial position for mimic joints + for (const auto & mimic_joint : hardware_info.mimic_joints) { + this->dataPtr->sim_joints_[mimic_joint.joint_index]->SetPosition( + 0, + mimic_joint.offset + mimic_joint.multiplier * + this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index], + true); + } } void GazeboSystem::registerSensors(