diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 760ac77c..37c45eb1 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -25,10 +25,14 @@ #include #include +#include #include #include #include +#include +#include #include +#include #include #include #include @@ -76,7 +80,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 +242,13 @@ 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); + "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"; } @@ -573,40 +560,94 @@ 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) { + 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) { - if (!this->dataPtr->ecm->Component( + 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; + double error = this->dataPtr->joints_[i].joint_position_cmd - + this->dataPtr->joints_[i].joint_position; - // Calculate target velcity - double targetVel = -error; + // 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->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({targetVel})); + ignition::gazebo::components::JointVelocityReset({targetVel})); } else if (!vel->Data().empty()) { vel->Data()[0] = targetVel; } @@ -618,7 +659,8 @@ hardware_interface::return_type IgnitionSystem::write( { 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 +671,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