From bb54c124f647d16799ba3d39932bd9003adaf31e Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 22 Feb 2023 17:25:52 +0100 Subject: [PATCH 01/24] Introduce PID for position control. --- ign_ros2_control/src/ign_system.cpp | 137 ++++++++++++------ .../config/gripper_controller.yaml | 3 + .../gripper_mimic_joint_example.launch.py | 13 ++ 3 files changed, 112 insertions(+), 41 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 95244cba..19eac9ba 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -30,12 +30,17 @@ #include #include #include +#include + #include #include #include #include #include +// pid stuff +#include + #include @@ -67,6 +72,9 @@ struct jointData /// \brief handles to the joints from within Gazebo ignition::gazebo::Entity sim_joint; + /// \brief PID for position and velocity control + gz::math::PID pid; + /// \brief Control method defined in the URDF for each joint. ign_ros2_control::IgnitionSystemInterface::ControlMethod joint_control_method; }; @@ -276,7 +284,8 @@ bool IgnitionSystem::initSim( "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint << "' with mutiplier: " << mimic_joint.multiplier); this->dataPtr->mimic_joints_.push_back(mimic_joint); - suffix = "_mimic"; + // TODO (livanov93): add parameter if suffix is used or not + // suffix = "_mimic"; } RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); @@ -339,6 +348,37 @@ bool IgnitionSystem::initSim( if (!std::isnan(initial_position)) { this->dataPtr->joints_[j].joint_position_cmd = initial_position; } + + // init position PID + + // assuming every joint has axis + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + // PID parameters + double p_gain = 100.0; + double i_gain = 0.0; + double d_gain = 50.0; + // set integral max and min component to 10 percent of the max effort + double iMax = std::isnan(jointAxis->Data().Effort()) ? 1.0 : jointAxis->Data().Effort() * 0.1; + double iMin = std::isnan(jointAxis->Data().Effort()) ? 1.0 : -jointAxis->Data().Effort() * 0.1; + double cmdMax = std::isnan(jointAxis->Data().Effort()) ? 1000.0 : jointAxis->Data().Effort(); + double cmdMin = std::isnan(jointAxis->Data().Effort()) ? -1000.0 : -jointAxis->Data().Effort(); + double cmdOffset = 0.0; + + igndbg << "[JointController "<dataPtr->joints_[j].pid.Init(p_gain, i_gain, d_gain, iMax, iMin, cmdMax, cmdMin, cmdOffset); + } else if (joint_info.command_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); this->dataPtr->command_interfaces_.emplace_back( @@ -580,9 +620,14 @@ IgnitionSystem::perform_command_mode_switch( hardware_interface::return_type IgnitionSystem::write( const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) + const rclcpp::Duration & period) { for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + // assuming every joint has axis + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) @@ -598,25 +643,28 @@ hardware_interface::return_type IgnitionSystem::write( {this->dataPtr->joints_[i].joint_velocity_cmd}); } } else if (this->dataPtr->joints_[i].joint_control_method & POSITION) { - // Get error in position - double error; - error = (this->dataPtr->joints_[i].joint_position - - this->dataPtr->joints_[i].joint_position_cmd) * *this->dataPtr->update_rate; - // Calculate target velcity - double target_vel = -this->dataPtr->position_proportional_gain_ * error; + // calculate error with clamped position command + double error = this->dataPtr->joints_[i].joint_position - std::clamp(this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), jointAxis->Data().Upper()); - auto vel = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + // error can be maximal 10 percent of the range + error = copysign(1.0, error) * std::clamp(std::abs(error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())*0.1); - if (vel == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointVelocityCmd({target_vel})); - } else if (!vel->Data().empty()) { - vel->Data()[0] = target_vel; - } + // calculate target force/torque + double target_force = this->dataPtr->joints_[i].pid.Update(error, std::chrono::duration(period.to_chrono())); + + auto forceCmd = + this->dataPtr->ecm->Component(this->dataPtr->joints_[i].sim_joint); + + if (forceCmd == nullptr) + { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, + ignition::gazebo::components::JointForceCmd({0})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); + } } else if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) @@ -631,7 +679,7 @@ hardware_interface::return_type IgnitionSystem::write( *jointEffortCmd = ignition::gazebo::components::JointForceCmd( {this->dataPtr->joints_[i].joint_effort_cmd}); } - } else { + } /*else { // Fallback case is a velocity command of zero double target_vel = 0.0; auto vel = @@ -647,39 +695,46 @@ hardware_interface::return_type IgnitionSystem::write( } else if (!vel->Data().empty()) { vel->Data()[0] = target_vel; } - } + }*/ } // set values of all mimic joints with respect to mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { + // assuming every mimic joint has axis + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + if (mimic_interface == "position") { // Get the joint position - double position_mimicked_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; + double position_mimicked_joint = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position; - double position_mimic_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + double position_mimic_joint = this->dataPtr->joints_[mimic_joint.joint_index].joint_position; - double position_error = - position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier; + // calculate error with clamped position command + double position_error = + position_mimic_joint - std::clamp(position_mimicked_joint * mimic_joint.multiplier, jointAxis->Data().Lower(), jointAxis->Data().Upper()); - double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate); + // error can be maximal 10 percent of the range + position_error = copysign(1.0, position_error) * std::clamp(std::abs(position_error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())*0.1); - auto vel = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + // calculate target force/torque + double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid.Update(position_error, std::chrono::duration(period.to_chrono())); - if (vel == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointVelocityCmd({velocity_sp})); - } else if (!vel->Data().empty()) { - vel->Data()[0] = velocity_sp; - } - } + auto forceCmd = + this->dataPtr->ecm->Component(this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + + if (forceCmd == nullptr) + { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({0})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); + } + }/* if (mimic_interface == "velocity") { // get the velocity of mimicked joint double velocity_mimicked_joint = @@ -720,7 +775,7 @@ hardware_interface::return_type IgnitionSystem::write( {mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort}); } - } + }*/ } } diff --git a/ign_ros2_control_demos/config/gripper_controller.yaml b/ign_ros2_control_demos/config/gripper_controller.yaml index d72296e1..0f5ba64d 100644 --- a/ign_ros2_control_demos/config/gripper_controller.yaml +++ b/ign_ros2_control_demos/config/gripper_controller.yaml @@ -5,6 +5,9 @@ controller_manager: gripper_controller: type: forward_command_controller/ForwardCommandController + gripper_action_controller: + type: position_controllers/GripperActionController + joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster diff --git a/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py b/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py index bf12d3d8..84385598 100644 --- a/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py +++ b/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py @@ -75,6 +75,13 @@ def generate_launch_description(): output='screen' ) + # load action controller but don't start it + load_gripper_action_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'configured', + 'gripper_action_controller'], + output='screen' + ) + return LaunchDescription([ # Launch gazebo environment IncludeLaunchDescription( @@ -94,6 +101,12 @@ def generate_launch_description(): on_exit=[load_gripper_controller], ) ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_gripper_controller, + on_exit=[load_gripper_action_controller], + ) + ), node_robot_state_publisher, ignition_spawn_entity, # Launch Arguments From 0104d7a407ddd4481a63ea89106bf788b313e5fd Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Feb 2023 00:24:33 +0100 Subject: [PATCH 02/24] Add generate parameter library. --- ign_ros2_control/CMakeLists.txt | 19 +- .../include/ign_ros2_control/ign_system.hpp | 10 + ign_ros2_control/package.xml | 1 + .../src/ign_ros2_control_parameters.yaml | 51 +++++ ign_ros2_control/src/ign_system.cpp | 182 +++++++++++------- 5 files changed, 193 insertions(+), 70 deletions(-) create mode 100644 ign_ros2_control/src/ign_ros2_control_parameters.yaml diff --git a/ign_ros2_control/CMakeLists.txt b/ign_ros2_control/CMakeLists.txt index f11c290f..f3775b08 100644 --- a/ign_ros2_control/CMakeLists.txt +++ b/ign_ros2_control/CMakeLists.txt @@ -23,6 +23,11 @@ find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(yaml_cpp_vendor REQUIRED) +find_package(generate_parameter_library REQUIRED) + +generate_parameter_library(ign_ros2_control_parameters + src/ign_ros2_control_parameters.yaml +) if("$ENV{IGNITION_VERSION}" STREQUAL "citadel") find_package(ignition-gazebo3 REQUIRED) @@ -58,6 +63,7 @@ target_link_libraries(${PROJECT_NAME}-system ignition-gazebo${IGN_GAZEBO_VER}::core ignition-plugin${IGN_PLUGIN_VER}::register ) +target_link_libraries(${PROJECT_NAME}-system ign_ros2_control_parameters) ament_target_dependencies(${PROJECT_NAME}-system ament_index_cpp controller_manager @@ -73,6 +79,8 @@ ament_target_dependencies(${PROJECT_NAME}-system add_library(ign_hardware_plugins SHARED src/ign_system.cpp ) +target_include_directories(ign_hardware_plugins PRIVATE include) +target_link_libraries(ign_hardware_plugins ign_ros2_control_parameters) ament_target_dependencies(ign_hardware_plugins rclcpp_lifecycle hardware_interface @@ -82,9 +90,14 @@ target_link_libraries(ign_hardware_plugins ignition-gazebo${IGN_GAZEBO_VER}::core ) +install(DIRECTORY include/ + DESTINATION include +) + ## Install install(TARGETS - ign_hardware_plugins + ign_hardware_plugins ign_ros2_control_parameters + EXPORT export_ign_ros2_control ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -98,7 +111,9 @@ endif() ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME} ign_hardware_plugins) - +ament_export_targets( + export_ign_ros2_control HAS_LIBRARY_TARGET +) # Install directories install(TARGETS ${PROJECT_NAME}-system DESTINATION lib diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp index 08dfd847..22ed5ab1 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp +++ b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp @@ -25,6 +25,8 @@ #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ign_ros2_control_parameters.hpp" + namespace ign_ros2_control { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -88,6 +90,14 @@ class IgnitionSystem : public IgnitionSystemInterface /// \brief Private data class std::unique_ptr dataPtr; + + // Parameters from ROS for ign_ros2_control + std::shared_ptr param_listener_; + Params params_; + + rclcpp::Node::SharedPtr node_; + + std::thread spin_thread_; }; } // namespace ign_ros2_control diff --git a/ign_ros2_control/package.xml b/ign_ros2_control/package.xml index 90195285..7d31d748 100644 --- a/ign_ros2_control/package.xml +++ b/ign_ros2_control/package.xml @@ -22,6 +22,7 @@ rclcpp_lifecycle hardware_interface controller_manager + generate_parameter_library ament_lint_auto ament_lint_common diff --git a/ign_ros2_control/src/ign_ros2_control_parameters.yaml b/ign_ros2_control/src/ign_ros2_control_parameters.yaml new file mode 100644 index 00000000..b1684bad --- /dev/null +++ b/ign_ros2_control/src/ign_ros2_control_parameters.yaml @@ -0,0 +1,51 @@ +ign_ros2_control: + joints: { + type: string_array, + default_value: [], + description: "Names of joints used by the controller", + validation: { + unique<>: null, + } + } + gains: + __map_joints: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + iMax: { + type: double, + default_value: 0.0, + description: "Integral positive clamp." + } + iMin: { + type: double, + default_value: 0.0, + description: "Integral negative clamp." + } + cmdMax: { + type: double, + default_value: 0.0, + description: "Maximum value for the PID command." + } + cmdMin: { + type: double, + default_value: 0.0, + description: "Maximum value for the PID command." + } + cmdOffset: { + type: double, + default_value: 0.0, + description: "Offset value for the command which is added to the result of the PID controller." + } diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 19eac9ba..a0da47e8 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -352,32 +352,39 @@ bool IgnitionSystem::initSim( // init position PID // assuming every joint has axis - const auto * jointAxis = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); // PID parameters - double p_gain = 100.0; - double i_gain = 0.0; - double d_gain = 50.0; + double p_gain = 100.0; + double i_gain = 0.0; + double d_gain = 50.0; // set integral max and min component to 10 percent of the max effort - double iMax = std::isnan(jointAxis->Data().Effort()) ? 1.0 : jointAxis->Data().Effort() * 0.1; - double iMin = std::isnan(jointAxis->Data().Effort()) ? 1.0 : -jointAxis->Data().Effort() * 0.1; - double cmdMax = std::isnan(jointAxis->Data().Effort()) ? 1000.0 : jointAxis->Data().Effort(); - double cmdMin = std::isnan(jointAxis->Data().Effort()) ? -1000.0 : -jointAxis->Data().Effort(); + double iMax = std::isnan(jointAxis->Data().Effort()) ? 1.0 : jointAxis->Data().Effort() * + 0.1; + double iMin = std::isnan(jointAxis->Data().Effort()) ? 1.0 : -jointAxis->Data().Effort() * + 0.1; + double cmdMax = + std::isnan(jointAxis->Data().Effort()) ? 1000.0 : jointAxis->Data().Effort(); + double cmdMin = + std::isnan(jointAxis->Data().Effort()) ? -1000.0 : -jointAxis->Data().Effort(); double cmdOffset = 0.0; - igndbg << "[JointController "<dataPtr->joints_[j].pid.Init(p_gain, i_gain, d_gain, iMax, iMin, cmdMax, cmdMin, cmdOffset); + this->dataPtr->joints_[j].pid.Init( + p_gain, i_gain, d_gain, iMax, iMin, cmdMax, cmdMin, + cmdOffset); } else if (joint_info.command_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); @@ -411,6 +418,18 @@ bool IgnitionSystem::initSim( registerSensors(hardware_info); + this->node_ = rclcpp::Node::make_shared( + hardware_info.name, + rclcpp::NodeOptions().allow_undeclared_parameters( + true).automatically_declare_parameters_from_overrides(true)); + + auto spin = [this]() + { + rclcpp::spin(node_); + }; + + spin_thread_ = std::thread(spin); + return true; } @@ -487,6 +506,14 @@ CallbackReturn IgnitionSystem::on_init(const hardware_interface::HardwareInfo & system_info) { RCLCPP_WARN(this->nh_->get_logger(), "On init..."); + try { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(this->node_); + params_ = param_listener_->get_params(); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -623,10 +650,10 @@ hardware_interface::return_type IgnitionSystem::write( const rclcpp::Duration & period) { for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - // assuming every joint has axis - const auto * jointAxis = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + // assuming every joint has axis + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { if (!this->dataPtr->ecm->Component( @@ -644,27 +671,37 @@ hardware_interface::return_type IgnitionSystem::write( } } else if (this->dataPtr->joints_[i].joint_control_method & POSITION) { - // calculate error with clamped position command - double error = this->dataPtr->joints_[i].joint_position - std::clamp(this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), jointAxis->Data().Upper()); - - // error can be maximal 10 percent of the range - error = copysign(1.0, error) * std::clamp(std::abs(error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())*0.1); - - // calculate target force/torque - double target_force = this->dataPtr->joints_[i].pid.Update(error, std::chrono::duration(period.to_chrono())); - - auto forceCmd = - this->dataPtr->ecm->Component(this->dataPtr->joints_[i].sim_joint); + // calculate error with clamped position command + double error = this->dataPtr->joints_[i].joint_position - std::clamp( + this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), + jointAxis->Data().Upper()); + + // error can be maximal 10 percent of the range + error = + copysign( + 1.0, + error) * + std::clamp( + std::abs(error), 0.0, + std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower()) * 0.1); + + // calculate target force/torque + double target_force = this->dataPtr->joints_[i].pid.Update( + error, std::chrono::duration( + period.to_chrono())); + + auto forceCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); - if (forceCmd == nullptr) - { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointForceCmd({0})); - } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); - } + if (forceCmd == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, + ignition::gazebo::components::JointForceCmd({0})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); + } } else if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) @@ -701,39 +738,48 @@ hardware_interface::return_type IgnitionSystem::write( // set values of all mimic joints with respect to mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { - // assuming every mimic joint has axis - const auto * jointAxis = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + // assuming every mimic joint has axis + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); if (mimic_interface == "position") { // Get the joint position - double position_mimicked_joint = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position; + double position_mimicked_joint = + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position; - double position_mimic_joint = this->dataPtr->joints_[mimic_joint.joint_index].joint_position; + double position_mimic_joint = + this->dataPtr->joints_[mimic_joint.joint_index].joint_position; - // calculate error with clamped position command - double position_error = - position_mimic_joint - std::clamp(position_mimicked_joint * mimic_joint.multiplier, jointAxis->Data().Lower(), jointAxis->Data().Upper()); + // calculate error with clamped position command + double position_error = + position_mimic_joint - std::clamp( + position_mimicked_joint * mimic_joint.multiplier, + jointAxis->Data().Lower(), jointAxis->Data().Upper()); - // error can be maximal 10 percent of the range - position_error = copysign(1.0, position_error) * std::clamp(std::abs(position_error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())*0.1); + // error can be maximal 10 percent of the range + position_error = copysign(1.0, position_error) * std::clamp( + std::abs( + position_error), 0.0, std::abs( + jointAxis->Data().Upper() - jointAxis->Data().Lower()) * 0.1); - // calculate target force/torque - double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid.Update(position_error, std::chrono::duration(period.to_chrono())); + // calculate target force/torque + double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid.Update( + position_error, + std::chrono::duration(period.to_chrono())); - auto forceCmd = - this->dataPtr->ecm->Component(this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + auto forceCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - if (forceCmd == nullptr) - { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointForceCmd({0})); - } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); - } + if (forceCmd == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({0})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); + } }/* if (mimic_interface == "velocity") { // get the velocity of mimicked joint From c018f7c92ebc4dca812d21b8eb38880600941cb9 Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Feb 2023 11:06:27 +0100 Subject: [PATCH 03/24] Refresh params every loop step. --- .../include/ign_ros2_control/ign_system.hpp | 3 +- ign_ros2_control/src/ign_system.cpp | 146 +++++++++++------- 2 files changed, 91 insertions(+), 58 deletions(-) diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp index 22ed5ab1..2d830082 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp +++ b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp @@ -95,8 +95,7 @@ class IgnitionSystem : public IgnitionSystemInterface std::shared_ptr param_listener_; Params params_; - rclcpp::Node::SharedPtr node_; - + rclcpp::Node::SharedPtr param_node_; std::thread spin_thread_; }; diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index a0da47e8..ec8a8eab 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -199,10 +199,20 @@ bool IgnitionSystem::initSim( return false; } + // parameters needed for joint control + std::vector joint_names; + this->param_node_ = rclcpp::Node::make_shared( + hardware_info.name, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + std::vector param_vec; + for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) { auto & joint_info = hardware_info.joints[j]; std::string joint_name = this->dataPtr->joints_[j].name = joint_info.name; + // add as one of node parameters + joint_names.push_back(joint_name); + ignition::gazebo::Entity simjoint = enableJoints[joint_name]; this->dataPtr->joints_[j].sim_joint = simjoint; @@ -230,6 +240,39 @@ bool IgnitionSystem::initSim( _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointForce()); } + // init PID + // assuming every joint has axis + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[j].sim_joint); + // PID parameters + double p_gain = 100.0; + double i_gain = 1.0; + double d_gain = 50.0; + // set integral max and min component to 10 percent of the max effort + double iMax = std::isnan(jointAxis->Data().Effort()) ? 500.0 : jointAxis->Data().Effort() * + 0.5; + double iMin = std::isnan(jointAxis->Data().Effort()) ? 500.0 : -jointAxis->Data().Effort() * + 0.5; + double cmdMax = + std::isnan(jointAxis->Data().Effort()) ? 1000.0 : jointAxis->Data().Effort(); + double cmdMin = + std::isnan(jointAxis->Data().Effort()) ? -1000.0 : -jointAxis->Data().Effort(); + double cmdOffset = 0.0; + + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".p", p_gain}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".i", i_gain}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".d", d_gain}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMax", iMax}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMin", iMin}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMax", cmdMax}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMin", cmdMin}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdOffset", cmdOffset}); + + this->dataPtr->joints_[j].pid.Init( + p_gain, i_gain, d_gain, iMax, iMin, cmdMax, cmdMin, + cmdOffset); + // Accept this joint and continue configuration RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name); @@ -348,44 +391,6 @@ bool IgnitionSystem::initSim( if (!std::isnan(initial_position)) { this->dataPtr->joints_[j].joint_position_cmd = initial_position; } - - // init position PID - - // assuming every joint has axis - const auto * jointAxis = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); - // PID parameters - double p_gain = 100.0; - double i_gain = 0.0; - double d_gain = 50.0; - // set integral max and min component to 10 percent of the max effort - double iMax = std::isnan(jointAxis->Data().Effort()) ? 1.0 : jointAxis->Data().Effort() * - 0.1; - double iMin = std::isnan(jointAxis->Data().Effort()) ? 1.0 : -jointAxis->Data().Effort() * - 0.1; - double cmdMax = - std::isnan(jointAxis->Data().Effort()) ? 1000.0 : jointAxis->Data().Effort(); - double cmdMin = - std::isnan(jointAxis->Data().Effort()) ? -1000.0 : -jointAxis->Data().Effort(); - double cmdOffset = 0.0; - - igndbg << "[JointController " << joint_name << - "] Position based PID with Force/Torque output:" << std::endl; - igndbg << "p_gain: [" << p_gain << "]" << std::endl; - igndbg << "i_gain: [" << i_gain << "]" << std::endl; - igndbg << "d_gain: [" << d_gain << "]" << std::endl; - igndbg << "i_max: [" << iMax << "]" << std::endl; - igndbg << "i_min: [" << iMin << "]" << std::endl; - igndbg << "cmd_max: [" << cmdMax << "]" << std::endl; - igndbg << "cmd_min: [" << cmdMin << "]" << std::endl; - igndbg << "cmd_offset: [" << cmdOffset << "]" << std::endl; - - - this->dataPtr->joints_[j].pid.Init( - p_gain, i_gain, d_gain, iMax, iMin, cmdMax, cmdMin, - cmdOffset); - } else if (joint_info.command_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); this->dataPtr->command_interfaces_.emplace_back( @@ -416,19 +421,32 @@ bool IgnitionSystem::initSim( } } - registerSensors(hardware_info); + rclcpp::Parameter joint_names_parameter("joints", joint_names); + if (!this->param_node_->has_parameter("joints")) { + this->param_node_->set_parameter(joint_names_parameter); + } + for (const auto & p : param_vec) { + if (!this->param_node_->has_parameter(p.get_name())) { + this->param_node_->set_parameter(p); + } + } - this->node_ = rclcpp::Node::make_shared( - hardware_info.name, - rclcpp::NodeOptions().allow_undeclared_parameters( - true).automatically_declare_parameters_from_overrides(true)); + spin_thread_ = std::thread([this]() {rclcpp::spin(this->param_node_);}); - auto spin = [this]() - { - rclcpp::spin(node_); - }; + try { + // Create the parameter listener and get the parameters + param_listener_ = std::make_shared(this->param_node_); + params_ = param_listener_->get_params(); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return false; + } + + // update the params + param_vec.push_back(joint_names_parameter); + param_listener_->update(param_vec); - spin_thread_ = std::thread(spin); + registerSensors(hardware_info); return true; } @@ -506,14 +524,7 @@ CallbackReturn IgnitionSystem::on_init(const hardware_interface::HardwareInfo & system_info) { RCLCPP_WARN(this->nh_->get_logger(), "On init..."); - try { - // Create the parameter listener and get the parameters - param_listener_ = std::make_shared(this->node_); - params_ = param_listener_->get_params(); - } catch (const std::exception & e) { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } + if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -649,12 +660,35 @@ hardware_interface::return_type IgnitionSystem::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + // refresh params + param_listener_->refresh_dynamic_parameters(); + params_ = param_listener_->get_params(); + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { // assuming every joint has axis const auto * jointAxis = this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); + // update PIDs + this->dataPtr->joints_[i].pid.SetPGain( + params_.gains.joints_map[this->dataPtr->joints_[i].name].p); + this->dataPtr->joints_[i].pid.SetIGain( + params_.gains.joints_map[this->dataPtr->joints_[i].name].i); + this->dataPtr->joints_[i].pid.SetDGain( + params_.gains.joints_map[this->dataPtr->joints_[i].name].d); + this->dataPtr->joints_[i].pid.SetIMax( + params_.gains.joints_map[this->dataPtr->joints_[i].name].iMax); + this->dataPtr->joints_[i].pid.SetIMin( + params_.gains.joints_map[this->dataPtr->joints_[i].name].iMin); + this->dataPtr->joints_[i].pid.SetCmdMax( + params_.gains.joints_map[this->dataPtr->joints_[i].name].cmdMax); + this->dataPtr->joints_[i].pid.SetCmdMin( + params_.gains.joints_map[this->dataPtr->joints_[i].name].cmdMin); + this->dataPtr->joints_[i].pid.SetCmdOffset( + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].cmdOffset); + if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) From cb75106d01cae3afc9b9ee6e74dd6507348191fc Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Feb 2023 12:46:58 +0100 Subject: [PATCH 04/24] Reorganize calcs. --- ign_ros2_control/src/ign_system.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index ec8a8eab..218af0be 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -246,10 +246,10 @@ bool IgnitionSystem::initSim( this->dataPtr->ecm->Component( this->dataPtr->joints_[j].sim_joint); // PID parameters - double p_gain = 100.0; - double i_gain = 1.0; - double d_gain = 50.0; - // set integral max and min component to 10 percent of the max effort + double p_gain = 1000.0; + double i_gain = 0.0; + double d_gain = 0.0; + // set integral max and min component to 50 percent of the max effort double iMax = std::isnan(jointAxis->Data().Effort()) ? 500.0 : jointAxis->Data().Effort() * 0.5; double iMin = std::isnan(jointAxis->Data().Effort()) ? 500.0 : -jointAxis->Data().Effort() * @@ -710,14 +710,13 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), jointAxis->Data().Upper()); - // error can be maximal 10 percent of the range error = copysign( 1.0, error) * std::clamp( std::abs(error), 0.0, - std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower()) * 0.1); + std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); // calculate target force/torque double target_force = this->dataPtr->joints_[i].pid.Update( @@ -791,11 +790,10 @@ hardware_interface::return_type IgnitionSystem::write( position_mimicked_joint * mimic_joint.multiplier, jointAxis->Data().Lower(), jointAxis->Data().Upper()); - // error can be maximal 10 percent of the range position_error = copysign(1.0, position_error) * std::clamp( std::abs( position_error), 0.0, std::abs( - jointAxis->Data().Upper() - jointAxis->Data().Lower()) * 0.1); + jointAxis->Data().Upper() - jointAxis->Data().Lower())); // calculate target force/torque double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid.Update( From 6d5eda3d18205a2f893e3d6c6d1a5f9fd1579bdf Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Feb 2023 17:31:52 +0100 Subject: [PATCH 05/24] Add pid params via hardware parameters. --- ign_ros2_control/src/ign_system.cpp | 78 ++++++++++++++++++----------- 1 file changed, 50 insertions(+), 28 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 218af0be..9a1e8f6c 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -43,6 +43,7 @@ #include +#include #include @@ -246,19 +247,47 @@ bool IgnitionSystem::initSim( this->dataPtr->ecm->Component( this->dataPtr->joints_[j].sim_joint); // PID parameters - double p_gain = 1000.0; - double i_gain = 0.0; - double d_gain = 0.0; + double p_gain = + (hardware_info.joints[j].parameters.find("p") == + hardware_info.joints[j].parameters.end() ) ? 100.0 : stod( + hardware_info.joints[j].parameters.at( + "p")); + double i_gain = + (hardware_info.joints[j].parameters.find("i") == + hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( + hardware_info.joints[j].parameters.at( + "i")); + double d_gain = + (hardware_info.joints[j].parameters.find("d") == + hardware_info.joints[j].parameters.end() ) ? 5.0 : stod( + hardware_info.joints[j].parameters.at( + "d")); // set integral max and min component to 50 percent of the max effort - double iMax = std::isnan(jointAxis->Data().Effort()) ? 500.0 : jointAxis->Data().Effort() * - 0.5; - double iMin = std::isnan(jointAxis->Data().Effort()) ? 500.0 : -jointAxis->Data().Effort() * - 0.5; + double iMax = + (hardware_info.joints[j].parameters.find("iMax") == + hardware_info.joints[j].parameters.end() ) ? 500.0 : stod( + hardware_info.joints[j].parameters.at( + "iMax")); + double iMin = + (hardware_info.joints[j].parameters.find("iMin") == + hardware_info.joints[j].parameters.end() ) ? -500 : stod( + hardware_info.joints[j].parameters.at( + "iMin")); double cmdMax = - std::isnan(jointAxis->Data().Effort()) ? 1000.0 : jointAxis->Data().Effort(); + (hardware_info.joints[j].parameters.find("cmdMax") == + hardware_info.joints[j].parameters.end() ) ? 1000.0 : stod( + hardware_info.joints[j].parameters.at( + "cmdMax")); double cmdMin = - std::isnan(jointAxis->Data().Effort()) ? -1000.0 : -jointAxis->Data().Effort(); - double cmdOffset = 0.0; + (hardware_info.joints[j].parameters.find("cmdMin") == + hardware_info.joints[j].parameters.end() ) ? -1000.0 : stod( + hardware_info.joints[j].parameters.at( + "cmdMin")); + double cmdOffset = + (hardware_info.joints[j].parameters.find("cmdOffset") == + hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( + hardware_info.joints[j].parameters.at( + "cmdOffset")); param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".p", p_gain}); param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".i", i_gain}); @@ -665,6 +694,7 @@ hardware_interface::return_type IgnitionSystem::write( params_ = param_listener_->get_params(); for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + // assuming every joint has axis const auto * jointAxis = this->dataPtr->ecm->Component( @@ -709,7 +739,6 @@ hardware_interface::return_type IgnitionSystem::write( double error = this->dataPtr->joints_[i].joint_position - std::clamp( this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), jointAxis->Data().Upper()); - error = copysign( 1.0, @@ -723,6 +752,10 @@ hardware_interface::return_type IgnitionSystem::write( error, std::chrono::duration( period.to_chrono())); + igndbg << "target_force ( joint [" << + this->dataPtr->joints_[i].name << "] ) = " << target_force << + "\n"; + auto forceCmd = this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); @@ -749,25 +782,10 @@ hardware_interface::return_type IgnitionSystem::write( *jointEffortCmd = ignition::gazebo::components::JointForceCmd( {this->dataPtr->joints_[i].joint_effort_cmd}); } - } /*else { - // Fallback case is a velocity command of zero - double target_vel = 0.0; - auto vel = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); - - if (vel == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointVelocityCmd({target_vel})); - } else if (!vel->Data().empty()) { - vel->Data()[0] = target_vel; - } else if (!vel->Data().empty()) { - vel->Data()[0] = target_vel; - } - }*/ + } } + // set values of all mimic joints with respect to mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { @@ -800,6 +818,10 @@ hardware_interface::return_type IgnitionSystem::write( position_error, std::chrono::duration(period.to_chrono())); + igndbg << "target_force ( mimic joint [" << + this->dataPtr->joints_[mimic_joint.joint_index].name << "] ) = " << target_force << + "\n"; + auto forceCmd = this->dataPtr->ecm->Component( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); From 87496ff96b1cb707465d459c84d25d50a794aa01 Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 2 Mar 2023 09:44:48 +0100 Subject: [PATCH 06/24] Add pid sim params into demos urdfs. --- .../urdf/test_cart_position.xacro.urdf | 8 ++++++++ .../urdf/test_cart_velocity.xacro.urdf | 8 ++++++++ .../urdf/test_diff_drive.xacro.urdf | 16 ++++++++++++++++ .../urdf/test_gripper_mimic_joint.xacro.urdf | 16 ++++++++++++++++ .../urdf/test_tricycle_drive.xacro.urdf | 16 ++++++++++++++++ 5 files changed, 64 insertions(+) diff --git a/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf index eeff47ec..d6ff323e 100644 --- a/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -57,6 +57,14 @@ + 100.0 + 0.0 + 25.0 + 50 + -50 + 100 + -100 + 0.0 diff --git a/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index 59a960a1..e63011af 100644 --- a/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -123,6 +123,14 @@ + 100.0 + 0.0 + 25.0 + 50 + -50 + 100 + -100 + 0.0 diff --git a/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf b/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf index e608ed9e..c2d2b2c1 100644 --- a/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf @@ -143,6 +143,14 @@ + 100.0 + 0.0 + 25.0 + 50 + -50 + 100 + -100 + 0.0 @@ -151,6 +159,14 @@ + 100.0 + 0.0 + 25.0 + 50 + -50 + 100 + -100 + 0.0 diff --git a/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf b/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf index 8e0074f4..195cf5f0 100644 --- a/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf @@ -77,6 +77,14 @@ + 100.0 + 0.0 + 25.0 + 50 + -50 + 100 + -100 + 0.0 right_finger_joint @@ -85,6 +93,14 @@ + 100.0 + 1.0 + 25.0 + 50 + -50 + 100 + -100 + 0.0 diff --git a/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf b/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf index 7d58323d..a9831564 100644 --- a/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf @@ -157,11 +157,27 @@ + 100.0 + 0.0 + 25.0 + 50 + -50 + 100 + -100 + 0.0 + 100.0 + 0.0 + 25.0 + 50 + -50 + 100 + -100 + 0.0 From c11b195d8e3eb51961e540bf6ded304f406b3c8c Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 2 Mar 2023 09:45:25 +0100 Subject: [PATCH 07/24] Testing clamping. --- ign_ros2_control/src/ign_system.cpp | 44 ++++++++++++++--------------- 1 file changed, 21 insertions(+), 23 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 9a1e8f6c..377f8036 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -31,6 +31,8 @@ #include #include #include +#include +#include #include #include @@ -46,6 +48,7 @@ #include #include +#include struct jointData { @@ -241,11 +244,6 @@ bool IgnitionSystem::initSim( _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointForce()); } - // init PID - // assuming every joint has axis - const auto * jointAxis = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[j].sim_joint); // PID parameters double p_gain = (hardware_info.joints[j].parameters.find("p") == @@ -752,10 +750,6 @@ hardware_interface::return_type IgnitionSystem::write( error, std::chrono::duration( period.to_chrono())); - igndbg << "target_force ( joint [" << - this->dataPtr->joints_[i].name << "] ) = " << target_force << - "\n"; - auto forceCmd = this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); @@ -794,6 +788,10 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->ecm->Component( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + const auto * jointAxisMimicked = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint); + if (mimic_interface == "position") { // Get the joint position double position_mimicked_joint = @@ -806,33 +804,33 @@ hardware_interface::return_type IgnitionSystem::write( double position_error = position_mimic_joint - std::clamp( position_mimicked_joint * mimic_joint.multiplier, - jointAxis->Data().Lower(), jointAxis->Data().Upper()); + jointAxisMimicked->Data().Lower(), jointAxisMimicked->Data().Upper()); position_error = copysign(1.0, position_error) * std::clamp( std::abs( - position_error), 0.0, std::abs( - jointAxis->Data().Upper() - jointAxis->Data().Lower())); + position_error), 0.0, std::abs(jointAxisMimicked->Data().Upper() - jointAxisMimicked->Data().Lower())); // calculate target force/torque - double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid.Update( + double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid.Update( position_error, std::chrono::duration(period.to_chrono())); - igndbg << "target_force ( mimic joint [" << - this->dataPtr->joints_[mimic_joint.joint_index].name << "] ) = " << target_force << - "\n"; - - auto forceCmd = - this->dataPtr->ecm->Component( + auto velCmd = + this->dataPtr->ecm->Component( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - if (forceCmd == nullptr) { +// igndbg<<"position_error ["<dataPtr->joints_[mimic_joint.joint_index].name<<"] = "<dataPtr->joints_[mimic_joint.joint_index].name<<"] = "<dataPtr->ecm->CreateComponent( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointForceCmd({0})); + ignition::gazebo::components::JointVelocityCmd({0})); + } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); + *velCmd = ignition::gazebo::components::JointVelocityCmd( + {target_vel}); } }/* if (mimic_interface == "velocity") { From c3bd60190ed561ef333e5f4eacb4bfb750de6553 Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 3 Mar 2023 17:37:15 +0100 Subject: [PATCH 08/24] Change pid params. Cascade control. Comment mimic stuff for now. --- .../include/ign_ros2_control/ign_system.hpp | 4 + .../src/ign_ros2_control_parameters.yaml | 56 +++- ign_ros2_control/src/ign_system.cpp | 293 ++++++++++++------ .../urdf/test_gripper_mimic_joint.xacro.urdf | 48 ++- 4 files changed, 289 insertions(+), 112 deletions(-) diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp index 2d830082..85c2f353 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp +++ b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp @@ -27,6 +27,8 @@ #include "ign_ros2_control_parameters.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" + namespace ign_ros2_control { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -97,6 +99,8 @@ class IgnitionSystem : public IgnitionSystemInterface rclcpp::Node::SharedPtr param_node_; std::thread spin_thread_; + std::atomic stop_spin_ = false; + rclcpp::executors::SingleThreadedExecutor::SharedPtr exec_; }; } // namespace ign_ros2_control diff --git a/ign_ros2_control/src/ign_ros2_control_parameters.yaml b/ign_ros2_control/src/ign_ros2_control_parameters.yaml index b1684bad..f5eb2c9e 100644 --- a/ign_ros2_control/src/ign_ros2_control_parameters.yaml +++ b/ign_ros2_control/src/ign_ros2_control_parameters.yaml @@ -9,42 +9,82 @@ ign_ros2_control: } gains: __map_joints: - p: { + p_pos: { type: double, default_value: 0.0, description: "Proportional gain for PID" } - i: { + i_pos: { type: double, default_value: 0.0, description: "Integral gain for PID" } - d: { + d_pos: { type: double, default_value: 0.0, description: "Derivative gain for PID" } - iMax: { + iMax_pos: { type: double, default_value: 0.0, description: "Integral positive clamp." } - iMin: { + iMin_pos: { type: double, default_value: 0.0, description: "Integral negative clamp." } - cmdMax: { + cmdMax_pos: { type: double, default_value: 0.0, description: "Maximum value for the PID command." } - cmdMin: { + cmdMin_pos: { type: double, default_value: 0.0, description: "Maximum value for the PID command." } - cmdOffset: { + cmdOffset_pos: { + type: double, + default_value: 0.0, + description: "Offset value for the command which is added to the result of the PID controller." + } + p_vel: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i_vel: { + type: double, + default_value: 0.0, + description: "Integral gain for PID" + } + d_vel: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + iMax_vel: { + type: double, + default_value: 0.0, + description: "Integral positive clamp." + } + iMin_vel: { + type: double, + default_value: 0.0, + description: "Integral negative clamp." + } + cmdMax_vel: { + type: double, + default_value: 0.0, + description: "Maximum value for the PID command." + } + cmdMin_vel: { + type: double, + default_value: 0.0, + description: "Maximum value for the PID command." + } + cmdOffset_vel: { type: double, default_value: 0.0, description: "Offset value for the command which is added to the result of the PID controller." diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 377f8036..ffb2e63b 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -40,7 +40,7 @@ #include #include -// pid stuff +// pid_pos stuff #include #include @@ -76,8 +76,11 @@ struct jointData /// \brief handles to the joints from within Gazebo ignition::gazebo::Entity sim_joint; - /// \brief PID for position and velocity control - gz::math::PID pid; + /// \brief PID for position control + gz::math::PID pid_pos; + + /// \brief PID for velocity control + gz::math::PID pid_vel; /// \brief Control method defined in the URDF for each joint. ign_ros2_control::IgnitionSystemInterface::ControlMethod joint_control_method; @@ -245,60 +248,115 @@ bool IgnitionSystem::initSim( } // PID parameters - double p_gain = - (hardware_info.joints[j].parameters.find("p") == + double p_gain_pos = + (hardware_info.joints[j].parameters.find("p_pos") == + hardware_info.joints[j].parameters.end() ) ? 100.0 : stod( + hardware_info.joints[j].parameters.at( + "p_pos")); + double i_gain_pos = + (hardware_info.joints[j].parameters.find("i_pos") == + hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( + hardware_info.joints[j].parameters.at( + "i_pos")); + double d_gain_pos = + (hardware_info.joints[j].parameters.find("d_pos") == + hardware_info.joints[j].parameters.end() ) ? 5.0 : stod( + hardware_info.joints[j].parameters.at( + "d_pos")); + // set integral max and min component to 50 percent of the max effort + double iMax_pos = + (hardware_info.joints[j].parameters.find("iMax_pos") == + hardware_info.joints[j].parameters.end() ) ? 500.0 : stod( + hardware_info.joints[j].parameters.at( + "iMax_pos")); + double iMin_pos = + (hardware_info.joints[j].parameters.find("iMin_pos") == + hardware_info.joints[j].parameters.end() ) ? -500 : stod( + hardware_info.joints[j].parameters.at( + "iMin_pos")); + double cmdMax_pos = + (hardware_info.joints[j].parameters.find("cmdMax_pos") == + hardware_info.joints[j].parameters.end() ) ? 1000.0 : stod( + hardware_info.joints[j].parameters.at( + "cmdMax_pos")); + double cmdMin_pos = + (hardware_info.joints[j].parameters.find("cmdMin_pos") == + hardware_info.joints[j].parameters.end() ) ? -1000.0 : stod( + hardware_info.joints[j].parameters.at( + "cmdMin_pos")); + double cmdOffset_pos = + (hardware_info.joints[j].parameters.find("cmdOffset_pos") == + hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( + hardware_info.joints[j].parameters.at( + "cmdOffset_pos")); + + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".p_pos", p_gain_pos}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".i_pos", i_gain_pos}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".d_pos", d_gain_pos}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMax_pos", iMax_pos}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMin_pos", iMin_pos}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMax_pos", cmdMax_pos}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMin_pos", cmdMin_pos}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdOffset_pos", cmdOffset_pos}); + + this->dataPtr->joints_[j].pid_pos.Init( + p_gain_pos, i_gain_pos, d_gain_pos, iMax_pos, iMin_pos, cmdMax_pos, cmdMin_pos, + cmdOffset_pos); + + double p_gain_vel = + (hardware_info.joints[j].parameters.find("p_vel") == hardware_info.joints[j].parameters.end() ) ? 100.0 : stod( hardware_info.joints[j].parameters.at( - "p")); - double i_gain = - (hardware_info.joints[j].parameters.find("i") == + "p_vel")); + double i_gain_vel = + (hardware_info.joints[j].parameters.find("i_vel") == hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( hardware_info.joints[j].parameters.at( - "i")); - double d_gain = - (hardware_info.joints[j].parameters.find("d") == + "i_vel")); + double d_gain_vel = + (hardware_info.joints[j].parameters.find("d_vel") == hardware_info.joints[j].parameters.end() ) ? 5.0 : stod( hardware_info.joints[j].parameters.at( - "d")); + "d_vel")); // set integral max and min component to 50 percent of the max effort - double iMax = - (hardware_info.joints[j].parameters.find("iMax") == + double iMax_vel = + (hardware_info.joints[j].parameters.find("iMax_vel") == hardware_info.joints[j].parameters.end() ) ? 500.0 : stod( hardware_info.joints[j].parameters.at( - "iMax")); - double iMin = - (hardware_info.joints[j].parameters.find("iMin") == + "iMax_vel")); + double iMin_vel = + (hardware_info.joints[j].parameters.find("iMin_vel") == hardware_info.joints[j].parameters.end() ) ? -500 : stod( hardware_info.joints[j].parameters.at( - "iMin")); - double cmdMax = - (hardware_info.joints[j].parameters.find("cmdMax") == + "iMin_vel")); + double cmdMax_vel = + (hardware_info.joints[j].parameters.find("cmdMax_vel") == hardware_info.joints[j].parameters.end() ) ? 1000.0 : stod( hardware_info.joints[j].parameters.at( - "cmdMax")); - double cmdMin = - (hardware_info.joints[j].parameters.find("cmdMin") == + "cmdMax_vel")); + double cmdMin_vel = + (hardware_info.joints[j].parameters.find("cmdMin_vel") == hardware_info.joints[j].parameters.end() ) ? -1000.0 : stod( hardware_info.joints[j].parameters.at( - "cmdMin")); - double cmdOffset = - (hardware_info.joints[j].parameters.find("cmdOffset") == + "cmdMin_vel")); + double cmdOffset_vel = + (hardware_info.joints[j].parameters.find("cmdOffset_vel") == hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( hardware_info.joints[j].parameters.at( - "cmdOffset")); + "cmdOffset_vel")); - param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".p", p_gain}); - param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".i", i_gain}); - param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".d", d_gain}); - param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMax", iMax}); - param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMin", iMin}); - param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMax", cmdMax}); - param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMin", cmdMin}); - param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdOffset", cmdOffset}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".p_vel", p_gain_vel}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".i_vel", i_gain_vel}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".d_vel", d_gain_vel}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMax_vel", iMax_vel}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMin_vel", iMin_vel}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMax_vel", cmdMax_vel}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMin_vel", cmdMin_vel}); + param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdOffset_vel", cmdOffset_vel}); - this->dataPtr->joints_[j].pid.Init( - p_gain, i_gain, d_gain, iMax, iMin, cmdMax, cmdMin, - cmdOffset); + this->dataPtr->joints_[j].pid_vel.Init( + p_gain_vel, i_gain_vel, d_gain_vel, iMax_vel, iMin_vel, cmdMax_vel, cmdMin_vel, + cmdOffset_vel); // Accept this joint and continue configuration RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name); @@ -458,7 +516,17 @@ bool IgnitionSystem::initSim( } } - spin_thread_ = std::thread([this]() {rclcpp::spin(this->param_node_);}); + spin_thread_ = std::thread( + [this]() { + exec_ = std::make_shared(); + exec_->add_node(this->param_node_); + + while (rclcpp::ok() && !stop_spin_) { + exec_->spin_once(); + } + exec_->remove_node(this->param_node_); + exec_.reset(); + }); try { // Create the parameter listener and get the parameters @@ -587,6 +655,8 @@ CallbackReturn IgnitionSystem::on_activate(const rclcpp_lifecycle::State & previ CallbackReturn IgnitionSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state) { + stop_spin_ = true; + spin_thread_.join(); return CallbackReturn::SUCCESS; return hardware_interface::SystemInterface::on_deactivate(previous_state); } @@ -699,23 +769,49 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->joints_[i].sim_joint); // update PIDs - this->dataPtr->joints_[i].pid.SetPGain( - params_.gains.joints_map[this->dataPtr->joints_[i].name].p); - this->dataPtr->joints_[i].pid.SetIGain( - params_.gains.joints_map[this->dataPtr->joints_[i].name].i); - this->dataPtr->joints_[i].pid.SetDGain( - params_.gains.joints_map[this->dataPtr->joints_[i].name].d); - this->dataPtr->joints_[i].pid.SetIMax( - params_.gains.joints_map[this->dataPtr->joints_[i].name].iMax); - this->dataPtr->joints_[i].pid.SetIMin( - params_.gains.joints_map[this->dataPtr->joints_[i].name].iMin); - this->dataPtr->joints_[i].pid.SetCmdMax( - params_.gains.joints_map[this->dataPtr->joints_[i].name].cmdMax); - this->dataPtr->joints_[i].pid.SetCmdMin( - params_.gains.joints_map[this->dataPtr->joints_[i].name].cmdMin); - this->dataPtr->joints_[i].pid.SetCmdOffset( + this->dataPtr->joints_[i].pid_pos.SetPGain( + params_.gains.joints_map[this->dataPtr->joints_[i].name].p_pos); + this->dataPtr->joints_[i].pid_pos.SetIGain( + params_.gains.joints_map[this->dataPtr->joints_[i].name].i_pos); + this->dataPtr->joints_[i].pid_pos.SetDGain( + params_.gains.joints_map[this->dataPtr->joints_[i].name].d_pos); + this->dataPtr->joints_[i].pid_pos.SetIMax( + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].iMax_pos); + this->dataPtr->joints_[i].pid_pos.SetIMin( + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].iMin_pos); + this->dataPtr->joints_[i].pid_pos.SetCmdMax( + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].cmdMax_pos); + this->dataPtr->joints_[i].pid_pos.SetCmdMin( params_.gains.joints_map[this->dataPtr->joints_[i]. - name].cmdOffset); + name].cmdMin_pos); + this->dataPtr->joints_[i].pid_pos.SetCmdOffset( + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].cmdOffset_pos); + + this->dataPtr->joints_[i].pid_vel.SetPGain( + params_.gains.joints_map[this->dataPtr->joints_[i].name].p_vel); + this->dataPtr->joints_[i].pid_vel.SetIGain( + params_.gains.joints_map[this->dataPtr->joints_[i].name].i_vel); + this->dataPtr->joints_[i].pid_vel.SetDGain( + params_.gains.joints_map[this->dataPtr->joints_[i].name].d_vel); + this->dataPtr->joints_[i].pid_vel.SetIMax( + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].iMax_vel); + this->dataPtr->joints_[i].pid_vel.SetIMin( + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].iMin_vel); + this->dataPtr->joints_[i].pid_vel.SetCmdMax( + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].cmdMax_vel); + this->dataPtr->joints_[i].pid_vel.SetCmdMin( + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].cmdMin_vel); + this->dataPtr->joints_[i].pid_vel.SetCmdOffset( + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].cmdOffset_vel); if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { if (!this->dataPtr->ecm->Component( @@ -734,20 +830,29 @@ hardware_interface::return_type IgnitionSystem::write( } else if (this->dataPtr->joints_[i].joint_control_method & POSITION) { // calculate error with clamped position command - double error = this->dataPtr->joints_[i].joint_position - std::clamp( + double position_error = this->dataPtr->joints_[i].joint_position - std::clamp( this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), jointAxis->Data().Upper()); - error = + position_error = copysign( 1.0, - error) * + position_error) * std::clamp( - std::abs(error), 0.0, + std::abs(position_error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); - // calculate target force/torque - double target_force = this->dataPtr->joints_[i].pid.Update( - error, std::chrono::duration( + // calculate target velocity - output of outer pid - input to inner pid + double target_vel = this->dataPtr->joints_[i].pid_pos.Update( + position_error, std::chrono::duration( + period.to_chrono())); + + double velocity_error = this->dataPtr->joints_[i].joint_velocity - std::clamp( + target_vel, -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); + + // calculate target force/torque - output of inner pid + double target_force = this->dataPtr->joints_[i].pid_vel.Update( + velocity_error, std::chrono::duration( period.to_chrono())); auto forceCmd = @@ -757,7 +862,7 @@ hardware_interface::return_type IgnitionSystem::write( if (forceCmd == nullptr) { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointForceCmd({0})); + ignition::gazebo::components::JointForceCmd({target_force})); } else { *forceCmd = ignition::gazebo::components::JointForceCmd( {target_force}); @@ -788,51 +893,63 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->ecm->Component( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - const auto * jointAxisMimicked = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint); + const auto * jointAxisMimicked = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint); if (mimic_interface == "position") { // Get the joint position + double position_mimicked_joint = - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position; + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; double position_mimic_joint = - this->dataPtr->joints_[mimic_joint.joint_index].joint_position; + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; // calculate error with clamped position command double position_error = position_mimic_joint - std::clamp( position_mimicked_joint * mimic_joint.multiplier, - jointAxisMimicked->Data().Lower(), jointAxisMimicked->Data().Upper()); + jointAxis->Data().Lower(), jointAxis->Data().Upper()); position_error = copysign(1.0, position_error) * std::clamp( std::abs( - position_error), 0.0, std::abs(jointAxisMimicked->Data().Upper() - jointAxisMimicked->Data().Lower())); + position_error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); - // calculate target force/torque - double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid.Update( - position_error, - std::chrono::duration(period.to_chrono())); + // calculate target velocity - output of outer pid - input to inner pid + double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update( + position_error, std::chrono::duration( + period.to_chrono())); - auto velCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); +// double velocity_error = this->dataPtr->joints_[mimic_joint.joint_index].joint_velocity - +// std::clamp( +// target_vel, -1.0 * jointAxis->Data().MaxVelocity(), +// jointAxis->Data().MaxVelocity()); -// igndbg<<"position_error ["<dataPtr->joints_[mimic_joint.joint_index].name<<"] = "<dataPtr->joints_[mimic_joint.joint_index].name<<"] = "<dataPtr->joints_[mimic_joint.joint_index].joint_velocity - + target_vel; + // calculate target force/torque - output of inner pid + double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( + velocity_error, std::chrono::duration( + period.to_chrono())); - if (velCmd == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointVelocityCmd({0})); + auto forceCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - } else { - *velCmd = ignition::gazebo::components::JointVelocityCmd( - {target_vel}); - } - }/* +// if (forceCmd == nullptr) { +// this->dataPtr->ecm->CreateComponent( +// this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, +// ignition::gazebo::components::JointForceCmd({target_force})); +// } else { +// *forceCmd = ignition::gazebo::components::JointForceCmd( +// {target_force}); +// } + } + /* if (mimic_interface == "velocity") { // get the velocity of mimicked joint double velocity_mimicked_joint = diff --git a/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf b/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf index 195cf5f0..1ceb1697 100644 --- a/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf @@ -77,14 +77,22 @@ - 100.0 - 0.0 - 25.0 - 50 - -50 - 100 - -100 - 0.0 + 100.0 + 0.0 + 20.0 + 5.0 + -5.0 + 10.0 + -10.0 + 0.0 + 10.0 + 1.0 + 0.0 + 50 + -50 + 100 + -100 + 0.0 right_finger_joint @@ -93,14 +101,22 @@ - 100.0 - 1.0 - 25.0 - 50 - -50 - 100 - -100 - 0.0 + 100.0 + 0.0 + 20.0 + 5.0 + -5.0 + 10.0 + -10.0 + 0.0 + 10.0 + 1.0 + 0.0 + 50 + -50 + 100 + -100 + 0.0 From 29e9943ac5144209b46aa2cece54a39ee453b2f4 Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 8 Mar 2023 12:36:00 +0100 Subject: [PATCH 09/24] Debug output. --- ign_ros2_control/src/ign_system.cpp | 53 ++++++++++++++--------------- 1 file changed, 26 insertions(+), 27 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index ffb2e63b..ad2da3f9 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -884,7 +884,6 @@ hardware_interface::return_type IgnitionSystem::write( } } - // set values of all mimic joints with respect to mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { @@ -899,55 +898,55 @@ hardware_interface::return_type IgnitionSystem::write( if (mimic_interface == "position") { // Get the joint position - - double position_mimicked_joint = + const auto * jp_mimicked = this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint); + double position_mimicked_joint = jp_mimicked->Data()[0]; - double position_mimic_joint = + const auto * jp_mimic = this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + double position_mimic_joint = jp_mimic->Data()[0]; // calculate error with clamped position command - double position_error = - position_mimic_joint - std::clamp( - position_mimicked_joint * mimic_joint.multiplier, - jointAxis->Data().Lower(), jointAxis->Data().Upper()); + double position_error = position_mimic_joint - position_mimicked_joint * + mimic_joint.multiplier; - position_error = copysign(1.0, position_error) * std::clamp( - std::abs( - position_error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); + double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate); // calculate target velocity - output of outer pid - input to inner pid double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update( position_error, std::chrono::duration( period.to_chrono())); -// double velocity_error = this->dataPtr->joints_[mimic_joint.joint_index].joint_velocity - -// std::clamp( -// target_vel, -1.0 * jointAxis->Data().MaxVelocity(), -// jointAxis->Data().MaxVelocity()); + const auto * jv_mimic = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + double velocity_mimic_joint = jv_mimic->Data()[0]; - double velocity_error = this->dataPtr->joints_[mimic_joint.joint_index].joint_velocity - - target_vel; + double velocity_error = velocity_mimic_joint - target_vel; // calculate target force/torque - output of inner pid double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( velocity_error, std::chrono::duration( period.to_chrono())); + igndbg << "[" << this->dataPtr->joints_[mimic_joint.joint_index].name << "]\t" << + position_mimicked_joint << " \t" << position_mimic_joint << " \t" << target_force << + "\n"; + auto forceCmd = this->dataPtr->ecm->Component( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); -// if (forceCmd == nullptr) { -// this->dataPtr->ecm->CreateComponent( -// this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, -// ignition::gazebo::components::JointForceCmd({target_force})); -// } else { -// *forceCmd = ignition::gazebo::components::JointForceCmd( -// {target_force}); -// } + if (forceCmd == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({target_force})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); + } } /* if (mimic_interface == "velocity") { From 25187eae1c23d938e786ded1499f1fda1c8c4154 Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 9 Mar 2023 18:10:27 +0100 Subject: [PATCH 10/24] Adapt pid params for examples. --- .../urdf/test_cart_effort.xacro.urdf | 2 +- .../urdf/test_cart_position.xacro.urdf | 12 +++------ .../urdf/test_cart_velocity.xacro.urdf | 11 ++------ .../urdf/test_diff_drive.xacro.urdf | 20 +++------------ .../urdf/test_tricycle_drive.xacro.urdf | 25 +++++++------------ 5 files changed, 19 insertions(+), 51 deletions(-) diff --git a/ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf b/ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf index 6fa0af31..8c0164a4 100644 --- a/ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf @@ -40,7 +40,7 @@ - + diff --git a/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf index d6ff323e..c621f01b 100644 --- a/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -40,7 +40,7 @@ - + @@ -57,14 +57,8 @@ - 100.0 - 0.0 - 25.0 - 50 - -50 - 100 - -100 - 0.0 + 10.0 + 100.0 diff --git a/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index e63011af..4ba9e2b8 100644 --- a/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -73,7 +73,7 @@ - + @@ -123,14 +123,7 @@ - 100.0 - 0.0 - 25.0 - 50 - -50 - 100 - -100 - 0.0 + 100.0 diff --git a/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf b/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf index c2d2b2c1..071f3ad3 100644 --- a/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf @@ -36,6 +36,7 @@ + @@ -70,6 +71,7 @@ + @@ -143,14 +145,7 @@ - 100.0 - 0.0 - 25.0 - 50 - -50 - 100 - -100 - 0.0 + 1000.0 @@ -159,14 +154,7 @@ - 100.0 - 0.0 - 25.0 - 50 - -50 - 100 - -100 - 0.0 + 1000.0 diff --git a/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf b/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf index a9831564..4bf34eee 100644 --- a/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf @@ -71,6 +71,7 @@ + @@ -99,6 +100,7 @@ + @@ -121,6 +123,8 @@ + + @@ -148,6 +152,8 @@ + + @@ -157,27 +163,14 @@ - 100.0 - 0.0 - 25.0 - 50 - -50 - 100 - -100 - 0.0 + 10.0 + 100.0 - 100.0 - 0.0 - 25.0 - 50 - -50 - 100 - -100 - 0.0 + 100.0 From 01e6f680b1dcafc429f0a888a5a8f4d2511209a4 Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 9 Mar 2023 18:10:49 +0100 Subject: [PATCH 11/24] Move all control to force - based output. --- ign_ros2_control/src/ign_system.cpp | 159 ++++++++++++++++++---------- 1 file changed, 103 insertions(+), 56 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index ad2da3f9..bdd89ce8 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -30,6 +30,8 @@ #include #include #include +#include + #include #include #include @@ -66,9 +68,11 @@ struct jointData /// \brief Current cmd joint position double joint_position_cmd; + double joint_position_to_force_cmd; /// \brief Current cmd joint velocity double joint_velocity_cmd; + double joint_velocity_to_force_cmd; /// \brief Current cmd joint effort double joint_effort_cmd; @@ -247,10 +251,21 @@ bool IgnitionSystem::initSim( _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointForce()); } + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[j].sim_joint); + + double upper = jointAxis->Data().Upper(); + double lower = jointAxis->Data().Lower(); + double max_velocity = jointAxis->Data().MaxVelocity(); + double max_effort = jointAxis->Data().Effort(); + + double dummy_guess_p_pos = 10 * max_velocity / abs(upper - lower); + // PID parameters double p_gain_pos = (hardware_info.joints[j].parameters.find("p_pos") == - hardware_info.joints[j].parameters.end() ) ? 100.0 : stod( + hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos : stod( hardware_info.joints[j].parameters.at( "p_pos")); double i_gain_pos = @@ -260,28 +275,28 @@ bool IgnitionSystem::initSim( "i_pos")); double d_gain_pos = (hardware_info.joints[j].parameters.find("d_pos") == - hardware_info.joints[j].parameters.end() ) ? 5.0 : stod( + hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos / 100.0 : stod( hardware_info.joints[j].parameters.at( "d_pos")); // set integral max and min component to 50 percent of the max effort double iMax_pos = (hardware_info.joints[j].parameters.find("iMax_pos") == - hardware_info.joints[j].parameters.end() ) ? 500.0 : stod( + hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( hardware_info.joints[j].parameters.at( "iMax_pos")); double iMin_pos = (hardware_info.joints[j].parameters.find("iMin_pos") == - hardware_info.joints[j].parameters.end() ) ? -500 : stod( + hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( hardware_info.joints[j].parameters.at( "iMin_pos")); double cmdMax_pos = (hardware_info.joints[j].parameters.find("cmdMax_pos") == - hardware_info.joints[j].parameters.end() ) ? 1000.0 : stod( + hardware_info.joints[j].parameters.end() ) ? max_velocity : stod( hardware_info.joints[j].parameters.at( "cmdMax_pos")); double cmdMin_pos = (hardware_info.joints[j].parameters.find("cmdMin_pos") == - hardware_info.joints[j].parameters.end() ) ? -1000.0 : stod( + hardware_info.joints[j].parameters.end() ) ? -1.0 * max_velocity : stod( hardware_info.joints[j].parameters.at( "cmdMin_pos")); double cmdOffset_pos = @@ -305,38 +320,38 @@ bool IgnitionSystem::initSim( double p_gain_vel = (hardware_info.joints[j].parameters.find("p_vel") == - hardware_info.joints[j].parameters.end() ) ? 100.0 : stod( + hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos / 100.0 : stod( hardware_info.joints[j].parameters.at( "p_vel")); double i_gain_vel = (hardware_info.joints[j].parameters.find("i_vel") == - hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( + hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos / 1000.0 : stod( hardware_info.joints[j].parameters.at( "i_vel")); double d_gain_vel = (hardware_info.joints[j].parameters.find("d_vel") == - hardware_info.joints[j].parameters.end() ) ? 5.0 : stod( + hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( hardware_info.joints[j].parameters.at( "d_vel")); // set integral max and min component to 50 percent of the max effort double iMax_vel = (hardware_info.joints[j].parameters.find("iMax_vel") == - hardware_info.joints[j].parameters.end() ) ? 500.0 : stod( + hardware_info.joints[j].parameters.end() ) ? max_effort / 2.0 : stod( hardware_info.joints[j].parameters.at( "iMax_vel")); double iMin_vel = (hardware_info.joints[j].parameters.find("iMin_vel") == - hardware_info.joints[j].parameters.end() ) ? -500 : stod( + hardware_info.joints[j].parameters.end() ) ? -1.0 * max_effort / 2.0 : stod( hardware_info.joints[j].parameters.at( "iMin_vel")); double cmdMax_vel = (hardware_info.joints[j].parameters.find("cmdMax_vel") == - hardware_info.joints[j].parameters.end() ) ? 1000.0 : stod( + hardware_info.joints[j].parameters.end() ) ? max_effort : stod( hardware_info.joints[j].parameters.at( "cmdMax_vel")); double cmdMin_vel = (hardware_info.joints[j].parameters.find("cmdMin_vel") == - hardware_info.joints[j].parameters.end() ) ? -1000.0 : stod( + hardware_info.joints[j].parameters.end() ) ? -1.0 * max_effort : stod( hardware_info.joints[j].parameters.at( "cmdMin_vel")); double cmdOffset_vel = @@ -814,19 +829,31 @@ hardware_interface::return_type IgnitionSystem::write( name].cmdOffset_vel); if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { - if (!this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint)) - { + + double velocity_error = this->dataPtr->joints_[i].joint_velocity - std::clamp( + this->dataPtr->joints_[i].joint_velocity_cmd, -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); + + // calculate target force/torque - output of inner pid + double target_force = this->dataPtr->joints_[i].pid_vel.Update( + velocity_error, std::chrono::duration( + period.to_chrono())); + + this->dataPtr->joints_[i].joint_velocity_to_force_cmd = target_force; + + auto forceCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + if (forceCmd == nullptr) { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointVelocityCmd({0})); + ignition::gazebo::components::JointForceCmd({target_force})); } else { - const auto jointVelCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); - *jointVelCmd = ignition::gazebo::components::JointVelocityCmd( - {this->dataPtr->joints_[i].joint_velocity_cmd}); + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); } + } else if (this->dataPtr->joints_[i].joint_control_method & POSITION) { // calculate error with clamped position command @@ -855,6 +882,8 @@ hardware_interface::return_type IgnitionSystem::write( velocity_error, std::chrono::duration( period.to_chrono())); + this->dataPtr->joints_[i].joint_position_to_force_cmd = target_force; + auto forceCmd = this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); @@ -892,49 +921,51 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->ecm->Component( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - const auto * jointAxisMimicked = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint); - if (mimic_interface == "position") { // Get the joint position - const auto * jp_mimicked = + double position_mimicked_joint = this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint); - double position_mimicked_joint = jp_mimicked->Data()[0]; + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; - const auto * jp_mimic = + double position_mimic_joint = this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - double position_mimic_joint = jp_mimic->Data()[0]; + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; - // calculate error with clamped position command - double position_error = position_mimic_joint - position_mimicked_joint * - mimic_joint.multiplier; + double position_error = position_mimic_joint - std::clamp( + position_mimicked_joint * + mimic_joint.multiplier, jointAxis->Data().Lower(), + jointAxis->Data().Upper()); - double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate); + position_error = + copysign( + 1.0, + position_error) * + std::clamp( + std::abs(position_error), 0.0, + std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); // calculate target velocity - output of outer pid - input to inner pid double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update( position_error, std::chrono::duration( period.to_chrono())); - const auto * jv_mimic = + // get mimic joint velocity + double velocity_mimic_joint = this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - double velocity_mimic_joint = jv_mimic->Data()[0]; + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; - double velocity_error = velocity_mimic_joint - target_vel; + double velocity_error = velocity_mimic_joint - std::clamp( + target_vel, -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); + this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( + mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position_to_force_cmd); // calculate target force/torque - output of inner pid double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( velocity_error, std::chrono::duration( period.to_chrono())); - igndbg << "[" << this->dataPtr->joints_[mimic_joint.joint_index].name << "]\t" << - position_mimicked_joint << " \t" << position_mimic_joint << " \t" << target_force << - "\n"; - auto forceCmd = this->dataPtr->ecm->Component( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); @@ -948,25 +979,41 @@ hardware_interface::return_type IgnitionSystem::write( {target_force}); } } - /* if (mimic_interface == "velocity") { // get the velocity of mimicked joint double velocity_mimicked_joint = this->dataPtr->ecm->Component( this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; - if (!this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) - { + // get mimic joint velocity + double velocity_mimic_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + + double velocity_error = velocity_mimic_joint - std::clamp( + mimic_joint.multiplier * velocity_mimicked_joint, -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); + + this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( + mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_to_force_cmd); + + // calculate target force/torque - output of inner pid + double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( + velocity_error, std::chrono::duration( + period.to_chrono())); + + auto forceCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + + if (forceCmd == nullptr) { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointVelocityCmd({0})); + ignition::gazebo::components::JointForceCmd({target_force})); } else { - const auto jointVelCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - *jointVelCmd = ignition::gazebo::components::JointVelocityCmd( - {mimic_joint.multiplier * velocity_mimicked_joint}); + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); } } if (mimic_interface == "effort") { @@ -987,9 +1034,9 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); *jointEffortCmd = ignition::gazebo::components::JointForceCmd( {mimic_joint.multiplier * - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort}); + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd}); } - }*/ + } } } From ea36469cf40a7c30b602624bae52f231f38e186c Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 15 Mar 2023 16:23:24 +0100 Subject: [PATCH 12/24] Introduce MimicJointSystem plugin. --- ign_ros2_control/CMakeLists.txt | 21 + .../ign_ros2_control/MimicJointSystem.hh | 94 ++++ ign_ros2_control/src/MimicJointSystem.cc | 411 ++++++++++++++++++ ign_ros2_control/src/ign_system.cpp | 145 +----- .../urdf/test_gripper_mimic_joint.xacro.urdf | 44 +- 5 files changed, 559 insertions(+), 156 deletions(-) create mode 100644 ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh create mode 100644 ign_ros2_control/src/MimicJointSystem.cc diff --git a/ign_ros2_control/CMakeLists.txt b/ign_ros2_control/CMakeLists.txt index f3775b08..7f7230be 100644 --- a/ign_ros2_control/CMakeLists.txt +++ b/ign_ros2_control/CMakeLists.txt @@ -44,10 +44,31 @@ elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress") set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) message(STATUS "Compiling against Ignition Fortress") + find_package(ignition-cmake2 REQUIRED) + find_package(ignition-plugin1 REQUIRED COMPONENTS register) + set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) + add_library(mimic-joint-system SHARED src/MimicJointSystem.cc) + target_link_libraries(mimic-joint-system + PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + PRIVATE ignition-gazebo6::ignition-gazebo6) + install(TARGETS mimic-joint-system + DESTINATION lib) + else() find_package(ignition-gazebo6 REQUIRED) set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) message(STATUS "Compiling against Ignition Fortress") + + find_package(ignition-cmake2 REQUIRED) + find_package(ignition-plugin1 REQUIRED COMPONENTS register) + set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) + add_library(mimic-joint-system SHARED src/MimicJointSystem.cc) + target_link_libraries(mimic-joint-system + ignition-gazebo${IGN_GAZEBO_VER}::core + ignition-plugin${IGN_PLUGIN_VER}::register + ) + install(TARGETS mimic-joint-system + DESTINATION lib) endif() find_package(ignition-plugin1 REQUIRED) diff --git a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh new file mode 100644 index 00000000..1abaec5d --- /dev/null +++ b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh @@ -0,0 +1,94 @@ +/* + * Copyright 2023 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2023-03-15 + * + */ +//---------------------------------------------------------------------- + +#ifndef IGNITION_GAZEBO_SYSTEMS_MIMICJOINTSYSTEM_HH_ +#define IGNITION_GAZEBO_SYSTEMS_MIMICJOINTSYSTEM_HH_ + +//! [header] +#include + +#include + +//! +//! joint_name +//! mimic_joint_name +//! 1.0 +//! 0.0 +//! 0 +//! 0 +//! 100.0 +//! 0.1 +//! 0.0 +//! 5.0 +//! -5.0 +//! 500.0 +//! -500.0 +//! 0.0 +//! 0.001 +//! false +//! + +namespace ign_ros2_control +{ + class MimicJointSystemPrivate; + + class MimicJointSystem: + // This class is a system. + public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + // This class also implements the ISystemPreUpdate, ISystemUpdate, + // and ISystemPostUpdate interfaces. + public ignition::gazebo::ISystemPreUpdate, + public ignition::gazebo::ISystemUpdate, + public ignition::gazebo::ISystemPostUpdate + { + public: MimicJointSystem(); + + public: ~MimicJointSystem() override=default; + + // Documentation inherited + public: void Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; + + public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + public: void Update(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) override; + + private: + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} +//! [header] + +#endif diff --git a/ign_ros2_control/src/MimicJointSystem.cc b/ign_ros2_control/src/MimicJointSystem.cc new file mode 100644 index 00000000..4d1e8fb8 --- /dev/null +++ b/ign_ros2_control/src/MimicJointSystem.cc @@ -0,0 +1,411 @@ +/* + * Copyright 2023 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Lovro Ivanov lovro.ivanov@gmail.com + * \date 2023-03-15 + * + */ +//---------------------------------------------------------------------- + +#include "ign_ros2_control/MimicJointSystem.hh" + +#include +#include + +#include +#include +#include + +#include "ignition/gazebo/components/JointForceCmd.hh" +#include "ignition/gazebo/components/JointVelocityCmd.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/JointType.hh" + +#include "ignition/gazebo/Model.hh" + +using namespace ign_ros2_control; + +class ign_ros2_control::MimicJointSystemPrivate { + +/// \brief Joint Entity + +public: + ignition::gazebo::Entity jointEntity {ignition::gazebo::kNullEntity}; + +public: + ignition::gazebo::Entity mimicJointEntity {ignition::gazebo::kNullEntity}; + +/// \brief Joint name + +public: + std::string jointName; + +public: + std::string mimicJointName; + +/// \brief Commanded joint position + +public: + double jointPosCmd {0.0}; + +public: + double multiplier {1.0}; + +public: + double offset {0.0}; + +/// \brief Model interface + +public: + ignition::gazebo::Model model {ignition::gazebo::kNullEntity}; + +/// \brief Position PID controller. + +public: + ignition::math::PID posPid; + +/// \brief Joint index to be used. + +public: + unsigned int jointIndex = 0u; + +public: + unsigned int mimicJointIndex = 0u; + +public: + double deadZone = 1e-6; + + // approach adopted from https://github.com/gazebosim/gz-sim/blob/330eaf2f135301e90096fe91897f052cdaa77013/src/systems/joint_position_controller/JointPositionController.cc#L69-L77 +/// \brief Operation modes + enum OperationMode + { + /// \brief Use PID to achieve positional control + PID, + /// \brief Bypass PID completely. This means the joint will move to that + /// position bypassing the physics engine. + ABS + }; + +/// \brief Joint position mode + +public: + OperationMode mode = OperationMode::PID; + +}; + +MimicJointSystem::MimicJointSystem() +: dataPtr(std::make_unique < ign_ros2_control::MimicJointSystemPrivate > ()) +{ +} + +void MimicJointSystem::Configure( + const ignition::gazebo::Entity & _entity, + const std::shared_ptr < const sdf::Element > & _sdf, + ignition::gazebo::EntityComponentManager & _ecm, + ignition::gazebo::EventManager & /* _eventMgr*/) +{ + + // Make sure the controller is attached to a valid model + this->dataPtr->model = ignition::gazebo::Model(_entity); + if (!this->dataPtr->model.Valid(_ecm)) { + + ignerr << "MimicJointSystem Failed to initialize because [" << + this->dataPtr->model.Name(_ecm) << + "] (Entity=" << _entity << ")] is not a model.. "; + return; + } + + // Get params from SDF + this->dataPtr->jointName = _sdf->Get < std::string > ("joint_name"); + + if (this->dataPtr->jointName.empty()) { + ignerr << "MimicJointSystem found an empty joint_name parameter. " + << "Failed to initialize."; + return; + } + + this->dataPtr->mimicJointName = _sdf->Get < std::string > ("mimic_joint_name"); + if (this->dataPtr->mimicJointName.empty()) { + ignerr << "MimicJointSystem found an empty mimic_joint_name parameter. " + << "Failed to initialize."; + return; + } + + if (_sdf->HasElement("multiplier")) { + this->dataPtr->multiplier = _sdf->Get < double > ("multiplier"); + } + + if (_sdf->HasElement("offset")) { + this->dataPtr->offset = _sdf->Get < double > ("offset"); + } + + if (_sdf->HasElement("joint_index")) { + this->dataPtr->jointIndex = _sdf->Get < unsigned int > ("joint_index"); + } + + if (_sdf->HasElement("mimic_joint_index")) { + this->dataPtr->mimicJointIndex = _sdf->Get < unsigned int > ("mimic_joint_index"); + } + + if (_sdf->HasElement("dead_zone")) { + this->dataPtr->deadZone = _sdf->Get < double > ("dead_zone"); + } + + // PID parameters + double p = 1; + double i = 0.1; + double d = 0.01; + double iMax = 1; + double iMin = -1; + double cmdMax = 1000; + double cmdMin = -1000; + double cmdOffset = 0; + + if (_sdf->HasElement("p_gain")) { + p = _sdf->Get < double > ("p_gain"); + } + if (_sdf->HasElement("i_gain")) { + i = _sdf->Get < double > ("i_gain"); + } + if (_sdf->HasElement("d_gain")) { + d = _sdf->Get < double > ("d_gain"); + } + if (_sdf->HasElement("i_max")) { + iMax = _sdf->Get < double > ("i_max"); + } + if (_sdf->HasElement("i_min")) { + iMin = _sdf->Get < double > ("i_min"); + } + if (_sdf->HasElement("cmd_max")) { + cmdMax = _sdf->Get < double > ("cmd_max"); + } + if (_sdf->HasElement("cmd_min")) { + cmdMin = _sdf->Get < double > ("cmd_min"); + } + if (_sdf->HasElement("cmd_offset")) { + cmdOffset = _sdf->Get < double > ("cmd_offset"); + } + if (_sdf->HasElement("use_velocity_commands")) { + auto useVelocityCommands = _sdf->Get < bool > ("use_velocity_commands"); + if (useVelocityCommands) { + this->dataPtr->mode = + MimicJointSystemPrivate::OperationMode::ABS; + } + } + + this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset); + + + if (_sdf->HasElement("initial_position")) { + this->dataPtr->jointPosCmd = _sdf->Get < double > ("initial_position"); + } + + igndbg << "[MimicJointSystem] system parameters:\n"; + igndbg << "p_gain: [" << p << "]\n"; + igndbg << "i_gain: [" << i << "]\n"; + igndbg << "d_gain: [" << d << "]\n"; + igndbg << "i_max: [" << iMax << "]\n"; + igndbg << "i_min: [" << iMin << "]\n"; + igndbg << "cmd_max: [" << cmdMax << "]\n"; + igndbg << "cmd_min: [" << cmdMin << "]\n"; + igndbg << "cmd_offset: [" << cmdOffset << "]\n"; + igndbg << "initial_position: [" << this->dataPtr->jointPosCmd << "]\n"; + +} + +void MimicJointSystem::PreUpdate( + const ignition::gazebo::UpdateInfo & _info, + ignition::gazebo::EntityComponentManager & _ecm) +{ + + if (_info.dt < std::chrono::steady_clock::duration::zero()) { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast < std::chrono::seconds > (_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + // If the joint hasn't been identified yet, look for it + if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity || + this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) + { + + auto jointEntities = _ecm.ChildrenByComponents( + this->dataPtr->model.Entity(), ignition::gazebo::components::Joint()); + + // Iterate over all joints and verify whether they can be enabled or not + for (const auto & jointEntity : jointEntities) { + const auto jointName = _ecm.Component < ignition::gazebo::components::Name > ( + jointEntity)->Data(); + if (jointName == this->dataPtr->jointName) { + this->dataPtr->jointEntity = jointEntity; + } else if (jointName == this->dataPtr->mimicJointName) { + this->dataPtr->mimicJointEntity = jointEntity; + } + } + } + + if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) + return; + + + if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) + return; + + + if (_info.paused) { + return; + } + + auto jointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition > ( + this->dataPtr->jointEntity); + if (!jointPosComp) { + _ecm.CreateComponent( + this->dataPtr->jointEntity, + ignition::gazebo::components::JointPosition()); + } + + auto mimicJointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition > ( + this->dataPtr->mimicJointEntity); + if (!mimicJointPosComp) { + _ecm.CreateComponent( + this->dataPtr->mimicJointEntity, + ignition::gazebo::components::JointPosition()); + } + + if (jointPosComp == nullptr || jointPosComp->Data().empty() || mimicJointPosComp == nullptr || + mimicJointPosComp->Data().empty() ) + { + return; + } + + // Sanity check: Make sure that the joint index is valid. + if (this->dataPtr->jointIndex >= jointPosComp->Data().size()) { + static std::unordered_set < ignition::gazebo::Entity > reported; + if (reported.find(this->dataPtr->jointEntity) == reported.end()) { + ignerr << "[MimicJointSystem]: Detected an invalid " + << "parameter. The index specified is [" + << this->dataPtr->jointIndex << "] but joint [" + << this->dataPtr->jointName << "] only has [" + << jointPosComp->Data().size() << "] index[es]. " + << "This controller will be ignored" << std::endl; + reported.insert(this->dataPtr->jointEntity); + } + return; + } + + // Sanity check: Make sure that the mimic joint index is valid. + if (this->dataPtr->mimicJointIndex >= mimicJointPosComp->Data().size()) { + static std::unordered_set < ignition::gazebo::Entity > mimic_reported; + if (mimic_reported.find(this->dataPtr->mimicJointEntity) == mimic_reported.end()) { + ignerr << "[MimicJointSystem]: Detected an invalid " + << "parameter. The index specified is [" + << this->dataPtr->mimicJointIndex << "] but joint [" + << this->dataPtr->mimicJointName << "] only has [" + << mimicJointPosComp->Data().size() << "] index[es]. " + << "This controller will be ignored" << std::endl; + mimic_reported.insert(this->dataPtr->mimicJointEntity); + } + return; + } + + // Get error in position + double error = jointPosComp->Data().at(this->dataPtr->jointIndex) - + (mimicJointPosComp->Data().at(this->dataPtr->mimicJointIndex) * + this->dataPtr->multiplier + this->dataPtr->offset); + + if (fabs(error) > this->dataPtr->deadZone) { + + // Check if the mode is ABS + if (this->dataPtr->mode == + MimicJointSystemPrivate::OperationMode::ABS) + { + // Calculate target velcity + double targetVel = 0; + + // Get time in seconds + auto dt = std::chrono::duration < double > (_info.dt).count(); + + // Get the maximum amount in m that this joint may move + auto maxMovement = this->dataPtr->posPid.CmdMax() * dt; + + // Limit the maximum change to maxMovement + if (abs(error) > maxMovement) { + targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() : + -this->dataPtr->posPid.CmdMax(); + } else { + targetVel = -error; + } + + // Update velocity command. + auto vel = _ecm.Component < ignition::gazebo::components::JointVelocityCmd > + (this->dataPtr->jointEntity); + + if (vel == nullptr) { + _ecm.CreateComponent( + this->dataPtr->jointEntity, + ignition::gazebo::components::JointVelocityCmd({targetVel})); + } else { + *vel = ignition::gazebo::components::JointVelocityCmd({targetVel}); + } + return; + } + + // Update force command. + double force = this->dataPtr->posPid.Update(error, _info.dt); + + auto forceComp = + _ecm.Component < ignition::gazebo::components::JointForceCmd > + (this->dataPtr->jointEntity); + if (forceComp == nullptr) { + _ecm.CreateComponent( + this->dataPtr->jointEntity, + ignition::gazebo::components::JointForceCmd({force})); + } else { + *forceComp = ignition::gazebo::components::JointForceCmd({force}); + } + } +} + +void MimicJointSystem::Update( + const ignition::gazebo::UpdateInfo & /*_info*/, + ignition::gazebo::EntityComponentManager & /*_ecm*/) +{ +// ignmsg << "MimicJointSystem::Update" << std::endl; +} + +void MimicJointSystem::PostUpdate( + const ignition::gazebo::UpdateInfo & /*_info*/, + const ignition::gazebo::EntityComponentManager & /*_ecm*/) +{ +// ignmsg << "MimicJointSystem::PostUpdate" << std::endl; +} + +//! [registerMimicJointSystem] + +IGNITION_ADD_PLUGIN( + ign_ros2_control::MimicJointSystem, + ignition::gazebo::System, + MimicJointSystem::ISystemConfigure, + MimicJointSystem::ISystemPreUpdate, + MimicJointSystem::ISystemUpdate, + MimicJointSystem::ISystemPostUpdate) +//! [registerMimicJointSystem] diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index bdd89ce8..84190aeb 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -68,11 +68,9 @@ struct jointData /// \brief Current cmd joint position double joint_position_cmd; - double joint_position_to_force_cmd; /// \brief Current cmd joint velocity double joint_velocity_cmd; - double joint_velocity_to_force_cmd; /// \brief Current cmd joint effort double joint_effort_cmd; @@ -95,6 +93,7 @@ struct MimicJoint std::size_t joint_index; std::size_t mimicked_joint_index; double multiplier = 1.0; + double mimic_ff_force_scaling = 0.01; std::vector interfaces_to_mimic; }; @@ -402,6 +401,12 @@ bool IgnitionSystem::initSim( mimic_joint.multiplier = 1.0; } + mimic_joint.mimic_ff_force_scaling = + (hardware_info.joints[j].parameters.find("mimic_ff_force_scaling") == + hardware_info.joints[j].parameters.end() ) ? mimic_joint.mimic_ff_force_scaling : stod( + hardware_info.joints[j].parameters.at( + "mimic_ff_force_scaling")); + // check joint info of mimicked joint auto & joint_info_mimicked = hardware_info.joints[mimic_joint.mimicked_joint_index]; const auto state_mimicked_interface = std::find_if( @@ -699,7 +704,8 @@ hardware_interface::return_type IgnitionSystem::read( this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0]; - // this->dataPtr->joint_effort_[j] = jointForce->Data()[0]; + // set effort state interface to computed/propagated effort command - passthrough because of ignitionrobotics/ign-physics#124 + this->dataPtr->joints_[i].joint_effort = this->dataPtr->joints_[i].joint_effort_cmd; } for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { @@ -839,7 +845,8 @@ hardware_interface::return_type IgnitionSystem::write( velocity_error, std::chrono::duration( period.to_chrono())); - this->dataPtr->joints_[i].joint_velocity_to_force_cmd = target_force; + // remember for potential effort state interface + this->dataPtr->joints_[i].joint_effort_cmd = target_force; auto forceCmd = this->dataPtr->ecm->Component( @@ -882,7 +889,8 @@ hardware_interface::return_type IgnitionSystem::write( velocity_error, std::chrono::duration( period.to_chrono())); - this->dataPtr->joints_[i].joint_position_to_force_cmd = target_force; + // remember for potential effort state interface + this->dataPtr->joints_[i].joint_effort_cmd = target_force; auto forceCmd = this->dataPtr->ecm->Component( @@ -913,133 +921,6 @@ hardware_interface::return_type IgnitionSystem::write( } } - // set values of all mimic joints with respect to mimicked joint - for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { - // assuming every mimic joint has axis - const auto * jointAxis = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - - if (mimic_interface == "position") { - // Get the joint position - double position_mimicked_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; - - double position_mimic_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; - - double position_error = position_mimic_joint - std::clamp( - position_mimicked_joint * - mimic_joint.multiplier, jointAxis->Data().Lower(), - jointAxis->Data().Upper()); - - position_error = - copysign( - 1.0, - position_error) * - std::clamp( - std::abs(position_error), 0.0, - std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); - - // calculate target velocity - output of outer pid - input to inner pid - double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update( - position_error, std::chrono::duration( - period.to_chrono())); - - // get mimic joint velocity - double velocity_mimic_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; - - double velocity_error = velocity_mimic_joint - std::clamp( - target_vel, -1.0 * jointAxis->Data().MaxVelocity(), - jointAxis->Data().MaxVelocity()); - - this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( - mimic_joint.multiplier * - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position_to_force_cmd); - // calculate target force/torque - output of inner pid - double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( - velocity_error, std::chrono::duration( - period.to_chrono())); - - auto forceCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - - if (forceCmd == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointForceCmd({target_force})); - } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); - } - } - if (mimic_interface == "velocity") { - // get the velocity of mimicked joint - double velocity_mimicked_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; - - // get mimic joint velocity - double velocity_mimic_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; - - double velocity_error = velocity_mimic_joint - std::clamp( - mimic_joint.multiplier * velocity_mimicked_joint, -1.0 * jointAxis->Data().MaxVelocity(), - jointAxis->Data().MaxVelocity()); - - this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( - mimic_joint.multiplier * - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_to_force_cmd); - - // calculate target force/torque - output of inner pid - double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( - velocity_error, std::chrono::duration( - period.to_chrono())); - - auto forceCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - - if (forceCmd == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointForceCmd({target_force})); - } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); - } - } - if (mimic_interface == "effort") { - // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 - // Get the joint force - // const auto * jointForce = - // _ecm.Component( - // this->dataPtr->sim_joints_[j]); - if (!this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) - { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointForceCmd({0})); - } else { - const auto jointEffortCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - *jointEffortCmd = ignition::gazebo::components::JointForceCmd( - {mimic_joint.multiplier * - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd}); - } - } - } - } - return hardware_interface::return_type::OK; } } // namespace ign_ros2_control diff --git a/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf b/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf index 1ceb1697..f15ae91e 100644 --- a/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf @@ -57,6 +57,7 @@ + @@ -65,6 +66,7 @@ + @@ -94,35 +96,29 @@ -100 0.0 - - right_finger_joint - 1 - - - - - 100.0 - 0.0 - 20.0 - 5.0 - -5.0 - 10.0 - -10.0 - 0.0 - 10.0 - 1.0 - 0.0 - 50 - -50 - 100 - -100 - 0.0 - $(find ign_ros2_control_demos)/config/gripper_controller.yaml + + left_finger_joint + right_finger_joint + 1.0 + 0.0 + 0 + 0 + 500.0 + 0.1 + 100.0 + 50.0 + -50.0 + 100.0 + -100.0 + 0.0 + 0.001 + false + From 65b7388dac991cbe2803e9648caa8c909c6cda03 Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 17 Mar 2023 01:29:03 +0100 Subject: [PATCH 13/24] Add mimic joint vel pid only support. --- ign_ros2_control/src/MimicJointSystem.cc | 10 +- ign_ros2_control/src/ign_system.cpp | 137 ++++++++++++++++++++++- 2 files changed, 139 insertions(+), 8 deletions(-) diff --git a/ign_ros2_control/src/MimicJointSystem.cc b/ign_ros2_control/src/MimicJointSystem.cc index 4d1e8fb8..e72a34c2 100644 --- a/ign_ros2_control/src/MimicJointSystem.cc +++ b/ign_ros2_control/src/MimicJointSystem.cc @@ -263,12 +263,14 @@ void MimicJointSystem::PreUpdate( } } - if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) + if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) { return; + } - if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) + if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) { return; + } if (_info.paused) { @@ -329,8 +331,8 @@ void MimicJointSystem::PreUpdate( // Get error in position double error = jointPosComp->Data().at(this->dataPtr->jointIndex) - - (mimicJointPosComp->Data().at(this->dataPtr->mimicJointIndex) * - this->dataPtr->multiplier + this->dataPtr->offset); + (mimicJointPosComp->Data().at(this->dataPtr->mimicJointIndex) * + this->dataPtr->multiplier + this->dataPtr->offset); if (fabs(error) > this->dataPtr->deadZone) { diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 84190aeb..bb2dcdbb 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -705,7 +705,7 @@ hardware_interface::return_type IgnitionSystem::read( this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0]; // set effort state interface to computed/propagated effort command - passthrough because of ignitionrobotics/ign-physics#124 - this->dataPtr->joints_[i].joint_effort = this->dataPtr->joints_[i].joint_effort_cmd; + this->dataPtr->joints_[i].joint_effort = this->dataPtr->joints_[i].joint_effort_cmd; } for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { @@ -845,8 +845,8 @@ hardware_interface::return_type IgnitionSystem::write( velocity_error, std::chrono::duration( period.to_chrono())); - // remember for potential effort state interface - this->dataPtr->joints_[i].joint_effort_cmd = target_force; + // remember for potential effort state interface + this->dataPtr->joints_[i].joint_effort_cmd = target_force; auto forceCmd = this->dataPtr->ecm->Component( @@ -886,7 +886,7 @@ hardware_interface::return_type IgnitionSystem::write( // calculate target force/torque - output of inner pid double target_force = this->dataPtr->joints_[i].pid_vel.Update( - velocity_error, std::chrono::duration( + position_error, std::chrono::duration( period.to_chrono())); // remember for potential effort state interface @@ -921,6 +921,135 @@ hardware_interface::return_type IgnitionSystem::write( } } + // set values of all mimic joints with respect to mimicked joint + for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { + for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { + // assuming every mimic joint has axis + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + + if (mimic_interface == "position") { + // Get the joint position + double position_mimicked_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; + + double position_mimic_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + + double position_error = position_mimic_joint - std::clamp( + position_mimicked_joint * + mimic_joint.multiplier, jointAxis->Data().Lower(), + jointAxis->Data().Upper()); + + position_error = + copysign( + 1.0, + position_error) * + std::clamp( + std::abs(position_error), 0.0, + std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); + + // calculate target velocity - output of outer pid - input to inner pid + double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update( + position_error, std::chrono::duration( + period.to_chrono())); + + // get mimic joint velocity + double velocity_mimic_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + + double velocity_error = velocity_mimic_joint - std::clamp( + target_vel, -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); + + this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( + mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd); + // calculate target force/torque - output of inner pid + double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( + position_error, std::chrono::duration( + period.to_chrono())); + + this->dataPtr->joints_[mimic_joint.joint_index].joint_effort_cmd = target_force; + + auto forceCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + + if (forceCmd == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({target_force})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); + } + } + if (mimic_interface == "velocity") { + // get the velocity of mimicked joint + double velocity_mimicked_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; + + // get mimic joint velocity + double velocity_mimic_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + + double velocity_error = velocity_mimic_joint - std::clamp( + mimic_joint.multiplier * velocity_mimicked_joint, -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); + + this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( + mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd); + + // calculate target force/torque - output of inner pid + double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( + velocity_error, std::chrono::duration( + period.to_chrono())); + + auto forceCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + + if (forceCmd == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({target_force})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd( + {target_force}); + } + } + if (mimic_interface == "effort") { + // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 + // Get the joint force + // const auto * jointForce = + // _ecm.Component( + // this->dataPtr->sim_joints_[j]); + if (!this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) + { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({0})); + } else { + const auto jointEffortCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointEffortCmd = ignition::gazebo::components::JointForceCmd( + {mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd}); + } + } + } + } + return hardware_interface::return_type::OK; } } // namespace ign_ros2_control From 1a89e0664955db392271522172c3213b2023b4c1 Mon Sep 17 00:00:00 2001 From: Lovro Date: Mon, 20 Mar 2023 12:38:46 +0100 Subject: [PATCH 14/24] Round commanded force. --- ign_ros2_control/src/ign_system.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index bb2dcdbb..65e25c30 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -889,7 +889,9 @@ hardware_interface::return_type IgnitionSystem::write( position_error, std::chrono::duration( period.to_chrono())); - // remember for potential effort state interface + target_force = round(target_force * 10000.0)/10000.0; + + // remember for potential effort state interface this->dataPtr->joints_[i].joint_effort_cmd = target_force; auto forceCmd = @@ -944,6 +946,8 @@ hardware_interface::return_type IgnitionSystem::write( mimic_joint.multiplier, jointAxis->Data().Lower(), jointAxis->Data().Upper()); + position_error = round(position_error * 10000.0)/10000.0; + position_error = copysign( 1.0, @@ -952,7 +956,9 @@ hardware_interface::return_type IgnitionSystem::write( std::abs(position_error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); - // calculate target velocity - output of outer pid - input to inner pid + + + // calculate target velocity - output of outer pid - input to inner pid double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update( position_error, std::chrono::duration( period.to_chrono())); @@ -974,6 +980,8 @@ hardware_interface::return_type IgnitionSystem::write( position_error, std::chrono::duration( period.to_chrono())); + target_force = round(target_force * 10000.0)/10000.0; + this->dataPtr->joints_[mimic_joint.joint_index].joint_effort_cmd = target_force; auto forceCmd = From 9bbb8749ce4a6072b0d6503c19833ff2d0744caa Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Mar 2023 14:07:44 +0100 Subject: [PATCH 15/24] Remove suffix for state interfaces as it confuses robot_state_publisher by default. --- ign_ros2_control/src/ign_system.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 65e25c30..683fb696 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -432,8 +432,6 @@ bool IgnitionSystem::initSim( "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint << "' with mutiplier: " << mimic_joint.multiplier); this->dataPtr->mimic_joints_.push_back(mimic_joint); - // TODO (livanov93): add parameter if suffix is used or not - // suffix = "_mimic"; } RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); From e40142c66370bbab409963333b429863e6dfabe1 Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Mar 2023 15:03:46 +0100 Subject: [PATCH 16/24] Add cascade control parameter. Code refactoring. --- .../src/ign_ros2_control_parameters.yaml | 7 + ign_ros2_control/src/ign_system.cpp | 146 ++++++++++++------ 2 files changed, 107 insertions(+), 46 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_parameters.yaml b/ign_ros2_control/src/ign_ros2_control_parameters.yaml index f5eb2c9e..61d4d86e 100644 --- a/ign_ros2_control/src/ign_ros2_control_parameters.yaml +++ b/ign_ros2_control/src/ign_ros2_control_parameters.yaml @@ -7,6 +7,13 @@ ign_ros2_control: unique<>: null, } } + mode: + __map_joints: + use_cascade_control: { + type: bool, + default_value: false, + description: "Defines if cascade control is ued in position control (outer position based loop and inner velocity based loop with force output)" + } gains: __map_joints: p_pos: { diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 683fb696..496aa8ff 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -254,6 +254,21 @@ bool IgnitionSystem::initSim( this->dataPtr->ecm->Component( this->dataPtr->joints_[j].sim_joint); + double use_cascade_control = + (hardware_info.joints[j].parameters.find("use_cascade_control") == + hardware_info.joints[j].parameters.end() ) ? false : [&](){ + if(hardware_info.joints[j].parameters.at( + "use_cascade_control") == "true" || hardware_info.joints[j].parameters.at( + "use_cascade_control") == "True"){ + return true; + }else{ + return false; + } + }(); + + param_vec.push_back(rclcpp::Parameter{"mode." + joint_name + ".use_cascade_control", p_gain_pos}); + + double upper = jointAxis->Data().Upper(); double lower = jointAxis->Data().Lower(); double max_velocity = jointAxis->Data().MaxVelocity(); @@ -834,9 +849,12 @@ hardware_interface::return_type IgnitionSystem::write( if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { - double velocity_error = this->dataPtr->joints_[i].joint_velocity - std::clamp( - this->dataPtr->joints_[i].joint_velocity_cmd, -1.0 * jointAxis->Data().MaxVelocity(), - jointAxis->Data().MaxVelocity()); + double velocity = this->dataPtr->joints_[i].joint_velocity; + double velocity_cmd_clamped = std::clamp( + this->dataPtr->joints_[i].joint_velocity_cmd, -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); + + double velocity_error = velocity - velocity_cmd_clamped; // calculate target force/torque - output of inner pid double target_force = this->dataPtr->joints_[i].pid_vel.Update( @@ -862,31 +880,51 @@ hardware_interface::return_type IgnitionSystem::write( } else if (this->dataPtr->joints_[i].joint_control_method & POSITION) { // calculate error with clamped position command - double position_error = this->dataPtr->joints_[i].joint_position - std::clamp( - this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), - jointAxis->Data().Upper()); - position_error = - copysign( - 1.0, - position_error) * - std::clamp( - std::abs(position_error), 0.0, - std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); - - // calculate target velocity - output of outer pid - input to inner pid - double target_vel = this->dataPtr->joints_[i].pid_pos.Update( - position_error, std::chrono::duration( - period.to_chrono())); + double position = this->dataPtr->joints_[i].joint_position; + double position_cmd_clamped = std::clamp( + this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), + jointAxis->Data().Upper()); - double velocity_error = this->dataPtr->joints_[i].joint_velocity - std::clamp( - target_vel, -1.0 * jointAxis->Data().MaxVelocity(), - jointAxis->Data().MaxVelocity()); + double position_error = position - position_cmd_clamped; + + double position_error_sign = copysign( + 1.0, + position_error); + + double position_error_abs_clamped = std::clamp( + std::abs(position_error), 0.0, + std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); + + // move forward with calculated position error + position_error = position_error_sign * position_error_abs_clamped; + + double position_or_velocity_error = 0.0; + + // check if cascade control is used for this joint + if( params_.mode.joints_map[this->dataPtr->joints_[i]. + name].use_cascade_control) + { + // calculate target velocity - output of outer pid - input to inner pid + double target_vel = this->dataPtr->joints_[i].pid_pos.Update( + position_error, std::chrono::duration(period.to_chrono())); + + double velocity_error = + this->dataPtr->joints_[i].joint_velocity - + std::clamp(target_vel, -1.0 * jointAxis->Data().MaxVelocity(), jointAxis->Data().MaxVelocity()); + + // prepare velocity error value for inner pid + position_or_velocity_error = velocity_error; + }else{ + // prepare velocity error value for inner pid + position_or_velocity_error = position_error; + } // calculate target force/torque - output of inner pid double target_force = this->dataPtr->joints_[i].pid_vel.Update( - position_error, std::chrono::duration( + position_or_velocity_error, std::chrono::duration( period.to_chrono())); + // round the force target_force = round(target_force * 10000.0)/10000.0; // remember for potential effort state interface @@ -939,47 +977,63 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->ecm->Component( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; - double position_error = position_mimic_joint - std::clamp( - position_mimicked_joint * - mimic_joint.multiplier, jointAxis->Data().Lower(), - jointAxis->Data().Upper()); + double position_target_from_mimicked_joint = std::clamp( + position_mimicked_joint * + mimic_joint.multiplier, jointAxis->Data().Lower(), + jointAxis->Data().Upper()); - position_error = round(position_error * 10000.0)/10000.0; + double position_error = position_mimic_joint - position_target_from_mimicked_joint; - position_error = - copysign( - 1.0, - position_error) * - std::clamp( - std::abs(position_error), 0.0, - std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); + // round position error for simulation stability + position_error = round(position_error * 10000.0)/10000.0; + double position_error_sign = copysign( + 1.0, + position_error); + double position_error_abs_clamped = std::clamp( + std::abs(position_error), 0.0, + std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); - // calculate target velocity - output of outer pid - input to inner pid - double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update( - position_error, std::chrono::duration( - period.to_chrono())); + position_error = position_error_sign * position_error_abs_clamped; - // get mimic joint velocity - double velocity_mimic_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + double position_or_velocity_error = 0.0; - double velocity_error = velocity_mimic_joint - std::clamp( - target_vel, -1.0 * jointAxis->Data().MaxVelocity(), - jointAxis->Data().MaxVelocity()); + // check if cascade control is used for this joint + if( params_.mode.joints_map[this->dataPtr->joints_[mimic_joint.joint_index]. + name].use_cascade_control) + { + // calculate target velocity - output of outer pid - input to inner pid + double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update( + position_error, std::chrono::duration(period.to_chrono())); + + // get mimic joint velocity + double velocity_mimic_joint = this->dataPtr->ecm + ->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint) + ->Data()[0]; + + position_or_velocity_error = velocity_mimic_joint - std::clamp(target_vel, -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); + }else{ + position_or_velocity_error = position_error; + } + // set command offset - feed forward term added to the pid output that is clamped by pid max command value + // while taking into account mimic multiplier this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd); + // calculate target force/torque - output of inner pid double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( - position_error, std::chrono::duration( + position_or_velocity_error, std::chrono::duration( period.to_chrono())); + // round force value for simulation stability target_force = round(target_force * 10000.0)/10000.0; + // remember for potential effort state interface this->dataPtr->joints_[mimic_joint.joint_index].joint_effort_cmd = target_force; auto forceCmd = From 6c671e4aefe874ac28fd09529eddd95c85de0293 Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Mar 2023 15:11:08 +0100 Subject: [PATCH 17/24] Identation, alphabetizing, usage description comment, empty destructor removal. --- ign_ros2_control/CMakeLists.txt | 13 ++++++------- .../include/ign_ros2_control/MimicJointSystem.hh | 9 ++++----- .../include/ign_ros2_control/ign_system.hpp | 6 +++--- 3 files changed, 13 insertions(+), 15 deletions(-) diff --git a/ign_ros2_control/CMakeLists.txt b/ign_ros2_control/CMakeLists.txt index 7f7230be..caa232f3 100644 --- a/ign_ros2_control/CMakeLists.txt +++ b/ign_ros2_control/CMakeLists.txt @@ -20,10 +20,10 @@ find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(yaml_cpp_vendor REQUIRED) -find_package(generate_parameter_library REQUIRED) generate_parameter_library(ign_ros2_control_parameters src/ign_ros2_control_parameters.yaml @@ -48,9 +48,9 @@ elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress") find_package(ignition-plugin1 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) add_library(mimic-joint-system SHARED src/MimicJointSystem.cc) - target_link_libraries(mimic-joint-system - PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} - PRIVATE ignition-gazebo6::ignition-gazebo6) + target_link_libraries(mimic-joint-system + PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + PRIVATE ignition-gazebo6::ignition-gazebo6) install(TARGETS mimic-joint-system DESTINATION lib) @@ -64,9 +64,8 @@ else() set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) add_library(mimic-joint-system SHARED src/MimicJointSystem.cc) target_link_libraries(mimic-joint-system - ignition-gazebo${IGN_GAZEBO_VER}::core - ignition-plugin${IGN_PLUGIN_VER}::register - ) + ignition-gazebo${IGN_GAZEBO_VER}::core + ignition-plugin${IGN_PLUGIN_VER}::register) install(TARGETS mimic-joint-system DESTINATION lib) endif() diff --git a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh index 1abaec5d..0fcf61ba 100644 --- a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh +++ b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh @@ -32,6 +32,7 @@ #include +// This is an example to go in your urdf (is that correct?) //! //! joint_name //! mimic_joint_name @@ -59,7 +60,7 @@ namespace ign_ros2_control // This class is a system. public ignition::gazebo::System, public ignition::gazebo::ISystemConfigure, - // This class also implements the ISystemPreUpdate, ISystemUpdate, + // This class also implements the ISystemPreUpdate, ISystemUpdate, // and ISystemPostUpdate interfaces. public ignition::gazebo::ISystemPreUpdate, public ignition::gazebo::ISystemUpdate, @@ -67,10 +68,8 @@ namespace ign_ros2_control { public: MimicJointSystem(); - public: ~MimicJointSystem() override=default; - - // Documentation inherited - public: void Configure(const ignition::gazebo::Entity &_entity, + // Documentation inherited + public: void Configure(const ignition::gazebo::Entity &_entity, const std::shared_ptr &_sdf, ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &_eventMgr) override; diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp index 85c2f353..9394f502 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp +++ b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp @@ -22,12 +22,12 @@ #include #include "ign_ros2_control/ign_system_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" - #include "ign_ros2_control_parameters.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + namespace ign_ros2_control { From 18318350f58b3f54d76f6342e78cd2cd725ba409 Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Mar 2023 15:44:14 +0100 Subject: [PATCH 18/24] Fix error. --- ign_ros2_control/src/ign_system.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 09d18335..9223867d 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -254,7 +254,7 @@ bool IgnitionSystem::initSim( this->dataPtr->ecm->Component( this->dataPtr->joints_[j].sim_joint); - double use_cascade_control = + bool use_cascade_control = (hardware_info.joints[j].parameters.find("use_cascade_control") == hardware_info.joints[j].parameters.end() ) ? false : [&](){ if(hardware_info.joints[j].parameters.at( @@ -266,7 +266,7 @@ bool IgnitionSystem::initSim( } }(); - param_vec.push_back(rclcpp::Parameter{"mode." + joint_name + ".use_cascade_control", p_gain_pos}); + param_vec.push_back(rclcpp::Parameter{"mode." + joint_name + ".use_cascade_control", use_cascade_control}); double upper = jointAxis->Data().Upper(); From e8ac35e2cb2ffc223d33dfed23334908ca291a34 Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Mar 2023 16:14:11 +0100 Subject: [PATCH 19/24] Namespace fix. --- ign_ros2_control/src/ign_system.cpp | 642 ++++++++++++++-------------- 1 file changed, 317 insertions(+), 325 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 9223867d..2c78b31e 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -177,8 +177,7 @@ bool IgnitionSystem::initSim( rclcpp::Node::SharedPtr & model_nh, std::map & enableJoints, const hardware_interface::HardwareInfo & hardware_info, - ignition::gazebo::EntityComponentManager & _ecm, - int & update_rate) + ignition::gazebo::EntityComponentManager & _ecm, int & update_rate) { this->dataPtr = std::make_unique(); this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); @@ -195,13 +194,12 @@ bool IgnitionSystem::initSim( constexpr double default_gain = 0.1; if (!this->nh_->get_parameter_or( - "position_proportional_gain", - this->dataPtr->position_proportional_gain_, default_gain)) + "position_proportional_gain", this->dataPtr->position_proportional_gain_, + default_gain)) { RCLCPP_WARN_STREAM( this->nh_->get_logger(), - "The position_proportional_gain parameter was not defined, defaulting to: " << - default_gain); + "The position_proportional_gain parameter was not defined, defaulting to: " << default_gain); } if (this->dataPtr->n_dof_ == 0) { @@ -211,7 +209,8 @@ bool IgnitionSystem::initSim( // parameters needed for joint control std::vector joint_names; - this->param_node_ = rclcpp::Node::make_shared( + this->param_node_ = + rclcpp::Node::make_shared( hardware_info.name, rclcpp::NodeOptions().allow_undeclared_parameters(true)); std::vector param_vec; @@ -252,22 +251,26 @@ bool IgnitionSystem::initSim( const auto * jointAxis = this->dataPtr->ecm->Component( - this->dataPtr->joints_[j].sim_joint); + this->dataPtr->joints_[ + j].sim_joint); bool use_cascade_control = - (hardware_info.joints[j].parameters.find("use_cascade_control") == - hardware_info.joints[j].parameters.end() ) ? false : [&](){ - if(hardware_info.joints[j].parameters.at( - "use_cascade_control") == "true" || hardware_info.joints[j].parameters.at( - "use_cascade_control") == "True"){ - return true; - }else{ - return false; - } - }(); - - param_vec.push_back(rclcpp::Parameter{"mode." + joint_name + ".use_cascade_control", use_cascade_control}); + (hardware_info.joints[j].parameters.find("use_cascade_control") == + hardware_info.joints[j].parameters.end()) ? + false : + [&]() { + if (hardware_info.joints[j].parameters.at("use_cascade_control") == "true" || + hardware_info.joints[j].parameters.at("use_cascade_control") == "True") + { + return true; + } else { + return false; + } + } (); + param_vec.push_back( + rclcpp::Parameter{"mode." + joint_name + ".use_cascade_control", + use_cascade_control}); double upper = jointAxis->Data().Upper(); double lower = jointAxis->Data().Lower(); @@ -278,46 +281,46 @@ bool IgnitionSystem::initSim( // PID parameters double p_gain_pos = - (hardware_info.joints[j].parameters.find("p_pos") == - hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos : stod( - hardware_info.joints[j].parameters.at( - "p_pos")); + (hardware_info.joints[j].parameters.find( + "p_pos") == hardware_info.joints[j].parameters.end()) ? + dummy_guess_p_pos : + stod(hardware_info.joints[j].parameters.at("p_pos")); double i_gain_pos = - (hardware_info.joints[j].parameters.find("i_pos") == - hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( - hardware_info.joints[j].parameters.at( - "i_pos")); + (hardware_info.joints[j].parameters.find( + "i_pos") == hardware_info.joints[j].parameters.end()) ? + 0.0 : + stod(hardware_info.joints[j].parameters.at("i_pos")); double d_gain_pos = - (hardware_info.joints[j].parameters.find("d_pos") == - hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos / 100.0 : stod( - hardware_info.joints[j].parameters.at( - "d_pos")); + (hardware_info.joints[j].parameters.find( + "d_pos") == hardware_info.joints[j].parameters.end()) ? + dummy_guess_p_pos / 100.0 : + stod(hardware_info.joints[j].parameters.at("d_pos")); // set integral max and min component to 50 percent of the max effort double iMax_pos = (hardware_info.joints[j].parameters.find("iMax_pos") == - hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( - hardware_info.joints[j].parameters.at( - "iMax_pos")); + hardware_info.joints[j].parameters.end()) ? + 0.0 : + stod(hardware_info.joints[j].parameters.at("iMax_pos")); double iMin_pos = (hardware_info.joints[j].parameters.find("iMin_pos") == - hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( - hardware_info.joints[j].parameters.at( - "iMin_pos")); + hardware_info.joints[j].parameters.end()) ? + 0.0 : + stod(hardware_info.joints[j].parameters.at("iMin_pos")); double cmdMax_pos = (hardware_info.joints[j].parameters.find("cmdMax_pos") == - hardware_info.joints[j].parameters.end() ) ? max_velocity : stod( - hardware_info.joints[j].parameters.at( - "cmdMax_pos")); + hardware_info.joints[j].parameters.end()) ? + max_velocity : + stod(hardware_info.joints[j].parameters.at("cmdMax_pos")); double cmdMin_pos = (hardware_info.joints[j].parameters.find("cmdMin_pos") == - hardware_info.joints[j].parameters.end() ) ? -1.0 * max_velocity : stod( - hardware_info.joints[j].parameters.at( - "cmdMin_pos")); + hardware_info.joints[j].parameters.end()) ? + -1.0 * max_velocity : + stod(hardware_info.joints[j].parameters.at("cmdMin_pos")); double cmdOffset_pos = (hardware_info.joints[j].parameters.find("cmdOffset_pos") == - hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( - hardware_info.joints[j].parameters.at( - "cmdOffset_pos")); + hardware_info.joints[j].parameters.end()) ? + 0.0 : + stod(hardware_info.joints[j].parameters.at("cmdOffset_pos")); param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".p_pos", p_gain_pos}); param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".i_pos", i_gain_pos}); @@ -329,50 +332,50 @@ bool IgnitionSystem::initSim( param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdOffset_pos", cmdOffset_pos}); this->dataPtr->joints_[j].pid_pos.Init( - p_gain_pos, i_gain_pos, d_gain_pos, iMax_pos, iMin_pos, cmdMax_pos, cmdMin_pos, - cmdOffset_pos); + p_gain_pos, i_gain_pos, d_gain_pos, iMax_pos, iMin_pos, cmdMax_pos, + cmdMin_pos, cmdOffset_pos); double p_gain_vel = - (hardware_info.joints[j].parameters.find("p_vel") == - hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos / 100.0 : stod( - hardware_info.joints[j].parameters.at( - "p_vel")); + (hardware_info.joints[j].parameters.find( + "p_vel") == hardware_info.joints[j].parameters.end()) ? + dummy_guess_p_pos / 100.0 : + stod(hardware_info.joints[j].parameters.at("p_vel")); double i_gain_vel = - (hardware_info.joints[j].parameters.find("i_vel") == - hardware_info.joints[j].parameters.end() ) ? dummy_guess_p_pos / 1000.0 : stod( - hardware_info.joints[j].parameters.at( - "i_vel")); + (hardware_info.joints[j].parameters.find( + "i_vel") == hardware_info.joints[j].parameters.end()) ? + dummy_guess_p_pos / 1000.0 : + stod(hardware_info.joints[j].parameters.at("i_vel")); double d_gain_vel = - (hardware_info.joints[j].parameters.find("d_vel") == - hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( - hardware_info.joints[j].parameters.at( - "d_vel")); + (hardware_info.joints[j].parameters.find( + "d_vel") == hardware_info.joints[j].parameters.end()) ? + 0.0 : + stod(hardware_info.joints[j].parameters.at("d_vel")); // set integral max and min component to 50 percent of the max effort double iMax_vel = (hardware_info.joints[j].parameters.find("iMax_vel") == - hardware_info.joints[j].parameters.end() ) ? max_effort / 2.0 : stod( - hardware_info.joints[j].parameters.at( - "iMax_vel")); + hardware_info.joints[j].parameters.end()) ? + max_effort / 2.0 : + stod(hardware_info.joints[j].parameters.at("iMax_vel")); double iMin_vel = (hardware_info.joints[j].parameters.find("iMin_vel") == - hardware_info.joints[j].parameters.end() ) ? -1.0 * max_effort / 2.0 : stod( - hardware_info.joints[j].parameters.at( - "iMin_vel")); + hardware_info.joints[j].parameters.end()) ? + -1.0 * max_effort / 2.0 : + stod(hardware_info.joints[j].parameters.at("iMin_vel")); double cmdMax_vel = (hardware_info.joints[j].parameters.find("cmdMax_vel") == - hardware_info.joints[j].parameters.end() ) ? max_effort : stod( - hardware_info.joints[j].parameters.at( - "cmdMax_vel")); + hardware_info.joints[j].parameters.end()) ? + max_effort : + stod(hardware_info.joints[j].parameters.at("cmdMax_vel")); double cmdMin_vel = (hardware_info.joints[j].parameters.find("cmdMin_vel") == - hardware_info.joints[j].parameters.end() ) ? -1.0 * max_effort : stod( - hardware_info.joints[j].parameters.at( - "cmdMin_vel")); + hardware_info.joints[j].parameters.end()) ? + -1.0 * max_effort : + stod(hardware_info.joints[j].parameters.at("cmdMin_vel")); double cmdOffset_vel = (hardware_info.joints[j].parameters.find("cmdOffset_vel") == - hardware_info.joints[j].parameters.end() ) ? 0.0 : stod( - hardware_info.joints[j].parameters.at( - "cmdOffset_vel")); + hardware_info.joints[j].parameters.end()) ? + 0.0 : + stod(hardware_info.joints[j].parameters.at("cmdOffset_vel")); param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".p_vel", p_gain_vel}); param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".i_vel", i_gain_vel}); @@ -384,8 +387,8 @@ bool IgnitionSystem::initSim( param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdOffset_vel", cmdOffset_vel}); this->dataPtr->joints_[j].pid_vel.Init( - p_gain_vel, i_gain_vel, d_gain_vel, iMax_vel, iMin_vel, cmdMax_vel, cmdMin_vel, - cmdOffset_vel); + p_gain_vel, i_gain_vel, d_gain_vel, iMax_vel, iMin_vel, cmdMax_vel, + cmdMin_vel, cmdOffset_vel); // Accept this joint and continue configuration RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name); @@ -401,8 +404,7 @@ bool IgnitionSystem::initSim( return info.name == mimicked_joint; }); if (mimicked_joint_it == hardware_info.joints.end()) { - throw std::runtime_error( - std::string("Mimicked joint '") + mimicked_joint + "' not found"); + throw std::runtime_error(std::string("Mimicked joint '") + mimicked_joint + "' not found"); } MimicJoint mimic_joint; @@ -418,21 +420,28 @@ bool IgnitionSystem::initSim( mimic_joint.mimic_ff_force_scaling = (hardware_info.joints[j].parameters.find("mimic_ff_force_scaling") == - hardware_info.joints[j].parameters.end() ) ? mimic_joint.mimic_ff_force_scaling : stod( - hardware_info.joints[j].parameters.at( - "mimic_ff_force_scaling")); + hardware_info.joints[j].parameters.end()) ? + mimic_joint.mimic_ff_force_scaling : + stod(hardware_info.joints[j].parameters.at("mimic_ff_force_scaling")); // check joint info of mimicked joint auto & joint_info_mimicked = hardware_info.joints[mimic_joint.mimicked_joint_index]; - const auto state_mimicked_interface = std::find_if( + const auto state_mimicked_interface = + std::find_if( joint_info_mimicked.state_interfaces.begin(), joint_info_mimicked.state_interfaces.end(), [&mimic_joint](const hardware_interface::InterfaceInfo & interface_info) { bool pos = interface_info.name == "position"; - if (pos) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_POSITION);} + if (pos) { + mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_POSITION); + } bool vel = interface_info.name == "velocity"; - if (vel) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_VELOCITY);} + if (vel) { + mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_VELOCITY); + } bool eff = interface_info.name == "effort"; - if (vel) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_EFFORT);} + if (vel) { + mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_EFFORT); + } return pos || vel || eff; }); if (state_mimicked_interface == joint_info_mimicked.state_interfaces.end()) { @@ -443,9 +452,8 @@ bool IgnitionSystem::initSim( " ' to mimic"); } RCLCPP_INFO_STREAM( - this->nh_->get_logger(), - "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint << "' with mutiplier: " - << mimic_joint.multiplier); + this->nh_->get_logger(), "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint + << "' with mutiplier: " << mimic_joint.multiplier); this->dataPtr->mimic_joints_.push_back(mimic_joint); } @@ -470,8 +478,7 @@ bool IgnitionSystem::initSim( if (joint_info.state_interfaces[i].name == "position") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); this->dataPtr->state_interfaces_.emplace_back( - joint_name + suffix, - hardware_interface::HW_IF_POSITION, + joint_name + suffix, hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position); initial_position = get_initial_value(joint_info.state_interfaces[i]); this->dataPtr->joints_[j].joint_position = initial_position; @@ -479,8 +486,7 @@ bool IgnitionSystem::initSim( if (joint_info.state_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); this->dataPtr->state_interfaces_.emplace_back( - joint_name + suffix, - hardware_interface::HW_IF_VELOCITY, + joint_name + suffix, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity); initial_velocity = get_initial_value(joint_info.state_interfaces[i]); this->dataPtr->joints_[j].joint_velocity = initial_velocity; @@ -488,8 +494,7 @@ bool IgnitionSystem::initSim( if (joint_info.state_interfaces[i].name == "effort") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); this->dataPtr->state_interfaces_.emplace_back( - joint_name + suffix, - hardware_interface::HW_IF_EFFORT, + joint_name + suffix, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort); initial_effort = get_initial_value(joint_info.state_interfaces[i]); this->dataPtr->joints_[j].joint_effort = initial_effort; @@ -503,8 +508,7 @@ bool IgnitionSystem::initSim( if (joint_info.command_interfaces[i].name == "position") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); this->dataPtr->command_interfaces_.emplace_back( - joint_name + suffix, - hardware_interface::HW_IF_POSITION, + joint_name + suffix, hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position_cmd); if (!std::isnan(initial_position)) { this->dataPtr->joints_[j].joint_position_cmd = initial_position; @@ -512,8 +516,7 @@ bool IgnitionSystem::initSim( } else if (joint_info.command_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); this->dataPtr->command_interfaces_.emplace_back( - joint_name + suffix, - hardware_interface::HW_IF_VELOCITY, + joint_name + suffix, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity_cmd); if (!std::isnan(initial_velocity)) { this->dataPtr->joints_[j].joint_velocity_cmd = initial_velocity; @@ -522,8 +525,7 @@ bool IgnitionSystem::initSim( this->dataPtr->joints_[j].joint_control_method |= EFFORT; RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); this->dataPtr->command_interfaces_.emplace_back( - joint_name + suffix, - hardware_interface::HW_IF_EFFORT, + joint_name + suffix, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort_cmd); if (!std::isnan(initial_effort)) { this->dataPtr->joints_[j].joint_effort_cmd = initial_effort; @@ -579,8 +581,7 @@ bool IgnitionSystem::initSim( return true; } -void IgnitionSystem::registerSensors( - const hardware_interface::HardwareInfo & hardware_info) +void IgnitionSystem::registerSensors(const hardware_interface::HardwareInfo & hardware_info) { // Collect gazebo sensor handles size_t n_sensors = hardware_info.sensors.size(); @@ -594,23 +595,21 @@ void IgnitionSystem::registerSensors( // So we have resize only once the structures where the data will be stored, and we can safely // use pointers to the structures - this->dataPtr->ecm->Each( - [&](const ignition::gazebo::Entity & _entity, - const ignition::gazebo::components::Imu *, - const ignition::gazebo::components::Name * _name) -> bool - { + this->dataPtr->ecm->Each( + [&](const ignition::gazebo::Entity & _entity, const ignition::gazebo::components::Imu *, + const ignition::gazebo::components::Name * _name) -> bool { auto imuData = std::make_shared(); - RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data()); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), + "Loading sensor: " << _name->Data()); - auto sensorTopicComp = this->dataPtr->ecm->Component< - ignition::gazebo::components::SensorTopic>(_entity); + auto sensorTopicComp = this->dataPtr->ecm->Component( + _entity); if (sensorTopicComp) { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); } - RCLCPP_INFO_STREAM( - this->nh_->get_logger(), "\tState:"); + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); imuData->name = _name->Data(); imuData->sim_imu_sensors_ = _entity; @@ -622,15 +621,9 @@ void IgnitionSystem::registerSensors( } static const std::map interface_name_map = { - {"orientation.x", 0}, - {"orientation.y", 1}, - {"orientation.z", 2}, - {"orientation.w", 3}, - {"angular_velocity.x", 4}, - {"angular_velocity.y", 5}, - {"angular_velocity.z", 6}, - {"linear_acceleration.x", 7}, - {"linear_acceleration.y", 8}, + {"orientation.x", 0}, {"orientation.y", 1}, {"orientation.z", 2}, + {"orientation.w", 3}, {"angular_velocity.x", 4}, {"angular_velocity.y", 5}, + {"angular_velocity.z", 6}, {"linear_acceleration.x", 7}, {"linear_acceleration.y", 8}, {"linear_acceleration.z", 9}, }; @@ -639,8 +632,7 @@ void IgnitionSystem::registerSensors( size_t data_index = interface_name_map.at(state_interface.name); this->dataPtr->state_interfaces_.emplace_back( - imuData->name, - state_interface.name, + imuData->name, state_interface.name, &imuData->imu_sensor_data_[data_index]); } this->dataPtr->imus_.push_back(imuData); @@ -648,8 +640,7 @@ void IgnitionSystem::registerSensors( }); } -CallbackReturn -IgnitionSystem::on_init(const hardware_interface::HardwareInfo & system_info) +CallbackReturn IgnitionSystem::on_init(const hardware_interface::HardwareInfo & system_info) { RCLCPP_WARN(this->nh_->get_logger(), "On init..."); @@ -659,23 +650,19 @@ IgnitionSystem::on_init(const hardware_interface::HardwareInfo & system_info) return CallbackReturn::SUCCESS; } -CallbackReturn IgnitionSystem::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn IgnitionSystem::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO( - this->nh_->get_logger(), "System Successfully configured!"); + RCLCPP_INFO(this->nh_->get_logger(), "System Successfully configured!"); return CallbackReturn::SUCCESS; } -std::vector -IgnitionSystem::export_state_interfaces() +std::vector IgnitionSystem::export_state_interfaces() { return std::move(this->dataPtr->state_interfaces_); } -std::vector -IgnitionSystem::export_command_interfaces() +std::vector IgnitionSystem::export_command_interfaces() { return std::move(this->dataPtr->command_interfaces_); } @@ -723,13 +710,15 @@ hardware_interface::return_type IgnitionSystem::read( for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { if (this->dataPtr->imus_[i]->topicName.empty()) { - auto sensorTopicComp = this->dataPtr->ecm->Component< - ignition::gazebo::components::SensorTopic>(this->dataPtr->imus_[i]->sim_imu_sensors_); + auto sensorTopicComp = + this->dataPtr->ecm->Component( + this->dataPtr->imus_[i]->sim_imu_sensors_); if (sensorTopicComp) { this->dataPtr->imus_[i]->topicName = sensorTopicComp->Data(); RCLCPP_INFO_STREAM( - this->nh_->get_logger(), "IMU " << this->dataPtr->imus_[i]->name << - " has a topic name: " << sensorTopicComp->Data()); + this->nh_->get_logger(), + "IMU " << this->dataPtr->imus_[i]->name << " has a topic name: " << + sensorTopicComp->Data()); this->dataPtr->node.Subscribe( this->dataPtr->imus_[i]->topicName, &ImuData::OnIMU, @@ -748,17 +737,17 @@ IgnitionSystem::perform_command_mode_switch( 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 + "/" + - hardware_interface::HW_IF_POSITION)) + if (interface_name == + (this->dataPtr->joints_[j].name + "/" + hardware_interface::HW_IF_POSITION)) { this->dataPtr->joints_[j].joint_control_method &= static_cast(VELOCITY & EFFORT); - } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT hardware_interface::HW_IF_VELOCITY)) { this->dataPtr->joints_[j].joint_control_method &= static_cast(POSITION & EFFORT); - } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT hardware_interface::HW_IF_EFFORT)) { this->dataPtr->joints_[j].joint_control_method &= @@ -768,15 +757,15 @@ IgnitionSystem::perform_command_mode_switch( // Set joint control method bits corresponding to start interfaces for (const std::string & interface_name : start_interfaces) { - if (interface_name == (this->dataPtr->joints_[j].name + "/" + - hardware_interface::HW_IF_POSITION)) + if (interface_name == + (this->dataPtr->joints_[j].name + "/" + hardware_interface::HW_IF_POSITION)) { this->dataPtr->joints_[j].joint_control_method |= POSITION; - } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT hardware_interface::HW_IF_VELOCITY)) { this->dataPtr->joints_[j].joint_control_method |= VELOCITY; - } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT hardware_interface::HW_IF_EFFORT)) { this->dataPtr->joints_[j].joint_control_method |= EFFORT; @@ -796,19 +785,22 @@ hardware_interface::return_type IgnitionSystem::write( params_ = param_listener_->get_params(); for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - // assuming every joint has axis const auto * jointAxis = this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + this->dataPtr->joints_[ + i].sim_joint); // update PIDs this->dataPtr->joints_[i].pid_pos.SetPGain( - params_.gains.joints_map[this->dataPtr->joints_[i].name].p_pos); + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].p_pos); this->dataPtr->joints_[i].pid_pos.SetIGain( - params_.gains.joints_map[this->dataPtr->joints_[i].name].i_pos); + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].i_pos); this->dataPtr->joints_[i].pid_pos.SetDGain( - params_.gains.joints_map[this->dataPtr->joints_[i].name].d_pos); + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].d_pos); this->dataPtr->joints_[i].pid_pos.SetIMax( params_.gains.joints_map[this->dataPtr->joints_[i]. name].iMax_pos); @@ -822,15 +814,17 @@ hardware_interface::return_type IgnitionSystem::write( params_.gains.joints_map[this->dataPtr->joints_[i]. name].cmdMin_pos); this->dataPtr->joints_[i].pid_pos.SetCmdOffset( - params_.gains.joints_map[this->dataPtr->joints_[i]. - name].cmdOffset_pos); + params_.gains.joints_map[this->dataPtr->joints_[i].name].cmdOffset_pos); this->dataPtr->joints_[i].pid_vel.SetPGain( - params_.gains.joints_map[this->dataPtr->joints_[i].name].p_vel); + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].p_vel); this->dataPtr->joints_[i].pid_vel.SetIGain( - params_.gains.joints_map[this->dataPtr->joints_[i].name].i_vel); + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].i_vel); this->dataPtr->joints_[i].pid_vel.SetDGain( - params_.gains.joints_map[this->dataPtr->joints_[i].name].d_vel); + params_.gains.joints_map[this->dataPtr->joints_[i]. + name].d_vel); this->dataPtr->joints_[i].pid_vel.SetIMax( params_.gains.joints_map[this->dataPtr->joints_[i]. name].iMax_vel); @@ -844,28 +838,25 @@ hardware_interface::return_type IgnitionSystem::write( params_.gains.joints_map[this->dataPtr->joints_[i]. name].cmdMin_vel); this->dataPtr->joints_[i].pid_vel.SetCmdOffset( - params_.gains.joints_map[this->dataPtr->joints_[i]. - name].cmdOffset_vel); + params_.gains.joints_map[this->dataPtr->joints_[i].name].cmdOffset_vel); if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { - double velocity = this->dataPtr->joints_[i].joint_velocity; double velocity_cmd_clamped = std::clamp( - this->dataPtr->joints_[i].joint_velocity_cmd, -1.0 * jointAxis->Data().MaxVelocity(), - jointAxis->Data().MaxVelocity()); + this->dataPtr->joints_[i].joint_velocity_cmd, + -1.0 * jointAxis->Data().MaxVelocity(), jointAxis->Data().MaxVelocity()); double velocity_error = velocity - velocity_cmd_clamped; // calculate target force/torque - output of inner pid double target_force = this->dataPtr->joints_[i].pid_vel.Update( - velocity_error, std::chrono::duration( - period.to_chrono())); + velocity_error, + std::chrono::duration(period.to_chrono())); // remember for potential effort state interface this->dataPtr->joints_[i].joint_effort_cmd = target_force; - auto forceCmd = - this->dataPtr->ecm->Component( + auto forceCmd = this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); if (forceCmd == nullptr) { @@ -873,65 +864,61 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->joints_[i].sim_joint, ignition::gazebo::components::JointForceCmd({target_force})); } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); + *forceCmd = ignition::gazebo::components::JointForceCmd({target_force}); } - } else if (this->dataPtr->joints_[i].joint_control_method & POSITION) { - // calculate error with clamped position command double position = this->dataPtr->joints_[i].joint_position; double position_cmd_clamped = std::clamp( - this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), - jointAxis->Data().Upper()); + this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(), + jointAxis->Data().Upper()); double position_error = position - position_cmd_clamped; - double position_error_sign = copysign( - 1.0, - position_error); + double position_error_sign = copysign(1.0, position_error); - double position_error_abs_clamped = std::clamp( - std::abs(position_error), 0.0, - std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); + double position_error_abs_clamped = + std::clamp( + std::abs(position_error), 0.0, + std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); - // move forward with calculated position error + // move forward with calculated position error position_error = position_error_sign * position_error_abs_clamped; double position_or_velocity_error = 0.0; // check if cascade control is used for this joint - if( params_.mode.joints_map[this->dataPtr->joints_[i]. - name].use_cascade_control) - { + if (params_.mode.joints_map[this->dataPtr->joints_[i].name].use_cascade_control) { // calculate target velocity - output of outer pid - input to inner pid double target_vel = this->dataPtr->joints_[i].pid_pos.Update( - position_error, std::chrono::duration(period.to_chrono())); + position_error, std::chrono::duration( + period.to_chrono())); double velocity_error = - this->dataPtr->joints_[i].joint_velocity - - std::clamp(target_vel, -1.0 * jointAxis->Data().MaxVelocity(), jointAxis->Data().MaxVelocity()); + this->dataPtr->joints_[i].joint_velocity - + std::clamp( + target_vel, -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); // prepare velocity error value for inner pid position_or_velocity_error = velocity_error; - }else{ + } else { // prepare velocity error value for inner pid position_or_velocity_error = position_error; } // calculate target force/torque - output of inner pid double target_force = this->dataPtr->joints_[i].pid_vel.Update( - position_or_velocity_error, std::chrono::duration( - period.to_chrono())); + position_or_velocity_error, + std::chrono::duration(period.to_chrono())); // round the force - target_force = round(target_force * 10000.0)/10000.0; + target_force = round(target_force * 10000.0) / 10000.0; - // remember for potential effort state interface + // remember for potential effort state interface this->dataPtr->joints_[i].joint_effort_cmd = target_force; - auto forceCmd = - this->dataPtr->ecm->Component( + auto forceCmd = this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); if (forceCmd == nullptr) { @@ -939,8 +926,7 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->joints_[i].sim_joint, ignition::gazebo::components::JointForceCmd({target_force})); } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); + *forceCmd = ignition::gazebo::components::JointForceCmd({target_force}); } } else if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { if (!this->dataPtr->ecm->Component( @@ -956,162 +942,168 @@ hardware_interface::return_type IgnitionSystem::write( *jointEffortCmd = ignition::gazebo::components::JointForceCmd( {this->dataPtr->joints_[i].joint_effort_cmd}); } - } + } - // set values of all mimic joints with respect to mimicked joint - for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { - // assuming every mimic joint has axis - const auto * jointAxis = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + // set values of all mimic joints with respect to mimicked joint + for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { + for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { + // assuming every mimic joint has axis + const auto * jointAxis = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - if (mimic_interface == "position") { - // Get the joint position - double position_mimicked_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; + if (mimic_interface == "position") { + // Get the joint position + double position_mimicked_joint = this->dataPtr->ecm + ->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint) + ->Data()[0]; - double position_mimic_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + double position_mimic_joint = this->dataPtr->ecm + ->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint) + ->Data()[0]; - double position_target_from_mimicked_joint = std::clamp( - position_mimicked_joint * - mimic_joint.multiplier, jointAxis->Data().Lower(), - jointAxis->Data().Upper()); + double position_target_from_mimicked_joint = std::clamp( + position_mimicked_joint * mimic_joint.multiplier, + jointAxis->Data().Lower(), jointAxis->Data().Upper()); - double position_error = position_mimic_joint - position_target_from_mimicked_joint; + double position_error = position_mimic_joint - position_target_from_mimicked_joint; - // round position error for simulation stability - position_error = round(position_error * 10000.0)/10000.0; + // round position error for simulation stability + position_error = round(position_error * 10000.0) / 10000.0; - double position_error_sign = copysign( - 1.0, - position_error); + double position_error_sign = copysign(1.0, position_error); - double position_error_abs_clamped = std::clamp( + double position_error_abs_clamped = std::clamp( std::abs(position_error), 0.0, std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower())); - position_error = position_error_sign * position_error_abs_clamped; - - double position_or_velocity_error = 0.0; + position_error = position_error_sign * position_error_abs_clamped; + + double position_or_velocity_error = 0.0; + + // check if cascade control is used for this joint + if (params_.mode.joints_map[this->dataPtr->joints_[mimic_joint.joint_index].name]. + use_cascade_control) + { + // calculate target velocity - output of outer pid - input to inner pid + double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update( + position_error, + std::chrono::duration(period.to_chrono())); + + // get mimic joint velocity + double velocity_mimic_joint = this->dataPtr->ecm + ->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint) + ->Data()[0]; + + position_or_velocity_error = + velocity_mimic_joint - + std::clamp( + target_vel, -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); + } else { + position_or_velocity_error = position_error; + } + + // set command offset - feed forward term added to the pid output that is clamped by pid max command value + // while taking into account mimic multiplier + this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( + mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd); + + // calculate target force/torque - output of inner pid + double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( + position_or_velocity_error, + std::chrono::duration(period.to_chrono())); + + // round force value for simulation stability + target_force = round(target_force * 10000.0) / 10000.0; + + // remember for potential effort state interface + this->dataPtr->joints_[mimic_joint.joint_index].joint_effort_cmd = target_force; + + auto forceCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - // check if cascade control is used for this joint - if( params_.mode.joints_map[this->dataPtr->joints_[mimic_joint.joint_index]. - name].use_cascade_control) - { - // calculate target velocity - output of outer pid - input to inner pid - double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update( - position_error, std::chrono::duration(period.to_chrono())); + if (forceCmd == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({target_force})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd({target_force}); + } + } + if (mimic_interface == "velocity") { + // get the velocity of mimicked joint + double velocity_mimicked_joint = this->dataPtr->ecm + ->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint) + ->Data()[0]; // get mimic joint velocity double velocity_mimic_joint = this->dataPtr->ecm - ->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint) - ->Data()[0]; - - position_or_velocity_error = velocity_mimic_joint - std::clamp(target_vel, -1.0 * jointAxis->Data().MaxVelocity(), - jointAxis->Data().MaxVelocity()); - }else{ - position_or_velocity_error = position_error; - } - - // set command offset - feed forward term added to the pid output that is clamped by pid max command value - // while taking into account mimic multiplier - this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( - mimic_joint.multiplier * - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd); - - // calculate target force/torque - output of inner pid - double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( - position_or_velocity_error, std::chrono::duration( - period.to_chrono())); + ->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint) + ->Data()[0]; - // round force value for simulation stability - target_force = round(target_force * 10000.0)/10000.0; + double velocity_error = velocity_mimic_joint - std::clamp( + mimic_joint.multiplier * velocity_mimicked_joint, + -1.0 * jointAxis->Data().MaxVelocity(), + jointAxis->Data().MaxVelocity()); - // remember for potential effort state interface - this->dataPtr->joints_[mimic_joint.joint_index].joint_effort_cmd = target_force; - - auto forceCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - - if (forceCmd == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointForceCmd({target_force})); - } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); - } - } - if (mimic_interface == "velocity") { - // get the velocity of mimicked joint - double velocity_mimicked_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; - - // get mimic joint velocity - double velocity_mimic_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; - - double velocity_error = velocity_mimic_joint - std::clamp( - mimic_joint.multiplier * velocity_mimicked_joint, -1.0 * jointAxis->Data().MaxVelocity(), - jointAxis->Data().MaxVelocity()); + this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( + mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd); - this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( - mimic_joint.multiplier * - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd); + // calculate target force/torque - output of inner pid + double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( + velocity_error, + std::chrono::duration(period.to_chrono())); - // calculate target force/torque - output of inner pid - double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update( - velocity_error, std::chrono::duration( - period.to_chrono())); - - auto forceCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - - if (forceCmd == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointForceCmd({target_force})); - } else { - *forceCmd = ignition::gazebo::components::JointForceCmd( - {target_force}); - } - } - if (mimic_interface == "effort") { - // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 - // Get the joint force - // const auto * jointForce = - // _ecm.Component( - // this->dataPtr->sim_joints_[j]); - if (!this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) - { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointForceCmd({0})); - } else { - const auto jointEffortCmd = + auto forceCmd = this->dataPtr->ecm->Component( this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - *jointEffortCmd = ignition::gazebo::components::JointForceCmd( - {mimic_joint.multiplier * - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd}); + + if (forceCmd == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({target_force})); + } else { + *forceCmd = ignition::gazebo::components::JointForceCmd({target_force}); + } + } + if (mimic_interface == "effort") { + // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 + // Get the joint force + // const auto * jointForce = + // _ecm.Component( + // this->dataPtr->sim_joints_[j]); + if (!this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) + { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointForceCmd({0})); + } else { + const auto jointEffortCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointEffortCmd = ignition::gazebo::components::JointForceCmd( + {mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd}); + } } } } - } - return hardware_interface::return_type::OK; + return hardware_interface::return_type::OK; + } } -} // namespace ign_ros2_control + +} // namespace ign_ros2_control #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS( From 64f6b6755ae216304a9ea8b9acfcf7c61719152e Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 24 Mar 2023 17:50:59 +0100 Subject: [PATCH 20/24] Adapt pid values for simple position example. --- ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf index c621f01b..5a818074 100644 --- a/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -57,8 +57,11 @@ - 10.0 - 100.0 + 1000.0 + 100.0 + 100.0 + 5.0 + -5.0 From 6c5ecefc02cd7b0b1eaf6700284f4bd0506ba461 Mon Sep 17 00:00:00 2001 From: Lovro Date: Mon, 27 Mar 2023 10:33:21 +0200 Subject: [PATCH 21/24] Suggestions and fixes for humble CI. write method return value handling. --- ign_ros2_control/include/ign_ros2_control/ign_system.hpp | 2 +- ign_ros2_control/src/MimicJointSystem.cc | 5 +++-- ign_ros2_control/src/ign_system.cpp | 9 ++++----- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp index 9394f502..1949f77f 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp +++ b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp @@ -22,12 +22,12 @@ #include #include "ign_ros2_control/ign_system_interface.hpp" -#include "ign_ros2_control_parameters.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ign_ros2_control_parameters.hpp" namespace ign_ros2_control { diff --git a/ign_ros2_control/src/MimicJointSystem.cc b/ign_ros2_control/src/MimicJointSystem.cc index e72a34c2..56b083bf 100644 --- a/ign_ros2_control/src/MimicJointSystem.cc +++ b/ign_ros2_control/src/MimicJointSystem.cc @@ -371,6 +371,9 @@ void MimicJointSystem::PreUpdate( return; } + // If the mode is not ABS, default mode is PID + // (e.g.) MimicJointSystemPrivate::OperationMode::PID + // Update force command. double force = this->dataPtr->posPid.Update(error, _info.dt); @@ -391,14 +394,12 @@ void MimicJointSystem::Update( const ignition::gazebo::UpdateInfo & /*_info*/, ignition::gazebo::EntityComponentManager & /*_ecm*/) { -// ignmsg << "MimicJointSystem::Update" << std::endl; } void MimicJointSystem::PostUpdate( const ignition::gazebo::UpdateInfo & /*_info*/, const ignition::gazebo::EntityComponentManager & /*_ecm*/) { -// ignmsg << "MimicJointSystem::PostUpdate" << std::endl; } //! [registerMimicJointSystem] diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 2c78b31e..be6b8733 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -43,7 +43,7 @@ #include // pid_pos stuff -#include +#include #include @@ -79,10 +79,10 @@ struct jointData ignition::gazebo::Entity sim_joint; /// \brief PID for position control - gz::math::PID pid_pos; + ignition::math::PID pid_pos; /// \brief PID for velocity control - gz::math::PID pid_vel; + ignition::math::PID pid_vel; /// \brief Control method defined in the URDF for each joint. ign_ros2_control::IgnitionSystemInterface::ControlMethod joint_control_method; @@ -1098,9 +1098,8 @@ hardware_interface::return_type IgnitionSystem::write( } } } - - return hardware_interface::return_type::OK; } + return hardware_interface::return_type::OK; } } // namespace ign_ros2_control From a100537a775d82a1d11e0a8a91280c5df755bbcf Mon Sep 17 00:00:00 2001 From: Lovro Date: Mon, 27 Mar 2023 11:16:11 +0200 Subject: [PATCH 22/24] Satisfy ament tools. --- .../ign_ros2_control/MimicJointSystem.hh | 99 ++-- ign_ros2_control/src/MimicJointSystem.cc | 534 +++++++++--------- ign_ros2_control/src/ign_system.cpp | 19 +- 3 files changed, 326 insertions(+), 326 deletions(-) diff --git a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh index 0fcf61ba..afd6f57a 100644 --- a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh +++ b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh @@ -1,19 +1,16 @@ -/* - * Copyright 2023 Open Source Robotics Foundation, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. //---------------------------------------------------------------------- /*!\file @@ -24,8 +21,8 @@ */ //---------------------------------------------------------------------- -#ifndef IGNITION_GAZEBO_SYSTEMS_MIMICJOINTSYSTEM_HH_ -#define IGNITION_GAZEBO_SYSTEMS_MIMICJOINTSYSTEM_HH_ +#ifndef IGN_ROS2_CONTROL__MIMICJOINTSYSTEM_HH_ +#define IGN_ROS2_CONTROL__MIMICJOINTSYSTEM_HH_ //! [header] #include @@ -54,40 +51,52 @@ namespace ign_ros2_control { - class MimicJointSystemPrivate; +class MimicJointSystemPrivate; - class MimicJointSystem: - // This class is a system. - public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - // This class also implements the ISystemPreUpdate, ISystemUpdate, - // and ISystemPostUpdate interfaces. - public ignition::gazebo::ISystemPreUpdate, - public ignition::gazebo::ISystemUpdate, - public ignition::gazebo::ISystemPostUpdate - { - public: MimicJointSystem(); +class MimicJointSystem: +// This class is a system. +public ignition::gazebo::System, +public ignition::gazebo::ISystemConfigure, +// This class also implements the ISystemPreUpdate, ISystemUpdate, +// and ISystemPostUpdate interfaces. +public ignition::gazebo::ISystemPreUpdate, +public ignition::gazebo::ISystemUpdate, +public ignition::gazebo::ISystemPostUpdate +{ + public: + MimicJointSystem(); + + // Documentation inherited - // Documentation inherited - public: void Configure(const ignition::gazebo::Entity &_entity, - const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &_eventMgr) override; + public: + void Configure( + const ignition::gazebo::Entity & _entity, + const std::shared_ptr < const sdf::Element > & _sdf, + ignition::gazebo::EntityComponentManager & _ecm, + ignition::gazebo::EventManager & _eventMgr) override; - public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; + public: + void PreUpdate( + const ignition::gazebo::UpdateInfo & _info, + ignition::gazebo::EntityComponentManager & _ecgm) override; - public: void Update(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; + public: + void Update( + const ignition::gazebo::UpdateInfo & _info, + ignition::gazebo::EntityComponentManager & _ecm) override; - public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) override; + public: + void PostUpdate( + const ignition::gazebo::UpdateInfo & _info, + const ignition::gazebo::EntityComponentManager & _ecm) override; private: /// \brief Private data pointer - private: std::unique_ptr dataPtr; - }; -} + + private: + std::unique_ptr < MimicJointSystemPrivate > dataPtr; +}; +} // namespace ign_ros2_control //! [header] -#endif +#endif // IGN_ROS2_CONTROL__MIMICJOINTSYSTEM_HH_ diff --git a/ign_ros2_control/src/MimicJointSystem.cc b/ign_ros2_control/src/MimicJointSystem.cc index 56b083bf..ca3e9903 100644 --- a/ign_ros2_control/src/MimicJointSystem.cc +++ b/ign_ros2_control/src/MimicJointSystem.cc @@ -1,19 +1,16 @@ -/* - * Copyright 2023 Open Source Robotics Foundation, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. //---------------------------------------------------------------------- /*!\file @@ -42,10 +39,9 @@ #include "ignition/gazebo/Model.hh" -using namespace ign_ros2_control; - -class ign_ros2_control::MimicJointSystemPrivate { +class ign_ros2_control::MimicJointSystemPrivate +{ /// \brief Joint Entity public: @@ -73,17 +69,17 @@ class ign_ros2_control::MimicJointSystemPrivate { public: double offset {0.0}; -/// \brief Model interface + /// \brief Model interface public: ignition::gazebo::Model model {ignition::gazebo::kNullEntity}; -/// \brief Position PID controller. + /// \brief Position PID controller. public: ignition::math::PID posPid; -/// \brief Joint index to be used. + /// \brief Joint index to be used. public: unsigned int jointIndex = 0u; @@ -95,7 +91,7 @@ class ign_ros2_control::MimicJointSystemPrivate { double deadZone = 1e-6; // approach adopted from https://github.com/gazebosim/gz-sim/blob/330eaf2f135301e90096fe91897f052cdaa77013/src/systems/joint_position_controller/JointPositionController.cc#L69-L77 -/// \brief Operation modes + /// \brief Operation modes enum OperationMode { /// \brief Use PID to achieve positional control @@ -105,310 +101,302 @@ class ign_ros2_control::MimicJointSystemPrivate { ABS }; -/// \brief Joint position mode + /// \brief Joint position mode public: OperationMode mode = OperationMode::PID; - }; -MimicJointSystem::MimicJointSystem() -: dataPtr(std::make_unique < ign_ros2_control::MimicJointSystemPrivate > ()) +namespace ign_ros2_control { -} -void MimicJointSystem::Configure( - const ignition::gazebo::Entity & _entity, - const std::shared_ptr < const sdf::Element > & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & /* _eventMgr*/) -{ - - // Make sure the controller is attached to a valid model - this->dataPtr->model = ignition::gazebo::Model(_entity); - if (!this->dataPtr->model.Valid(_ecm)) { - - ignerr << "MimicJointSystem Failed to initialize because [" << - this->dataPtr->model.Name(_ecm) << - "] (Entity=" << _entity << ")] is not a model.. "; - return; + MimicJointSystem::MimicJointSystem() + : dataPtr(std::make_unique < ign_ros2_control::MimicJointSystemPrivate > ()) + { } - // Get params from SDF - this->dataPtr->jointName = _sdf->Get < std::string > ("joint_name"); - - if (this->dataPtr->jointName.empty()) { - ignerr << "MimicJointSystem found an empty joint_name parameter. " - << "Failed to initialize."; - return; - } + void MimicJointSystem::Configure( + const ignition::gazebo::Entity & _entity, + const std::shared_ptr < const sdf::Element > & _sdf, + ignition::gazebo::EntityComponentManager & _ecm, + ignition::gazebo::EventManager & /* _eventMgr*/) + { + // Make sure the controller is attached to a valid model + this->dataPtr->model = ignition::gazebo::Model(_entity); + if (!this->dataPtr->model.Valid(_ecm)) { + ignerr << "MimicJointSystem Failed to initialize because [" << + this->dataPtr->model.Name(_ecm) + << "] (Entity=" << _entity << ")] is not a model.. "; + return; + } - this->dataPtr->mimicJointName = _sdf->Get < std::string > ("mimic_joint_name"); - if (this->dataPtr->mimicJointName.empty()) { - ignerr << "MimicJointSystem found an empty mimic_joint_name parameter. " - << "Failed to initialize."; - return; - } + // Get params from SDF + this->dataPtr->jointName = _sdf->Get < std::string > ("joint_name"); - if (_sdf->HasElement("multiplier")) { - this->dataPtr->multiplier = _sdf->Get < double > ("multiplier"); - } + if (this->dataPtr->jointName.empty()) { + ignerr << "MimicJointSystem found an empty joint_name parameter. " + << "Failed to initialize."; + return; + } - if (_sdf->HasElement("offset")) { - this->dataPtr->offset = _sdf->Get < double > ("offset"); - } + this->dataPtr->mimicJointName = _sdf->Get < std::string > ("mimic_joint_name"); + if (this->dataPtr->mimicJointName.empty()) { + ignerr << "MimicJointSystem found an empty mimic_joint_name parameter. " + << "Failed to initialize."; + return; + } - if (_sdf->HasElement("joint_index")) { - this->dataPtr->jointIndex = _sdf->Get < unsigned int > ("joint_index"); - } + if (_sdf->HasElement("multiplier")) { + this->dataPtr->multiplier = _sdf->Get < double > ("multiplier"); + } - if (_sdf->HasElement("mimic_joint_index")) { - this->dataPtr->mimicJointIndex = _sdf->Get < unsigned int > ("mimic_joint_index"); - } + if (_sdf->HasElement("offset")) { + this->dataPtr->offset = _sdf->Get < double > ("offset"); + } - if (_sdf->HasElement("dead_zone")) { - this->dataPtr->deadZone = _sdf->Get < double > ("dead_zone"); - } + if (_sdf->HasElement("joint_index")) { + this->dataPtr->jointIndex = _sdf->Get < unsigned int > ("joint_index"); + } - // PID parameters - double p = 1; - double i = 0.1; - double d = 0.01; - double iMax = 1; - double iMin = -1; - double cmdMax = 1000; - double cmdMin = -1000; - double cmdOffset = 0; - - if (_sdf->HasElement("p_gain")) { - p = _sdf->Get < double > ("p_gain"); - } - if (_sdf->HasElement("i_gain")) { - i = _sdf->Get < double > ("i_gain"); - } - if (_sdf->HasElement("d_gain")) { - d = _sdf->Get < double > ("d_gain"); - } - if (_sdf->HasElement("i_max")) { - iMax = _sdf->Get < double > ("i_max"); - } - if (_sdf->HasElement("i_min")) { - iMin = _sdf->Get < double > ("i_min"); - } - if (_sdf->HasElement("cmd_max")) { - cmdMax = _sdf->Get < double > ("cmd_max"); - } - if (_sdf->HasElement("cmd_min")) { - cmdMin = _sdf->Get < double > ("cmd_min"); - } - if (_sdf->HasElement("cmd_offset")) { - cmdOffset = _sdf->Get < double > ("cmd_offset"); - } - if (_sdf->HasElement("use_velocity_commands")) { - auto useVelocityCommands = _sdf->Get < bool > ("use_velocity_commands"); - if (useVelocityCommands) { - this->dataPtr->mode = - MimicJointSystemPrivate::OperationMode::ABS; + if (_sdf->HasElement("mimic_joint_index")) { + this->dataPtr->mimicJointIndex = _sdf->Get < unsigned int > ("mimic_joint_index"); } - } - this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset); + if (_sdf->HasElement("dead_zone")) { + this->dataPtr->deadZone = _sdf->Get < double > ("dead_zone"); + } + // PID parameters + double p = 1; + double i = 0.1; + double d = 0.01; + double iMax = 1; + double iMin = -1; + double cmdMax = 1000; + double cmdMin = -1000; + double cmdOffset = 0; + + if (_sdf->HasElement("p_gain")) { + p = _sdf->Get < double > ("p_gain"); + } + if (_sdf->HasElement("i_gain")) { + i = _sdf->Get < double > ("i_gain"); + } + if (_sdf->HasElement("d_gain")) { + d = _sdf->Get < double > ("d_gain"); + } + if (_sdf->HasElement("i_max")) { + iMax = _sdf->Get < double > ("i_max"); + } + if (_sdf->HasElement("i_min")) { + iMin = _sdf->Get < double > ("i_min"); + } + if (_sdf->HasElement("cmd_max")) { + cmdMax = _sdf->Get < double > ("cmd_max"); + } + if (_sdf->HasElement("cmd_min")) { + cmdMin = _sdf->Get < double > ("cmd_min"); + } + if (_sdf->HasElement("cmd_offset")) { + cmdOffset = _sdf->Get < double > ("cmd_offset"); + } + if (_sdf->HasElement("use_velocity_commands")) { + auto useVelocityCommands = _sdf->Get < bool > ("use_velocity_commands"); + if (useVelocityCommands) { + this->dataPtr->mode = MimicJointSystemPrivate::OperationMode::ABS; + } + } - if (_sdf->HasElement("initial_position")) { - this->dataPtr->jointPosCmd = _sdf->Get < double > ("initial_position"); - } + this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset); - igndbg << "[MimicJointSystem] system parameters:\n"; - igndbg << "p_gain: [" << p << "]\n"; - igndbg << "i_gain: [" << i << "]\n"; - igndbg << "d_gain: [" << d << "]\n"; - igndbg << "i_max: [" << iMax << "]\n"; - igndbg << "i_min: [" << iMin << "]\n"; - igndbg << "cmd_max: [" << cmdMax << "]\n"; - igndbg << "cmd_min: [" << cmdMin << "]\n"; - igndbg << "cmd_offset: [" << cmdOffset << "]\n"; - igndbg << "initial_position: [" << this->dataPtr->jointPosCmd << "]\n"; - -} - -void MimicJointSystem::PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) -{ + if (_sdf->HasElement("initial_position")) { + this->dataPtr->jointPosCmd = _sdf->Get < double > ("initial_position"); + } - if (_info.dt < std::chrono::steady_clock::duration::zero()) { - ignwarn << "Detected jump back in time [" - << std::chrono::duration_cast < std::chrono::seconds > (_info.dt).count() - << "s]. System may not work properly." << std::endl; + igndbg << "[MimicJointSystem] system parameters:\n"; + igndbg << "p_gain: [" << p << "]\n"; + igndbg << "i_gain: [" << i << "]\n"; + igndbg << "d_gain: [" << d << "]\n"; + igndbg << "i_max: [" << iMax << "]\n"; + igndbg << "i_min: [" << iMin << "]\n"; + igndbg << "cmd_max: [" << cmdMax << "]\n"; + igndbg << "cmd_min: [" << cmdMin << "]\n"; + igndbg << "cmd_offset: [" << cmdOffset << "]\n"; + igndbg << "initial_position: [" << this->dataPtr->jointPosCmd << "]\n"; } - // If the joint hasn't been identified yet, look for it - if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity || - this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) + void MimicJointSystem::PreUpdate( + const ignition::gazebo::UpdateInfo & _info, + ignition::gazebo::EntityComponentManager & _ecm) { + if (_info.dt < std::chrono::steady_clock::duration::zero()) { + ignwarn << "Detected jump back in time [" << std::chrono::duration_cast < + std::chrono::seconds > (_info.dt).count() + << "s]. System may not work properly." << std::endl; + } - auto jointEntities = _ecm.ChildrenByComponents( - this->dataPtr->model.Entity(), ignition::gazebo::components::Joint()); - - // Iterate over all joints and verify whether they can be enabled or not - for (const auto & jointEntity : jointEntities) { - const auto jointName = _ecm.Component < ignition::gazebo::components::Name > ( - jointEntity)->Data(); - if (jointName == this->dataPtr->jointName) { - this->dataPtr->jointEntity = jointEntity; - } else if (jointName == this->dataPtr->mimicJointName) { - this->dataPtr->mimicJointEntity = jointEntity; + // If the joint hasn't been identified yet, look for it + if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity || + this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) + { + auto jointEntities = + _ecm.ChildrenByComponents( + this->dataPtr->model.Entity(), + ignition::gazebo::components::Joint()); + + // Iterate over all joints and verify whether they can be enabled or not + for (const auto & jointEntity : jointEntities) { + const auto jointName = _ecm.Component < ignition::gazebo::components::Name > + (jointEntity)->Data(); + if (jointName == this->dataPtr->jointName) { + this->dataPtr->jointEntity = jointEntity; + } else if (jointName == this->dataPtr->mimicJointName) { + this->dataPtr->mimicJointEntity = jointEntity; + } } } - } - - if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) { - return; - } - - - if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) { - return; - } - - if (_info.paused) { - return; - } - - auto jointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition > ( - this->dataPtr->jointEntity); - if (!jointPosComp) { - _ecm.CreateComponent( - this->dataPtr->jointEntity, - ignition::gazebo::components::JointPosition()); - } - - auto mimicJointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition > ( - this->dataPtr->mimicJointEntity); - if (!mimicJointPosComp) { - _ecm.CreateComponent( - this->dataPtr->mimicJointEntity, - ignition::gazebo::components::JointPosition()); - } - - if (jointPosComp == nullptr || jointPosComp->Data().empty() || mimicJointPosComp == nullptr || - mimicJointPosComp->Data().empty() ) - { - return; - } + if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) { + return; + } - // Sanity check: Make sure that the joint index is valid. - if (this->dataPtr->jointIndex >= jointPosComp->Data().size()) { - static std::unordered_set < ignition::gazebo::Entity > reported; - if (reported.find(this->dataPtr->jointEntity) == reported.end()) { - ignerr << "[MimicJointSystem]: Detected an invalid " - << "parameter. The index specified is [" - << this->dataPtr->jointIndex << "] but joint [" - << this->dataPtr->jointName << "] only has [" - << jointPosComp->Data().size() << "] index[es]. " - << "This controller will be ignored" << std::endl; - reported.insert(this->dataPtr->jointEntity); + if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) { + return; } - return; - } - // Sanity check: Make sure that the mimic joint index is valid. - if (this->dataPtr->mimicJointIndex >= mimicJointPosComp->Data().size()) { - static std::unordered_set < ignition::gazebo::Entity > mimic_reported; - if (mimic_reported.find(this->dataPtr->mimicJointEntity) == mimic_reported.end()) { - ignerr << "[MimicJointSystem]: Detected an invalid " - << "parameter. The index specified is [" - << this->dataPtr->mimicJointIndex << "] but joint [" - << this->dataPtr->mimicJointName << "] only has [" - << mimicJointPosComp->Data().size() << "] index[es]. " - << "This controller will be ignored" << std::endl; - mimic_reported.insert(this->dataPtr->mimicJointEntity); + if (_info.paused) { + return; } - return; - } - // Get error in position - double error = jointPosComp->Data().at(this->dataPtr->jointIndex) - - (mimicJointPosComp->Data().at(this->dataPtr->mimicJointIndex) * - this->dataPtr->multiplier + this->dataPtr->offset); + auto jointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition > + (this->dataPtr->jointEntity); + if (!jointPosComp) { + _ecm.CreateComponent( + this->dataPtr->jointEntity, + ignition::gazebo::components::JointPosition()); + } - if (fabs(error) > this->dataPtr->deadZone) { + auto mimicJointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition > + (this->dataPtr->mimicJointEntity); + if (!mimicJointPosComp) { + _ecm.CreateComponent( + this->dataPtr->mimicJointEntity, + ignition::gazebo::components::JointPosition()); + } - // Check if the mode is ABS - if (this->dataPtr->mode == - MimicJointSystemPrivate::OperationMode::ABS) + if (jointPosComp == nullptr || jointPosComp->Data().empty() || mimicJointPosComp == nullptr || + mimicJointPosComp->Data().empty()) { - // Calculate target velcity - double targetVel = 0; - - // Get time in seconds - auto dt = std::chrono::duration < double > (_info.dt).count(); - - // Get the maximum amount in m that this joint may move - auto maxMovement = this->dataPtr->posPid.CmdMax() * dt; + return; + } - // Limit the maximum change to maxMovement - if (abs(error) > maxMovement) { - targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() : - -this->dataPtr->posPid.CmdMax(); - } else { - targetVel = -error; + // Sanity check: Make sure that the joint index is valid. + if (this->dataPtr->jointIndex >= jointPosComp->Data().size()) { + static std::unordered_set < ignition::gazebo::Entity > reported; + if (reported.find(this->dataPtr->jointEntity) == reported.end()) { + ignerr << "[MimicJointSystem]: Detected an invalid " + << "parameter. The index specified is [" << this->dataPtr->jointIndex << + "] but joint [" + << this->dataPtr->jointName << "] only has [" << jointPosComp->Data().size() << + "] index[es]. " + << "This controller will be ignored" << std::endl; + reported.insert(this->dataPtr->jointEntity); } + return; + } - // Update velocity command. - auto vel = _ecm.Component < ignition::gazebo::components::JointVelocityCmd > - (this->dataPtr->jointEntity); - - if (vel == nullptr) { - _ecm.CreateComponent( - this->dataPtr->jointEntity, - ignition::gazebo::components::JointVelocityCmd({targetVel})); - } else { - *vel = ignition::gazebo::components::JointVelocityCmd({targetVel}); + // Sanity check: Make sure that the mimic joint index is valid. + if (this->dataPtr->mimicJointIndex >= mimicJointPosComp->Data().size()) { + static std::unordered_set < ignition::gazebo::Entity > mimic_reported; + if (mimic_reported.find(this->dataPtr->mimicJointEntity) == mimic_reported.end()) { + ignerr << "[MimicJointSystem]: Detected an invalid " + << "parameter. The index specified is [" << this->dataPtr->mimicJointIndex << + "] but joint [" + << this->dataPtr->mimicJointName << "] only has [" << + mimicJointPosComp->Data().size() << "] index[es]. " + << "This controller will be ignored" << std::endl; + mimic_reported.insert(this->dataPtr->mimicJointEntity); } return; } - // If the mode is not ABS, default mode is PID - // (e.g.) MimicJointSystemPrivate::OperationMode::PID + // Get error in position + double error = jointPosComp->Data().at(this->dataPtr->jointIndex) - + (mimicJointPosComp->Data().at(this->dataPtr->mimicJointIndex) * this->dataPtr->multiplier + + this->dataPtr->offset); + + if (fabs(error) > this->dataPtr->deadZone) { + // Check if the mode is ABS + if (this->dataPtr->mode == MimicJointSystemPrivate::OperationMode::ABS) { + // Calculate target velcity + double targetVel = 0; + + // Get time in seconds + auto dt = std::chrono::duration < double > (_info.dt).count(); + + // Get the maximum amount in m that this joint may move + auto maxMovement = this->dataPtr->posPid.CmdMax() * dt; + + // Limit the maximum change to maxMovement + if (abs(error) > maxMovement) { + targetVel = + (error < 0) ? this->dataPtr->posPid.CmdMax() : -this->dataPtr->posPid.CmdMax(); + } else { + targetVel = -error; + } + + // Update velocity command. + auto vel = _ecm.Component < ignition::gazebo::components::JointVelocityCmd > + (this->dataPtr->jointEntity); + + if (vel == nullptr) { + _ecm.CreateComponent( + this->dataPtr->jointEntity, ignition::gazebo::components::JointVelocityCmd( + {targetVel})); + } else { + *vel = ignition::gazebo::components::JointVelocityCmd({targetVel}); + } + return; + } - // Update force command. - double force = this->dataPtr->posPid.Update(error, _info.dt); + // If the mode is not ABS, default mode is PID + // (e.g.) MimicJointSystemPrivate::OperationMode::PID - auto forceComp = - _ecm.Component < ignition::gazebo::components::JointForceCmd > - (this->dataPtr->jointEntity); - if (forceComp == nullptr) { - _ecm.CreateComponent( - this->dataPtr->jointEntity, - ignition::gazebo::components::JointForceCmd({force})); - } else { - *forceComp = ignition::gazebo::components::JointForceCmd({force}); + // Update force command. + double force = this->dataPtr->posPid.Update(error, _info.dt); + + auto forceComp = _ecm.Component < ignition::gazebo::components::JointForceCmd > + (this->dataPtr->jointEntity); + if (forceComp == nullptr) { + _ecm.CreateComponent( + this->dataPtr->jointEntity, ignition::gazebo::components::JointForceCmd( + {force})); + } else { + *forceComp = ignition::gazebo::components::JointForceCmd({force}); + } } } -} -void MimicJointSystem::Update( - const ignition::gazebo::UpdateInfo & /*_info*/, - ignition::gazebo::EntityComponentManager & /*_ecm*/) -{ -} + void MimicJointSystem::Update( + const ignition::gazebo::UpdateInfo & /*_info*/, + ignition::gazebo::EntityComponentManager & /*_ecm*/) + { + } -void MimicJointSystem::PostUpdate( - const ignition::gazebo::UpdateInfo & /*_info*/, - const ignition::gazebo::EntityComponentManager & /*_ecm*/) -{ -} + void MimicJointSystem::PostUpdate( + const ignition::gazebo::UpdateInfo & /*_info*/, + const ignition::gazebo::EntityComponentManager & /*_ecm*/) + { + } +} // namespace ign_ros2_control //! [registerMimicJointSystem] IGNITION_ADD_PLUGIN( ign_ros2_control::MimicJointSystem, ignition::gazebo::System, - MimicJointSystem::ISystemConfigure, - MimicJointSystem::ISystemPreUpdate, - MimicJointSystem::ISystemUpdate, - MimicJointSystem::ISystemPostUpdate) + ign_ros2_control::MimicJointSystem::ISystemConfigure, + ign_ros2_control::MimicJointSystem::ISystemPreUpdate, + ign_ros2_control::MimicJointSystem::ISystemUpdate, + ign_ros2_control::MimicJointSystem::ISystemPostUpdate) //! [registerMimicJointSystem] diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index be6b8733..3e1e958b 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -16,6 +16,8 @@ #include +#include + #include #include #include @@ -47,8 +49,6 @@ #include -#include - #include #include @@ -452,7 +452,8 @@ bool IgnitionSystem::initSim( " ' to mimic"); } RCLCPP_INFO_STREAM( - this->nh_->get_logger(), "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint + this->nh_->get_logger(), "Joint '" << joint_name << + "'is mimicking joint '" << mimicked_joint << "' with mutiplier: " << mimic_joint.multiplier); this->dataPtr->mimic_joints_.push_back(mimic_joint); } @@ -603,7 +604,8 @@ void IgnitionSystem::registerSensors(const hardware_interface::HardwareInfo & ha this->nh_->get_logger(), "Loading sensor: " << _name->Data()); - auto sensorTopicComp = this->dataPtr->ecm->Component( + auto sensorTopicComp = + this->dataPtr->ecm->Component( _entity); if (sensorTopicComp) { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); @@ -704,7 +706,8 @@ hardware_interface::return_type IgnitionSystem::read( this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0]; - // set effort state interface to computed/propagated effort command - passthrough because of ignitionrobotics/ign-physics#124 + // set effort state interface to computed/propagated effort command + // - passthrough because of ignitionrobotics/ign-physics#124 this->dataPtr->joints_[i].joint_effort = this->dataPtr->joints_[i].joint_effort_cmd; } @@ -1007,7 +1010,8 @@ hardware_interface::return_type IgnitionSystem::write( position_or_velocity_error = position_error; } - // set command offset - feed forward term added to the pid output that is clamped by pid max command value + // set command offset - feed forward term added to the pid output + // that is clamped by pid max command value // while taking into account mimic multiplier this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset( mimic_joint.multiplier * @@ -1101,8 +1105,7 @@ hardware_interface::return_type IgnitionSystem::write( } return hardware_interface::return_type::OK; } - -} // namespace ign_ros2_control +} // namespace ign_ros2_control #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS( From 496dde5e2e7bad29d8599adc9917234c03f886df Mon Sep 17 00:00:00 2001 From: Lovro Date: Mon, 27 Mar 2023 12:25:53 +0200 Subject: [PATCH 23/24] Fix linters by bypassing for specific lines. --- .../ign_ros2_control/MimicJointSystem.hh | 84 ++++++++++--------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh index afd6f57a..78bb23e2 100644 --- a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh +++ b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh @@ -51,51 +51,53 @@ namespace ign_ros2_control { -class MimicJointSystemPrivate; -class MimicJointSystem: +// Forward declaration + class MimicJointSystemPrivate; // NOLINT + + class MimicJointSystem: // NOLINT // This class is a system. -public ignition::gazebo::System, -public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::System, // NOLINT + public ignition::gazebo::ISystemConfigure, // NOLINT // This class also implements the ISystemPreUpdate, ISystemUpdate, // and ISystemPostUpdate interfaces. -public ignition::gazebo::ISystemPreUpdate, -public ignition::gazebo::ISystemUpdate, -public ignition::gazebo::ISystemPostUpdate -{ - public: - MimicJointSystem(); - - // Documentation inherited - - public: - void Configure( - const ignition::gazebo::Entity & _entity, - const std::shared_ptr < const sdf::Element > & _sdf, - ignition::gazebo::EntityComponentManager & _ecm, - ignition::gazebo::EventManager & _eventMgr) override; - - public: - void PreUpdate( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecgm) override; - - public: - void Update( - const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) override; - - public: - void PostUpdate( - const ignition::gazebo::UpdateInfo & _info, - const ignition::gazebo::EntityComponentManager & _ecm) override; - - private: - /// \brief Private data pointer - - private: - std::unique_ptr < MimicJointSystemPrivate > dataPtr; -}; + public ignition::gazebo::ISystemPreUpdate, // NOLINT + public ignition::gazebo::ISystemUpdate, // NOLINT + public ignition::gazebo::ISystemPostUpdate // NOLINT + { +public: + MimicJointSystem(); + + // Documentation inherited + +public: + void Configure( + const ignition::gazebo::Entity & _entity, + const std::shared_ptr < const sdf::Element > & _sdf, + ignition::gazebo::EntityComponentManager & _ecm, + ignition::gazebo::EventManager & _eventMgr) override; + +public: + void PreUpdate( + const ignition::gazebo::UpdateInfo & _info, + ignition::gazebo::EntityComponentManager & _ecgm) override; + +public: + void Update( + const ignition::gazebo::UpdateInfo & _info, + ignition::gazebo::EntityComponentManager & _ecm) override; + +public: + void PostUpdate( + const ignition::gazebo::UpdateInfo & _info, + const ignition::gazebo::EntityComponentManager & _ecm) override; + +private: + /// \brief Private data pointer + +private: + std::unique_ptr < MimicJointSystemPrivate > dataPtr; + }; } // namespace ign_ros2_control //! [header] From 460d47c26dffee40f58a27c438b8d5912cfbe5e9 Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 31 Mar 2023 12:44:11 +0200 Subject: [PATCH 24/24] Remove suggestion comment. --- ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh index 78bb23e2..e9972e18 100644 --- a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh +++ b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh @@ -29,7 +29,7 @@ #include -// This is an example to go in your urdf (is that correct?) +// This is an example to go in your urdf //! //! joint_name //! mimic_joint_name