Template Class JointLimiterInterface

Inheritance Relationships

Derived Types

Class Documentation

template<typename JointLimitsStateDataType>
class JointLimiterInterface

Subclassed by joint_limits::JointSaturationLimiter< JointControlInterfacesData >, joint_limits::JointSaturationLimiter< JointLimitsStateDataType >

Public Functions

JointLimiterInterface() = default
virtual ~JointLimiterInterface() = default
inline virtual bool init(const std::vector<std::string> &joint_names, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &param_itf, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr &logging_itf, const std::string &robot_description_topic = "/robot_description")

Initialization of every JointLimiter.

Initialization of JointLimiter for defined joints with their names. Robot description topic provides a topic name where URDF of the robot can be found. This is needed to use joint limits from URDF (not implemented yet!). Override this method only if initialization and reading joint limits should be adapted. Otherwise, initialize your custom limiter in on_limit method.

Parameters:
  • joint_names[in] names of joints where limits should be applied.

  • param_itf[in] node parameters interface object to access parameters.

  • logging_itf[in] node logging interface to log if error happens.

  • robot_description_topic[in] string of a topic where robot description is accessible.

inline virtual bool init(const std::vector<std::string> &joint_names, const std::vector<joint_limits::JointLimits> &joint_limits, const std::vector<joint_limits::SoftJointLimits> &soft_joint_limits, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &param_itf, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr &logging_itf)

Wrapper init method that accepts the joint names and their limits directly

inline virtual bool init(const std::vector<std::string> &joint_names, const rclcpp::Node::SharedPtr &node, const std::string &robot_description_topic = "/robot_description")

Wrapper init method that accepts pointer to the Node. For details see other init method.

inline virtual bool init(const std::vector<std::string> &joint_names, const rclcpp_lifecycle::LifecycleNode::SharedPtr &lifecycle_node, const std::string &robot_description_topic = "/robot_description")

Wrapper init method that accepts pointer to the LifecycleNode. For details see other init method.

inline virtual bool configure(const JointLimitsStateDataType &current_joint_states)
inline virtual bool enforce(const JointLimitsStateDataType &current_joint_states, JointLimitsStateDataType &desired_joint_states, const rclcpp::Duration &dt)

Enforce joint limits to desired joint state for multiple physical quantities.

Generic enforce method that calls implementation-specific on_enforce method.

Parameters:
  • current_joint_states[in] current joint states a robot is in.

  • desired_joint_states[inout] joint state that should be adjusted to obey the limits.

  • dt[in] time delta to calculate missing integrals and derivation in joint limits.

Returns:

true if limits are enforced, otherwise false.

Protected Functions

virtual bool on_init() = 0

Method is realized by an implementation.

Implementation-specific initialization of limiter’s internal states and libraries.

Returns:

true if initialization was successful, otherwise false.

virtual bool on_configure(const JointLimitsStateDataType &current_joint_states) = 0

Method is realized by an implementation.

Implementation-specific configuration of limiter’s internal states and libraries.

Returns:

true if initialization was successful, otherwise false.

virtual bool on_enforce(const JointLimitsStateDataType &current_joint_states, JointLimitsStateDataType &desired_joint_states, const rclcpp::Duration &dt) = 0

Method is realized by an implementation.

Filter-specific implementation of the joint limits enforce algorithm for multiple dependent physical quantities.

Parameters:
  • current_joint_states[in] current joint states a robot is in.

  • desired_joint_states[inout] joint state that should be adjusted to obey the limits.

  • dt[in] time delta to calculate missing integrals and derivation in joint limits.

Returns:

true if limits are enforced, otherwise false.

inline bool has_logging_interface() const

Checks if the logging interface is set.

Note

this way of interfacing would be useful for instances where the logging interface is not available, for example in the ResourceManager or ResourceStorage classes.

Returns:

true if the logging interface is available, otherwise false.

inline bool has_parameter_interface() const

Checks if the parameter interface is set.

Note

this way of interfacing would be useful for instances where the logging interface is not available, for example in the ResourceManager or ResourceStorage classes.

Returns:

true if the parameter interface is available, otherwise false.

Protected Attributes

size_t number_of_joints_
std::vector<std::string> joint_names_
std::vector<joint_limits::JointLimits> joint_limits_
std::vector<joint_limits::SoftJointLimits> soft_joint_limits_
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_itf_
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_itf_