diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 2bf409bc..b0b275c8 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -244,9 +244,10 @@ bool IgnitionSystem::initSim( RCLCPP_INFO_STREAM( this->nh_->get_logger(), - "Joint '" << this->dataPtr->joints_[mimic_joint.joint_index].name << "'is mimicking joint '" << - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].name << "' with multiplier: " << - mimic_joint.multiplier); + "Joint '" << this->dataPtr->joints_[mimic_joint.joint_index].name << + "'is mimicking joint '" << + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].name << "' with multiplier: " << + mimic_joint.multiplier); this->dataPtr->mimic_joints_.push_back(mimic_joint); suffix = "_mimic"; @@ -510,7 +511,6 @@ IgnitionSystem::perform_command_mode_switch( const std::vector & stop_interfaces) { for (unsigned int j = 0; j < this->dataPtr->joints_.size(); j++) { - for (const std::string & interface_name : stop_interfaces) { // Clear joint control method bits corresponding to stop interfaces if (interface_name == (this->dataPtr->joints_[j].name + "/" + @@ -631,7 +631,6 @@ hardware_interface::return_type IgnitionSystem::write( } if (this->dataPtr->joints_[i].joint_control_method & POSITION) { - // Get error in position double error = this->dataPtr->joints_[i].joint_position_cmd - this->dataPtr->joints_[i].joint_position; @@ -660,7 +659,8 @@ hardware_interface::return_type IgnitionSystem::write( { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointForceCmd({this->dataPtr->joints_[i].joint_effort_cmd})); + ignition::gazebo::components::JointForceCmd( + {this->dataPtr->joints_[i].joint_effort_cmd})); } else { const auto jointEffortCmd = this->dataPtr->ecm->Component(