diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 87c16e4787..9b03924882 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -28,6 +28,9 @@ namespace admittance_controller { + +constexpr auto NUM_CARTESIAN_DOF = 6; // (3 translation + 3 rotation) + /// Configure admittance rule memory for num joints and load kinematics interface controller_interface::return_type AdmittanceRule::configure( const std::shared_ptr & node, const size_t num_joints) @@ -83,10 +86,10 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) { state_message_.joint_state.name = parameters_.joints; } - state_message_.mass.data.resize(6, 0.0); - state_message_.selected_axes.data.resize(6, 0); - state_message_.damping.data.resize(6, 0); - state_message_.stiffness.data.resize(6, 0); + state_message_.mass.data.resize(NUM_CARTESIAN_DOF, 0.0); + state_message_.selected_axes.data.resize(NUM_CARTESIAN_DOF, 0); + state_message_.damping.data.resize(NUM_CARTESIAN_DOF, 0); + state_message_.stiffness.data.resize(NUM_CARTESIAN_DOF, 0); state_message_.wrench_base.header.frame_id = parameters_.kinematics.base; state_message_.admittance_velocity.header.frame_id = parameters_.kinematics.base; state_message_.admittance_acceleration.header.frame_id = parameters_.kinematics.base; @@ -122,7 +125,7 @@ void AdmittanceRule::apply_parameters_update() vec_to_eigen(parameters_.admittance.stiffness, admittance_state_.stiffness); vec_to_eigen(parameters_.admittance.selected_axes, admittance_state_.selected_axes); - for (size_t i = 0; i < 6; ++i) + for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) { admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * @@ -331,16 +334,20 @@ void AdmittanceRule::process_wrench_measurements( const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_controller_state() { + for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) + { + state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; + state_message_.damping.data[i] = admittance_state_.damping[i]; + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); + state_message_.mass.data[i] = admittance_state_.mass[i]; + } + for (size_t i = 0; i < parameters_.joints.size(); ++i) { state_message_.joint_state.name[i] = parameters_.joints[i]; state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; - state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; - state_message_.damping.data[i] = admittance_state_.damping[i]; - state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); - state_message_.mass.data[i] = admittance_state_.mass[i]; } state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index be78f05bb9..143e5c2846 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -27,7 +27,6 @@ #include "gmock/gmock.h" -#include "6d_robot_description.hpp" #include "admittance_controller/admittance_controller.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,6 +37,7 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "semantic_components/force_torque_sensor.hpp" +#include "test_asset_6d_robot_description.hpp" #include "tf2_ros/transform_broadcaster.h" #include "trajectory_msgs/msg/joint_trajectory.hpp" diff --git a/admittance_controller/test/6d_robot_description.hpp b/admittance_controller/test/test_asset_6d_robot_description.hpp similarity index 98% rename from admittance_controller/test/6d_robot_description.hpp rename to admittance_controller/test/test_asset_6d_robot_description.hpp index f67b5bd054..4d38df7c30 100644 --- a/admittance_controller/test/6d_robot_description.hpp +++ b/admittance_controller/test/test_asset_6d_robot_description.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ -#define ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#ifndef TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ +#define TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ #include @@ -310,4 +310,4 @@ const auto valid_6d_robot_srdf = } // namespace ros2_control_test_assets -#endif // ROS2_CONTROL_TEST_ASSETS__6D_ROBOT_DESCRIPTION_HPP_ +#endif // TEST_ASSET_6D_ROBOT_DESCRIPTION_HPP_ diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 65ffc6795b..4d8c3752ae 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -138,4 +138,4 @@ That's it! Enjoy writing great controllers! Useful External References --------------------------- -- `Templates and scripts for generating controllers shell `_ +- `Templates and scripts for generating controllers shell `_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4c01c8ace0..cc033b76cf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1323,8 +1323,12 @@ void JointTrajectoryController::sort_to_local_joint_order( get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); return to_remap; } - std::vector output; - output.resize(mapping.size(), 0.0); + static std::vector output(dof_, 0.0); + // Only resize if necessary since it's an expensive operation + if (output.size() != mapping.size()) + { + output.resize(mapping.size(), 0.0); + } for (size_t index = 0; index < mapping.size(); ++index) { auto map_index = mapping[index];