diff --git a/.clang-format b/.clang-format index 9c21e1e17c..698bda7da8 100644 --- a/.clang-format +++ b/.clang-format @@ -10,6 +10,6 @@ ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle -ReflowComments: false +ReflowComments: true IncludeBlocks: Preserve ... diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 37b784011c..2bdccefdc5 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -41,7 +41,8 @@ class ChainableControllerInterface : public ControllerInterfaceBase virtual ~ChainableControllerInterface() = default; /** - * Control step update. Command interfaces are updated based on on reference inputs and current states. + * Control step update. Command interfaces are updated based on on reference inputs and current + * states. * **The method called in the (real-time) control loop.** * * \param[in] time The time at the start of this control loop iteration @@ -83,7 +84,9 @@ class ChainableControllerInterface : public ControllerInterfaceBase * * \param[in] flag marking a switch to or from chained mode. * - * \returns true if controller successfully switched between "chained" and "external" mode. \default returns true so the method don't have to be overridden if controller can always switch chained mode. + * \returns true if controller successfully switched between "chained" and "external" mode. + * \default returns true so the method don't have to be overridden if controller can always switch + * chained mode. */ virtual bool on_set_chained_mode(bool chained_mode); diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index fdaffac06f..05ee0c8830 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -74,15 +74,13 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy /// Get configuration for controller's required command interfaces. /** - * Method used by the controller_manager to get the set of command interfaces used by the controller. - * Each controller can use individual method to determine interface names that in simples case - * have the following format: `/`. - * The method is called only in `inactive` or `active` state, i.e., `on_configure` has to be - * called first. - * The configuration is used to check if controller can be activated and to claim interfaces from - * hardware. - * The claimed interfaces are populated in the - * \ref ControllerInterfaceBase::command_interfaces_ "command_interfaces_" member. + * Method used by the controller_manager to get the set of command interfaces used by the + * controller. Each controller can use individual method to determine interface names that in + * simples case have the following format: `/`. The method is called only in + * `inactive` or `active` state, i.e., `on_configure` has to be called first. The configuration is + * used to check if controller can be activated and to claim interfaces from hardware. The claimed + * interfaces are populated in the \ref ControllerInterfaceBase::command_interfaces_ + * "command_interfaces_" member. * * \returns configuration of command interfaces. */ @@ -131,7 +129,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy virtual CallbackReturn on_init() = 0; /** - * Control step update. Command interfaces are updated based on on reference inputs and current states. + * Control step update. Command interfaces are updated based on on reference inputs and current + * states. * **The method called in the (real-time) control loop.** * * \param[in] time The time at the start of this control loop iteration diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index 45b048fc72..b571751f55 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -23,19 +23,19 @@ namespace controller_interface { /// Reorder interfaces with references according to joint names or full interface names. /** - * Method to reorder and check if all expected interfaces are provided for the joint. - * Fill `ordered_interfaces` with references from `unordered_interfaces` in the same order as in - * `ordered_names`. - * - * \param[in] unordered_interfaces vector with loaned unordered state or command interfaces. - * \param[in] ordered_names vector with ordered names to order \p unordered_interfaces. - * The valued inputs are list of joint names or interface full names. - * If joint names are used for ordering, \p interface_type specifies valid interface. - * If full interface names are used for ordering, \p interface_type should be empty string (""). - * \param[in] interface_type used for ordering interfaces with respect to joint names. - * \param[out] ordered_interfaces vector with ordered interfaces. - * \return true if all interfaces or joints in \p ordered_names are found, otherwise false. - */ + * Method to reorder and check if all expected interfaces are provided for the joint. + * Fill `ordered_interfaces` with references from `unordered_interfaces` in the same order as in + * `ordered_names`. + * + * \param[in] unordered_interfaces vector with loaned unordered state or command interfaces. + * \param[in] ordered_names vector with ordered names to order \p unordered_interfaces. + * The valued inputs are list of joint names or interface full names. + * If joint names are used for ordering, \p interface_type specifies valid interface. + * If full interface names are used for ordering, \p interface_type should be empty string (""). + * \param[in] interface_type used for ordering interfaces with respect to joint names. + * \param[out] ordered_interfaces vector with ordered interfaces. + * \return true if all interfaces or joints in \p ordered_names are found, otherwise false. + */ template bool get_ordered_interfaces( std::vector & unordered_interfaces, const std::vector & ordered_names, diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 537f0447be..582cbc5f24 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -318,8 +318,8 @@ class ControllerManager : public rclcpp::Node const std::string & command_interface); /** - * Clear request lists used when switching controllers. The lists are shared between "callback" and - * "control loop" threads. + * Clear request lists used when switching controllers. The lists are shared between "callback" + * and "control loop" threads. */ void clear_requests(); @@ -428,7 +428,8 @@ class ControllerManager : public rclcpp::Node * lists match and returns a reference to it * This referenced list can be modified safely until switch_updated_controller_list() * is called, at this point the RT thread may start using it at any time - * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list + * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by + * rt list */ std::vector & get_unused_list( const std::lock_guard & guard); @@ -436,7 +437,8 @@ class ControllerManager : public rclcpp::Node /// get_updated_list Returns a const reference to the most updated list. /** * \warning May or may not being used by the realtime thread, read-only reference for safety - * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list + * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by + * rt list */ const std::vector & get_updated_list( const std::lock_guard & guard) const; @@ -444,7 +446,8 @@ class ControllerManager : public rclcpp::Node /** * switch_updated_list Switches the "updated" and "outdated" lists, and waits * until the RT thread is using the new "updated" list. - * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list + * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by + * rt list */ void switch_updated_list(const std::lock_guard & guard); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 64c81b0a34..abfd8eb45a 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -35,8 +35,9 @@ namespace hardware_interface /** * The typical examples are conveyors or motors. * - * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - * with the following meaning: + * Methods return values have type + * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following + * meaning: * * \returns CallbackReturn::SUCCESS method execution was successful. * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. @@ -46,7 +47,8 @@ namespace hardware_interface * The hardware ends after each method in a state with the following meaning: * * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is available. + * Hardware is initialized but communication is not started and therefore no interface is + * available. * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. @@ -126,12 +128,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * * \note This is a non-realtime evaluation of whether a set of command interface claims are * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \note All starting and stopping interface keys are passed to all components, so the function should - * return return_type::OK by default when given interface keys not relevant for this component. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping. - * \return return_type::OK if the new command interface combination can be prepared, - * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. + * \note All starting and stopping interface keys are passed to all components, so the function + * should return return_type::OK by default when given interface keys not relevant for this + * component. \param[in] start_interfaces vector of string identifiers for the command interfaces + * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs + * stopping. \return return_type::OK if the new command interface combination can be prepared, or + * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. */ virtual return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, @@ -145,11 +147,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * Perform the mode-switching for the new command interface combination. * * \note This is part of the realtime update loop, and should be fast. - * \note All starting and stopping interface keys are passed to all components, so the function should - * return return_type::OK by default when given interface keys not relevant for this component. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping. - * \return return_type::OK if the new command interface combination can be switched to, + * \note All starting and stopping interface keys are passed to all components, so the function + * should return return_type::OK by default when given interface keys not relevant for this + * component. \param[in] start_interfaces vector of string identifiers for the command interfaces + * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs + * stopping. \return return_type::OK if the new command interface combination can be switched to, * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. */ virtual return_type perform_command_mode_switch( diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 9f81a2a863..5112f7927e 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -26,10 +26,10 @@ namespace hardware_interface { /// Search XML snippet from URDF for information about a control component. /** - * \param[in] urdf string with robot's URDF - * \return vector filled with information about robot's control resources - * \throws std::runtime_error if a robot attribute or tag is not found - */ + * \param[in] urdf string with robot's URDF + * \return vector filled with information about robot's control resources + * \throws std::runtime_error if a robot attribute or tag is not found + */ HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index a5113380e1..4d2f995756 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -188,8 +188,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /** * Return list of cached controller names that use the hardware with name \p hardware_name. * - * \param[in] hardware_name the name of the hardware for which cached controllers should be returned. - * \returns list of cached controller names that depend on hardware with name \p hardware_name. + * \param[in] hardware_name the name of the hardware for which cached controllers should be + * returned. \returns list of cached controller names that depend on hardware with name \p + * hardware_name. */ std::vector get_cached_controllers_to_hardware(const std::string & hardware_name); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 2b2a4ce7cb..14a59e4588 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -35,8 +35,9 @@ namespace hardware_interface /** * The typical examples are Force-Torque Sensor (FTS), Interial Measurement Unit (IMU). * - * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - * with the following meaning: + * Methods return values have type + * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following + * meaning: * * \returns CallbackReturn::SUCCESS method execution was successful. * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. @@ -46,7 +47,8 @@ namespace hardware_interface * The hardware ends after each method in a state with the following meaning: * * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is available. + * Hardware is initialized but communication is not started and therefore no interface is + * available. * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 75e1a1bc29..e5c6f2f542 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -36,8 +36,9 @@ namespace hardware_interface * The common examples for these types of hardware are multi-joint systems with or without sensors * such as industrial or humanoid robots. * - * Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - * with the following meaning: + * Methods return values have type + * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following + * meaning: * * \returns CallbackReturn::SUCCESS method execution was successful. * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. @@ -47,7 +48,8 @@ namespace hardware_interface * The hardware ends after each method in a state with the following meaning: * * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is available. + * Hardware is initialized but communication is not started and therefore no interface is + * available. * * INACTIVE (on_configure, on_deactivate): * Communication with the hardware is started and it is configured. @@ -127,12 +129,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * * \note This is a non-realtime evaluation of whether a set of command interface claims are * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \note All starting and stopping interface keys are passed to all components, so the function should - * return return_type::OK by default when given interface keys not relevant for this component. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping. - * \return return_type::OK if the new command interface combination can be prepared, - * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. + * \note All starting and stopping interface keys are passed to all components, so the function + * should return return_type::OK by default when given interface keys not relevant for this + * component. \param[in] start_interfaces vector of string identifiers for the command interfaces + * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs + * stopping. \return return_type::OK if the new command interface combination can be prepared, or + * if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. */ virtual return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, @@ -146,11 +148,11 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * Perform the mode-switching for the new command interface combination. * * \note This is part of the realtime update loop, and should be fast. - * \note All starting and stopping interface keys are passed to all components, so the function should - * return return_type::OK by default when given interface keys not relevant for this component. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping. - * \return return_type::OK if the new command interface combination can be switched to, + * \note All starting and stopping interface keys are passed to all components, so the function + * should return return_type::OK by default when given interface keys not relevant for this + * component. \param[in] start_interfaces vector of string identifiers for the command interfaces + * starting. \param[in] stop_interfaces vector of string identifiers for the command interfacs + * stopping. \return return_type::OK if the new command interface combination can be switched to, * or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise. */ virtual return_type perform_command_mode_switch( diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 305c7ff7aa..1338489a07 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -304,11 +304,11 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( /// Search XML snippet from URDF for information about a control component. /** - * \param[in] component_it pointer to the iterator where component - * info should be found - * \return ComponentInfo filled with information about component - * \throws std::runtime_error if a component attribute or tag is not found - */ + * \param[in] component_it pointer to the iterator where component + * info should be found + * \return ComponentInfo filled with information about component + * \throws std::runtime_error if a component attribute or tag is not found + */ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it) { ComponentInfo component; @@ -417,10 +417,10 @@ ActuatorInfo parse_transmission_actuator_from_xml(const tinyxml2::XMLElement * e /// Search XML snippet from URDF for information about a transmission. /** - * \param[in] transmission_it pointer to the iterator where transmission info should be found - * \return TransmissionInfo filled with information about transmission - * \throws std::runtime_error if an attribute or tag is not found - */ + * \param[in] transmission_it pointer to the iterator where transmission info should be found + * \return TransmissionInfo filled with information about transmission + * \throws std::runtime_error if an attribute or tag is not found + */ TransmissionInfo parse_transmission_from_xml(const tinyxml2::XMLElement * transmission_it) { TransmissionInfo transmission; diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 8fec418dcf..5f9c09e95e 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -40,7 +40,7 @@ class TestActuator : public ActuatorInterface * if (info_.joints[0].command_interfaces.size() != 1) {return CallbackReturn::ERROR;} * // can only give feedback state for position and velocity * if (info_.joints[0].state_interfaces.size() != 2) {return CallbackReturn::ERROR;} - */ + */ return CallbackReturn::SUCCESS; } diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 5d1fd6a1f7..2f32d49271 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -28,11 +28,11 @@ namespace // utilities { /// Declare and initialize a parameter with a type. /** - * - * Wrapper function for templated node's declare_parameter() which checks if - * parameter is already declared. - * For use in all components that inherit from ControllerInterface - */ + * + * Wrapper function for templated node's declare_parameter() which checks if + * parameter is already declared. + * For use in all components that inherit from ControllerInterface + */ template auto auto_declare( const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, @@ -149,10 +149,9 @@ inline bool declare_parameters(const std::string & joint_name, const rclcpp::Nod } /** - * Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the lifecycle_node - * object. - * This is a convenience function. - * For parameters structure see the underlying `declare_parameters` function. + * Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the + * lifecycle_node object. This is a convenience function. For parameters structure see the + * underlying `declare_parameters` function. * * \param[in] joint_name name of the joint for which parameters will be declared. * \param[in] lifecycle_node lifecycle node for parameters should be declared. @@ -217,10 +216,12 @@ inline bool declare_parameters( * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". * \param[in] param_itf node parameters interface of the node where parameters are specified. * \param[in] logging_itf node logging interface to provide log errors. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter + * server will overwrite existing values. Values in \p limits not specified in the parameter server + * remain unchanged. * - * \returns True if a limits specification is found (i.e., the \p joint_limits/joint_name parameter exists in \p node), false otherwise. + * \returns True if a limits specification is found (i.e., the \p joint_limits/joint_name parameter + * exists in \p node), false otherwise. */ inline bool get_joint_limits( const std::string & joint_name, @@ -385,8 +386,9 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] node Node object for which parameters should be fetched. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter + * server will overwrite existing values. Values in \p limits not specified in the parameter server + * remain unchanged. * * \returns True if a limits specification is found, false otherwise. */ @@ -404,8 +406,9 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. Values in \p limits not specified in the parameter server remain unchanged. + * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter + * server will overwrite existing values. Values in \p limits not specified in the parameter server + * remain unchanged. * * \returns True if a limits specification is found, false otherwise. */ @@ -445,10 +448,10 @@ inline bool get_joint_limits( * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". * \param[in] param_itf node parameters interface of the node where parameters are specified. * \param[in] logging_itf node logging interface to provide log errors. - * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. - * \return True if a complete soft limits specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and - * \p soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the + * parameter server will overwrite existing values. \return True if a complete soft limits + * specification is found (ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and \p + * soft_upper_limit exist in \p joint_limits/joint_name namespace), false otherwise. */ inline bool get_joint_limits( const std::string & joint_name, @@ -512,8 +515,8 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] node Node object for which parameters should be fetched. - * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the + * parameter server will overwrite existing values. * * \returns True if a soft limits specification is found, false otherwise. */ @@ -533,8 +536,8 @@ inline bool get_joint_limits( * * \param[in] joint_name Name of joint whose limits are to be fetched. * \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched. - * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite - * existing values. + * \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the + * parameter server will overwrite existing values. * * \returns True if a soft limits specification is found, false otherwise. */ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp index 3f17ceea78..db553948d1 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -144,8 +144,8 @@ class JointSoftLimitsHandle : public JointLimitHandle joint_limits_interface::SoftJointLimits soft_limits_; }; -/** A handle used to enforce position and velocity limits of a position-controlled joint that does not have - soft limits. */ +/** A handle used to enforce position and velocity limits of a position-controlled joint that does + not have soft limits. */ class PositionJointSaturationHandle : public JointLimitHandle { public: @@ -170,8 +170,8 @@ class PositionJointSaturationHandle : public JointLimitHandle /// Enforce position and velocity limits for a joint that is not subject to soft limits. /** - * \param[in] period Control period. - */ + * \param[in] period Control period. + */ void enforce_limits(const rclcpp::Duration & period) { if (std::isnan(prev_pos_)) @@ -209,9 +209,9 @@ class PositionJointSaturationHandle : public JointLimitHandle /// A handle used to enforce position and velocity limits of a position-controlled joint. /** - * This class implements a very simple position and velocity limits enforcing policy, and tries to impose the least - * amount of requisites on the underlying hardware platform. - * This lowers considerably the entry barrier to use it, but also implies some limitations. + * This class implements a very simple position and velocity limits enforcing policy, and tries to + * impose the least amount of requisites on the underlying hardware platform. This lowers + * considerably the entry barrier to use it, but also implies some limitations. * * Requisites * - Position (for non-continuous joints) and velocity limits specification. @@ -219,21 +219,22 @@ class PositionJointSaturationHandle : public JointLimitHandle * * Open loop nature * - * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is checked for - * validity without relying on the actual position/velocity values. + * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is + * checked for validity without relying on the actual position/velocity values. * - * - Actual position values are \e not used because in some platforms there might be a substantial lag - * between sending a command and executing it (propagate command to hardware, reach control objective, - * read from hardware). + * - Actual position values are \e not used because in some platforms there might be a substantial + * lag between sending a command and executing it (propagate command to hardware, reach control + * objective, read from hardware). * - * - Actual velocity values are \e not used because of the above reason, and because some platforms might not expose - * trustworthy velocity measurements, or none at all. + * - Actual velocity values are \e not used because of the above reason, and because some platforms + * might not expose trustworthy velocity measurements, or none at all. * - * The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large - * position tracking errors. Only the command is guaranteed to comply with the limits specification. + * The downside of the open loop behavior is that velocity limits will not be enforced when + * recovering from large position tracking errors. Only the command is guaranteed to comply with the + * limits specification. * - * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate the command - * velocity. + * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate + * the command velocity. */ // TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? @@ -553,8 +554,8 @@ class VelocityJointSaturationHandle : public JointLimitHandle /** * A handle used to enforce position, velocity, and acceleration limits of a - * velocity-controlled joint. - */ + * velocity-controlled joint. + */ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle { public: diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp index 6f26a997ee..80cf7f0ed4 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp @@ -27,8 +27,8 @@ namespace joint_limits_interface /** * Populate a JointLimits instance from URDF joint data. * \param[in] urdf_joint URDF joint. - * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will overwrite existing - * values. Values in \e limits not present in \e urdf_joint remain unchanged. + * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will + * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged. * \return True if \e urdf_joint has a valid limits specification, false otherwise. */ inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 6bc89ab893..2002f6f9d7 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -30,9 +30,8 @@ namespace transmission_interface /// Implementation of a differential transmission. /** * - * This transmission relates two actuators and two joints through a differential mechanism, as illustrated - * below. - * \image html differential_transmission.png + * This transmission relates two actuators and two joints through a differential + * mechanism, as illustrated below. \image html differential_transmission.png * *
* @@ -76,28 +75,33 @@ namespace transmission_interface * *
* \f{eqnarray*}{ - * x_{a_1} & = & n_{a_1} \left[ n_{j_1} (x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2}) \right] \\[2.5em] - * x_{a_2} & = & n_{a_2} \left[ n_{j_1} (x_{j_1} - x_{off_1}) - n_{j_2} (x_{j_2} - x_{off_2}) \right] - * \f} + * x_{a_1} & = & n_{a_1} \left[ n_{j_1} (x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2}) + * \right] \\[2.5em] x_{a_2} & = & n_{a_2} \left[ n_{j_1} (x_{j_1} - x_{off_1}) - n_{j_2} (x_{j_2} - + * x_{off_2}) \right] \f} *
*
* * where: - * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. - * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. - * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates - * (cf. SimpleTransmission class documentation for a more detailed description of this variable). - * - \f$ n \f$ represents a transmission ratio. Reducers/amplifiers are allowed on both the actuator and joint sides - * (depicted as timing belts in the figure). - * A transmission ratio can take any real value \e except zero. In particular: - * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute - * value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. - * - Negative values represent a direction flip, ie. input and output move in opposite directions. - * - Important: Use transmission ratio signs to match this class' convention of positive actuator/joint - * directions with a given mechanical design, as they will in general not match. + * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, + * respectively. + * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space + * variables, respectively. + * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position + * coordinates (cf. SimpleTransmission class documentation for a more detailed description of this + * variable). + * - \f$ n \f$ represents a transmission ratio. Reducers/amplifiers are allowed on both the actuator + * and joint sides (depicted as timing belts in the figure). A transmission ratio can take any real + * value \e except zero. In particular: + * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, + * while if its absolute value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. + * - Negative values represent a direction flip, ie. input and output move in opposite + * directions. + * - Important: Use transmission ratio signs to match this class' convention of positive + * actuator/joint directions with a given mechanical design, as they will in general not match. * - * \note This implementation currently assumes a specific layout for location of the actuators and joint axes which is - * common in robotic mechanisms. Please file an enhancement ticket if your use case does not adhere to this layout. + * \note This implementation currently assumes a specific layout for location of the actuators and + * joint axes which is common in robotic mechanisms. Please file an enhancement ticket if your use + * case does not adhere to this layout. * * \ingroup transmission_types */ @@ -126,14 +130,16 @@ class DifferentialTransmission : public Transmission /// Transform variables from actuator to joint space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void actuator_to_joint() override; /// Transform variables from joint to actuator space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void joint_to_actuator() override; diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index b8eb05e8d3..39f5df6f61 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -29,77 +29,80 @@ namespace transmission_interface { /// Implementation of a four-bar-linkage transmission. /** -* -* This transmission relates two actuators and two joints through a mechanism in which the state of the -* first joint only depends on the first actuator, while the second joint depends on both actuators, as -* illustrated below. -* Although the class name makes specific reference to the four-bar-linkage, there are other mechanical layouts -* that yield the same behavior, such as the remote actuation example also depicted below. -* \image html four_bar_linkage_transmission.png -* -*
-* -* -* -* -* -* -* -* -* -* -*
Effort
Velocity
Position
-* Actuator to joint -* -* \f{eqnarray*}{ -* \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\ -* \tau_{j_2} & = & n_{j_2} (n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1}) -* \f} -* -* \f{eqnarray*}{ -* \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} }{ n_{j_1} n_{a_1} } \\ -* \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_2} / n_{a_2} - \dot{x}_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } -* \f} -* -* \f{eqnarray*}{ -* x_{j_1} & = & \frac{ x_{a_1} }{ n_{j_1} n_{a_1} } + x_{off_1} \\ -* x_{j_2} & = & \frac{ x_{a_2} / n_{a_2} - x_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } + x_{off_2} -* \f} -*
-* Joint to actuator -* -* \f{eqnarray*}{ -* \tau_{a_1} & = & \tau_{j_1} / (n_{j_1} n_{a_1}) \\ -* \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} } -* \f} -* -* \f{eqnarray*}{ -* \dot{x}_{a_1} & = & n_{j_1} n_{a_1} \dot{x}_{j_1} \\ -* \dot{x}_{a_2} & = & n_{a_2} (\dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2}) -* \f} -* -* \f{eqnarray*}{ -* x_{a_1} & = & n_{j_1} n_{a_1} (x_{j_1} - x_{off_1}) \\ -* x_{a_2} & = & n_{a_2} \left[(x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2})\right] -* \f} -*
-*
-* -* where: -* - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. -* - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. -* - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates. -* (cf. SimpleTransmission class documentation for a more detailed description of this variable). -* - \f$ n \f$ represents a transmission ratio (reducers/amplifiers are depicted as timing belts in the figure). -* A transmission ratio can take any real value \e except zero. In particular: -* - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute -* value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. -* - Negative values represent a direction flip, ie. input and output move in opposite directions. -* - Important: Use transmission ratio signs to match this class' convention of positive actuator/joint -* directions with a given mechanical design, as they will in general not match. -* -* \ingroup transmission_types -*/ + * + * This transmission relates two actuators and two joints through a mechanism in which + * the state of the first joint only depends on the first actuator, while the second joint depends + * on both actuators, as illustrated below. Although the class name makes specific reference to the + * four-bar-linkage, there are other mechanical layouts that yield the same behavior, such as the + * remote actuation example also depicted below. \image html four_bar_linkage_transmission.png + * + *
+ * + * + * + * + * + * + * + * + * + * + *
Effort
Velocity
Position
+ * Actuator to joint + * + * \f{eqnarray*}{ + * \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\ + * \tau_{j_2} & = & n_{j_2} (n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1}) + * \f} + * + * \f{eqnarray*}{ + * \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} }{ n_{j_1} n_{a_1} } \\ + * \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_2} / n_{a_2} - \dot{x}_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} + * } \f} + * + * \f{eqnarray*}{ + * x_{j_1} & = & \frac{ x_{a_1} }{ n_{j_1} n_{a_1} } + x_{off_1} \\ + * x_{j_2} & = & \frac{ x_{a_2} / n_{a_2} - x_{a_1} / (n_{j_1} n_{a_1}) }{ n_{j_2} } + x_{off_2} + * \f} + *
+ * Joint to actuator + * + * \f{eqnarray*}{ + * \tau_{a_1} & = & \tau_{j_1} / (n_{j_1} n_{a_1}) \\ + * \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} } + * \f} + * + * \f{eqnarray*}{ + * \dot{x}_{a_1} & = & n_{j_1} n_{a_1} \dot{x}_{j_1} \\ + * \dot{x}_{a_2} & = & n_{a_2} (\dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2}) + * \f} + * + * \f{eqnarray*}{ + * x_{a_1} & = & n_{j_1} n_{a_1} (x_{j_1} - x_{off_1}) \\ + * x_{a_2} & = & n_{a_2} \left[(x_{j_1} - x_{off_1}) + n_{j_2} (x_{j_2} - x_{off_2})\right] + * \f} + *
+ *
+ * + * where: + * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, + * respectively. + * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space + * variables, respectively. + * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position + * coordinates. (cf. SimpleTransmission class documentation for a more detailed description of this + * variable). + * - \f$ n \f$ represents a transmission ratio (reducers/amplifiers are depicted as timing belts in + * the figure). A transmission ratio can take any real value \e except zero. In particular: + * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, + * while if its absolute value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. + * - Negative values represent a direction flip, ie. input and output move in opposite + * directions. + * - Important: Use transmission ratio signs to match this class' convention of positive + * actuator/joint directions with a given mechanical design, as they will in general not match. + * + * \ingroup transmission_types + */ class FourBarLinkageTransmission : public Transmission { public: @@ -125,14 +128,16 @@ class FourBarLinkageTransmission : public Transmission /// Transform variables from actuator to joint space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void actuator_to_joint() override; /// Transform variables from joint to actuator space. /** * \pre Actuator and joint vectors must have size 2 and point to valid data. - * To call this method it is not required that all other data vectors contain valid data, and can even remain empty. + * To call this method it is not required that all other data vectors contain valid data, and can + * even remain empty. */ void joint_to_actuator() override; diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index a74cc3dddd..809472ab75 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -28,9 +28,9 @@ namespace transmission_interface { /// Implementation of a simple reducer transmission. /** - * This transmission relates one actuator and one joint through a reductor (or amplifier). - * Timing belts and gears are examples of this transmission type, and are illustrated below. - * \image html simple_transmission.png + * This transmission relates one actuator and one joint through a reductor (or + * amplifier). Timing belts and gears are examples of this transmission type, and are illustrated + * below. \image html simple_transmission.png * *
* @@ -63,17 +63,20 @@ namespace transmission_interface * * * where: - * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively. - * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively. - * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates. - * - \f$ n \f$ is the transmission ratio, and can be computed as the ratio between the output and input pulley - * radii for the timing belt; or the ratio between output and input teeth for the gear system. - * The transmission ratio can take any real value \e except zero. In particular: - * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute - * value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. - * - Negative values represent a direction flip, ie. actuator and joint move in opposite directions. For example, - * in timing belts actuator and joint move in the same direction, while in single-stage gear systems actuator and - * joint move in opposite directions. + * - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, + * respectively. + * - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space + * variables, respectively. + * - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position + * coordinates. + * - \f$ n \f$ is the transmission ratio, and can be computed as the ratio between the output and + * input pulley radii for the timing belt; or the ratio between output and input teeth for the gear + * system. The transmission ratio can take any real value \e except zero. In particular: + * - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, + * while if its absolute value lies in \f$ (0, 1) \f$ it's a velocity amplifier / effort reducer. + * - Negative values represent a direction flip, ie. actuator and joint move in opposite + * directions. For example, in timing belts actuator and joint move in the same direction, while in + * single-stage gear systems actuator and joint move in opposite directions. * * \ingroup transmission_types */ @@ -102,14 +105,16 @@ class SimpleTransmission : public Transmission /// Transform variables from actuator to joint space. /** * This method operates on the handles provided when configuring the transmission. - * To call this method it is not required that all supported interface types are provided, e.g. one can supply only velocity handles + * To call this method it is not required that all supported interface types are provided, e.g. + * one can supply only velocity handles */ void actuator_to_joint() override; /// Transform variables from joint to actuator space. /** * This method operates on the handles provided when configuring the transmission. - * To call this method it is not required that all supported interface types are provided, e.g. one can supply only velocity handles + * To call this method it is not required that all supported interface types are provided, e.g. + * one can supply only velocity handles */ void joint_to_actuator() override; diff --git a/transmission_interface/include/transmission_interface/transmission.hpp b/transmission_interface/include/transmission_interface/transmission.hpp index 8ca0196c4c..697e1a4eb7 100644 --- a/transmission_interface/include/transmission_interface/transmission.hpp +++ b/transmission_interface/include/transmission_interface/transmission.hpp @@ -26,21 +26,23 @@ namespace transmission_interface { /// Abstract base class for representing mechanical transmissions. /** - * Mechanical transmissions transform effort/flow variables such that their product (power) remains constant. - * Effort variables for linear and rotational domains are \e force and \e torque; while the flow variables are - * respectively linear velocity and angular velocity. + * Mechanical transmissions transform effort/flow variables such that their product (power) remains + * constant. Effort variables for linear and rotational domains are \e force and \e torque; while + * the flow variables are respectively linear velocity and angular velocity. * - * In robotics it is customary to place transmissions between actuators and joints. This interface adheres to this - * naming to identify the input and output spaces of the transformation. - * The provided interfaces allow bidirectional mappings between actuator and joint spaces for effort, velocity and - * position. Position is not a power variable, but the mappings can be implemented using the velocity map plus an - * integration constant representing the offset between actuator and joint zeros. + * In robotics it is customary to place transmissions between actuators and joints. This interface + * adheres to this naming to identify the input and output spaces of the transformation. The + * provided interfaces allow bidirectional mappings between actuator and joint spaces for effort, + * velocity and position. Position is not a power variable, but the mappings can be implemented + * using the velocity map plus an integration constant representing the offset between actuator and + * joint zeros. * * \par Credit - * This interface was inspired by similar existing implementations by PAL Robotics, S.L. and Willow Garage Inc. + * This interface was inspired by similar existing implementations by PAL Robotics, S.L. and Willow + * Garage Inc. * - * \note Implementations of this interface must take care of realtime-safety if the code is to be run in realtime - * contexts, as is often the case in robot control. + * \note Implementations of this interface must take care of realtime-safety if the code is to be + * run in realtime contexts, as is often the case in robot control. */ class Transmission { @@ -55,20 +57,20 @@ class Transmission /** * \param[in] act_data Actuator-space variables. * \param[out] jnt_data Joint-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the + * number of transmission actuators and joints. Data vectors not used in this map can remain + * empty. */ virtual void actuator_to_joint() = 0; /// Transform \e effort variables from joint to actuator space. /** - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the number of - * transmission actuators and joints. - * Data vectors not used in this map can remain empty. - */ + * \param[in] jnt_data Joint-space variables. + * \param[out] act_data Actuator-space variables. + * \pre All non-empty vectors must contain valid data and their size should be consistent with the + * number of transmission actuators and joints. Data vectors not used in this map can remain + * empty. + */ virtual void joint_to_actuator() = 0; /** \return Number of actuators managed by transmission,