From c2237cb16adfee0c0c5d588cf9c94ce0fb537572 Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 9 Sep 2022 11:38:13 +0200 Subject: [PATCH 1/4] Adapt mimic joint control method in perform_switch. Other mimic related small changes. Use components::VelocityResetCmd to be able to recover from joint limits. --- ign_ros2_control/src/ign_system.cpp | 222 ++++++++---------- .../config/gripper_controller.yaml | 8 + .../gripper_mimic_joint_example.launch.py | 15 +- 3 files changed, 120 insertions(+), 125 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 760ac77c..136efcf5 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -28,7 +28,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -76,7 +78,7 @@ struct MimicJoint std::size_t joint_index; std::size_t mimicked_joint_index; double multiplier = 1.0; - std::vector interfaces_to_mimic; +// std::vector interfaces_to_mimic; }; class ImuData @@ -238,30 +240,11 @@ bool IgnitionSystem::initSim( mimic_joint.multiplier = 1.0; } - // 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( - 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);} - bool vel = interface_info.name == "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);} - return pos || vel || eff; - }); - if (state_mimicked_interface == joint_info_mimicked.state_interfaces.end()) { - throw std::runtime_error( - std::string( - "For mimic joint '") + joint_info.name + - "' no state interface was found in mimicked joint '" + mimicked_joint + - " ' 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 '" << this->dataPtr->joints_[mimic_joint.joint_index].name << "'is mimicking joint '" << + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].name << "' with multiplier: "<< mimic_joint.multiplier); + this->dataPtr->mimic_joints_.push_back(mimic_joint); suffix = "_mimic"; } @@ -524,8 +507,9 @@ IgnitionSystem::perform_command_mode_switch( const std::vector & stop_interfaces) { for (unsigned int j = 0; j < this->dataPtr->joints_.size(); j++) { + for (const std::string & interface_name : stop_interfaces) { - // Clear joint control method bits corresponding to stop interfaces + // Clear joint control method bits corresponding to stop interfaces if (interface_name == (this->dataPtr->joints_[j].name + "/" + hardware_interface::HW_IF_POSITION)) { @@ -566,6 +550,48 @@ IgnitionSystem::perform_command_mode_switch( } } + for (unsigned int i = 0; i < this->dataPtr->mimic_joints_.size(); ++i){ + for (const std::string & interface_name : stop_interfaces) { + // Clear joint control method bits corresponding to stop interfaces + if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + + hardware_interface::HW_IF_POSITION)) + { + this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method &= + static_cast(VELOCITY & EFFORT); + } + if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + + hardware_interface::HW_IF_VELOCITY)) + { + this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method &= + static_cast(POSITION & EFFORT); + } + if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + + hardware_interface::HW_IF_EFFORT)) + { + this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method &= + static_cast(POSITION & VELOCITY); + } + } + + for (const std::string & interface_name : start_interfaces) { + if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + + hardware_interface::HW_IF_POSITION)) + { + this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method |= POSITION; + } + if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + + hardware_interface::HW_IF_VELOCITY)) + { + this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method |= VELOCITY; + } + if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + + hardware_interface::HW_IF_EFFORT)) + { + this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method |= EFFORT; + } + } + } + return hardware_interface::return_type::OK; } @@ -573,52 +599,74 @@ hardware_interface::return_type IgnitionSystem::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { - if (!this->dataPtr->ecm->Component( + for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { + + if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & POSITION && + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & POSITION) + { + this->dataPtr->joints_[mimic_joint.joint_index].joint_position_cmd = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position_cmd; + } + if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & VELOCITY && + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & VELOCITY) + { + this->dataPtr->joints_[mimic_joint.joint_index].joint_velocity_cmd = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_cmd; + } + if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & EFFORT && + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & EFFORT) + { + this->dataPtr->joints_[mimic_joint.joint_index].joint_effort_cmd = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd; + } + } + + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + + if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { + if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointVelocityCmd({0})); + ignition::gazebo::components::JointVelocityReset( + {this->dataPtr->joints_[i].joint_velocity_cmd})); } else { const auto jointVelCmd = - this->dataPtr->ecm->Component( + this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); - *jointVelCmd = ignition::gazebo::components::JointVelocityCmd( + *jointVelCmd = ignition::gazebo::components::JointVelocityReset( {this->dataPtr->joints_[i].joint_velocity_cmd}); } } - 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; + if (this->dataPtr->joints_[i].joint_control_method & POSITION) { - // Calculate target velcity - double targetVel = -error; + // Get error in position + double error; + error = (this->dataPtr->joints_[i].joint_position - + this->dataPtr->joints_[i].joint_position_cmd) * *this->dataPtr->update_rate; - auto vel = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + // Calculate target velocity + double targetVel = -error; - if (vel == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointVelocityCmd({targetVel})); - } else if (!vel->Data().empty()) { - vel->Data()[0] = targetVel; + 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::JointVelocityReset({targetVel})); + } else if (!vel->Data().empty()) { + vel->Data()[0] = targetVel; + } } - } - if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { + if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointForceCmd({0})); + ignition::gazebo::components::JointForceCmd({this->dataPtr->joints_[i].joint_effort_cmd})); } else { const auto jointEffortCmd = this->dataPtr->ecm->Component( @@ -629,80 +677,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) { - 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 - position_mimicked_joint * mimic_joint.multiplier; - - double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate); - - auto vel = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - - 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; - } - } - 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)) - { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointVelocityCmd({0})); - } 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}); - } - } - 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}); - } - } - } - } - return hardware_interface::return_type::OK; } } // namespace ign_ros2_control diff --git a/ign_ros2_control_demos/config/gripper_controller.yaml b/ign_ros2_control_demos/config/gripper_controller.yaml index d72296e1..3396cca3 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 @@ -13,3 +16,8 @@ gripper_controller: joints: - right_finger_joint interface_name: position + +gripper_action_controller: + ros__parameters: + default: true + joint: right_finger_joint 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 6018dc9d..49d7c1f0 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,13 +75,20 @@ 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( PythonLaunchDescriptionSource( [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), - launch_arguments=[('ign_args', [' -r -v 4 empty.sdf'])]), + launch_arguments=[('ign_args', [' -r -v 3 empty.sdf'])]), RegisterEventHandler( event_handler=OnProcessExit( target_action=ignition_spawn_entity, @@ -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 442a6d24180b28e5b7a7495bd2721bf7b339e97f Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 9 Sep 2022 20:20:28 +0200 Subject: [PATCH 2/4] Limit velocity command for non-mimic joints. --- ign_ros2_control/src/ign_system.cpp | 51 ++++++++++++++++++++--------- 1 file changed, 36 insertions(+), 15 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 136efcf5..dd80b99d 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -25,9 +25,11 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -550,7 +552,7 @@ IgnitionSystem::perform_command_mode_switch( } } - for (unsigned int i = 0; i < this->dataPtr->mimic_joints_.size(); ++i){ + /*for (unsigned int i = 0; i < this->dataPtr->mimic_joints_.size(); ++i){ for (const std::string & interface_name : stop_interfaces) { // Clear joint control method bits corresponding to stop interfaces if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + @@ -590,7 +592,7 @@ IgnitionSystem::perform_command_mode_switch( this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method |= EFFORT; } } - } + }*/ return hardware_interface::return_type::OK; } @@ -601,20 +603,39 @@ hardware_interface::return_type IgnitionSystem::write( { for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & POSITION && - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & POSITION) + if (this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & POSITION) { - this->dataPtr->joints_[mimic_joint.joint_index].joint_position_cmd = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position_cmd; + // Get error in position + double position_error = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position * mimic_joint.multiplier - + this->dataPtr->joints_[mimic_joint.joint_index].joint_position; + + double velocity_sp = position_error * (*this->dataPtr->update_rate); + + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointVelocityReset ({velocity_sp})); + } - if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & VELOCITY && - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & VELOCITY) + if (this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & VELOCITY) { - this->dataPtr->joints_[mimic_joint.joint_index].joint_velocity_cmd = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_cmd; + 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::JointVelocityReset( + {mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_cmd})); + } else { + const auto jointVelCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointVelCmd = ignition::gazebo::components::JointVelocityReset( + {mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_cmd}); + } } - if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & EFFORT && - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & EFFORT) + if (this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & EFFORT) { - this->dataPtr->joints_[mimic_joint.joint_index].joint_effort_cmd = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd; + } } @@ -640,12 +661,12 @@ hardware_interface::return_type IgnitionSystem::write( 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; + double error = this->dataPtr->joints_[i].joint_position_cmd - this->dataPtr->joints_[i].joint_position; // Calculate target velocity - double targetVel = -error; + double maxVel = this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint)->Data().MaxVelocity(); + double targetVel = std::clamp(error * (*this->dataPtr->update_rate), -1.0 * maxVel, maxVel); auto vel = this->dataPtr->ecm->Component( From b33e5b9b83fe42a7306b989362d2674355c152ed Mon Sep 17 00:00:00 2001 From: Lovro Date: Fri, 9 Sep 2022 20:30:57 +0200 Subject: [PATCH 3/4] Add force mimicking. --- ign_ros2_control/src/ign_system.cpp | 187 ++++++++++++---------------- 1 file changed, 80 insertions(+), 107 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index dd80b99d..37c45eb1 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -243,9 +243,11 @@ bool IgnitionSystem::initSim( } RCLCPP_INFO_STREAM( - this->nh_->get_logger(), - "Joint '" << this->dataPtr->joints_[mimic_joint.joint_index].name << "'is mimicking joint '" << - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].name << "' with multiplier: "<< mimic_joint.multiplier); + this->nh_->get_logger(), + "Joint '" << this->dataPtr->joints_[mimic_joint.joint_index].name << + "'is mimicking joint '" << + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].name << "' with multiplier: " << + mimic_joint.multiplier); this->dataPtr->mimic_joints_.push_back(mimic_joint); suffix = "_mimic"; @@ -509,9 +511,8 @@ IgnitionSystem::perform_command_mode_switch( const std::vector & stop_interfaces) { for (unsigned int j = 0; j < this->dataPtr->joints_.size(); j++) { - for (const std::string & interface_name : stop_interfaces) { - // Clear joint control method bits corresponding to stop interfaces + // Clear joint control method bits corresponding to stop interfaces if (interface_name == (this->dataPtr->joints_[j].name + "/" + hardware_interface::HW_IF_POSITION)) { @@ -552,48 +553,6 @@ IgnitionSystem::perform_command_mode_switch( } } - /*for (unsigned int i = 0; i < this->dataPtr->mimic_joints_.size(); ++i){ - for (const std::string & interface_name : stop_interfaces) { - // Clear joint control method bits corresponding to stop interfaces - if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + - hardware_interface::HW_IF_POSITION)) - { - this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method &= - static_cast(VELOCITY & EFFORT); - } - if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + - hardware_interface::HW_IF_VELOCITY)) - { - this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method &= - static_cast(POSITION & EFFORT); - } - if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + - hardware_interface::HW_IF_EFFORT)) - { - this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method &= - static_cast(POSITION & VELOCITY); - } - } - - for (const std::string & interface_name : start_interfaces) { - if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + - hardware_interface::HW_IF_POSITION)) - { - this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method |= POSITION; - } - if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + - hardware_interface::HW_IF_VELOCITY)) - { - this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method |= VELOCITY; - } - if (interface_name == (this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].mimicked_joint_index].name + "/" + - hardware_interface::HW_IF_EFFORT)) - { - this->dataPtr->joints_[this->dataPtr->mimic_joints_[i].joint_index].joint_control_method |= EFFORT; - } - } - }*/ - return hardware_interface::return_type::OK; } @@ -601,54 +560,67 @@ hardware_interface::return_type IgnitionSystem::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - - if (this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & POSITION) - { - // Get error in position - double position_error = this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position * mimic_joint.multiplier - - this->dataPtr->joints_[mimic_joint.joint_index].joint_position; - - double velocity_sp = position_error * (*this->dataPtr->update_rate); - - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - ignition::gazebo::components::JointVelocityReset ({velocity_sp})); - - } - if (this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & VELOCITY) - { - 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::JointVelocityReset( - {mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_cmd})); - } else { - const auto jointVelCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - *jointVelCmd = ignition::gazebo::components::JointVelocityReset( - {mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_cmd}); - } - } - if (this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & EFFORT) - { - - } + for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { + if (this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & POSITION) { + // Get error in position + double position_error = + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position * + mimic_joint.multiplier - + this->dataPtr->joints_[mimic_joint.joint_index].joint_position; + + double velocity_sp = position_error * (*this->dataPtr->update_rate); + + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + ignition::gazebo::components::JointVelocityReset({velocity_sp})); } + if (this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & VELOCITY) { + 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::JointVelocityReset( + {mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_cmd})); + } else { + const auto jointVelCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointVelCmd = ignition::gazebo::components::JointVelocityReset( + {mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_cmd}); + } + } + if (this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & EFFORT) { + 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( + {mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd})); + } 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}); + } + } + } - for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - - if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, ignition::gazebo::components::JointVelocityReset( - {this->dataPtr->joints_[i].joint_velocity_cmd})); + {this->dataPtr->joints_[i].joint_velocity_cmd})); } else { const auto jointVelCmd = this->dataPtr->ecm->Component( @@ -658,36 +630,37 @@ hardware_interface::return_type IgnitionSystem::write( } } - if (this->dataPtr->joints_[i].joint_control_method & POSITION) { + if (this->dataPtr->joints_[i].joint_control_method & POSITION) { + // Get error in position + double error = this->dataPtr->joints_[i].joint_position_cmd - + this->dataPtr->joints_[i].joint_position; - // Get error in position - double error = this->dataPtr->joints_[i].joint_position_cmd - this->dataPtr->joints_[i].joint_position; + // Calculate target velocity + double maxVel = this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint)->Data().MaxVelocity(); + double targetVel = std::clamp(error * (*this->dataPtr->update_rate), -1.0 * maxVel, maxVel); - // Calculate target velocity - double maxVel = this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint)->Data().MaxVelocity(); - double targetVel = std::clamp(error * (*this->dataPtr->update_rate), -1.0 * maxVel, maxVel); - - auto vel = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + 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::JointVelocityReset({targetVel})); - } else if (!vel->Data().empty()) { - vel->Data()[0] = targetVel; - } + if (vel == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, + ignition::gazebo::components::JointVelocityReset({targetVel})); + } else if (!vel->Data().empty()) { + vel->Data()[0] = targetVel; } + } - if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { + if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { if (!this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint)) { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointForceCmd({this->dataPtr->joints_[i].joint_effort_cmd})); + ignition::gazebo::components::JointForceCmd( + {this->dataPtr->joints_[i].joint_effort_cmd})); } else { const auto jointEffortCmd = this->dataPtr->ecm->Component( From 6fc1dbee9e1c7c7cf1cef0b3f8c35ef06187cfb4 Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 27 Sep 2022 11:12:36 +0200 Subject: [PATCH 4/4] Use simulation time. --- ign_ros2_control/src/ign_ros2_control_plugin.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 7c78b16b..e9bf16f6 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -279,7 +279,8 @@ void IgnitionROS2ControlPlugin::Configure( controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; } - std::vector arguments = {"--ros-args", "--params-file", paramFileName}; + std::vector arguments = {"--ros-args", "--params-file", paramFileName, + "--ros-args", "-p", "use_sim_time:=false"}; auto sdfPtr = const_cast(_sdf.get()); if (sdfPtr->HasElement("ros")) {