Class JointLimitsAggregator

Class Documentation

class JointLimitsAggregator

Unifies the joint limits from the given joint models with joint limits from the node parameters.

Does not support MultiDOF joints.

Public Static Functions

static JointLimitsContainer getAggregatedLimits(const rclcpp::Node::SharedPtr &node, const std::string &param_namespace, const std::vector<const moveit::core::JointModel*> &joint_models)

Aggregates(combines) the joint limits from joint model and node parameters. The rules for the combination are:

  1. Position and velocity limits in the node parameters must be stricter or equal if they are defined.

  2. Limits in the node parameters where the corresponding has_<position|velocity|acceleration|deceleration>_limits are „false“ are considered undefined(see point 1).

  3. Not all joints have to be limited by the node parameters. Selective limitation is possible.

  4. If max_deceleration is unset, it will be set to: max_deceleration = - max_acceleration.

Note

The acceleration/deceleration can only be set via the node parameters parameter since they are not supported in the urdf so far.

Parameters:
  • node – Node to use for accessing joint limit parameters

  • param_namespace – Namespace to use for looking up node parameters

  • joint_models – The joint models

Returns:

Container containing the limits

Protected Static Functions

static void updatePositionLimitFromJointModel(const moveit::core::JointModel *joint_model, JointLimit &joint_limit)

Update the position limits with the ones from the joint_model.

If the joint model has no position limit, the value is unchanged.

Parameters:
  • joint_model – The joint model

  • joint_limit – The joint_limit to be filled with new values.

static void updateVelocityLimitFromJointModel(const moveit::core::JointModel *joint_model, JointLimit &joint_limit)

Update the velocity limit with the one from the joint_model.

If the joint model has no velocity limit, the value is unchanged.

Parameters:
  • joint_model – The joint model

  • joint_limit – The joint_limit to be filled with new values.

static void checkPositionBoundsThrowing(const moveit::core::JointModel *joint_model, const JointLimit &joint_limit)

Checks if the position limits from the given joint_limit are stricter than the limits of the joint_model. Throws AggregationBoundsViolationException on violation.

Parameters:
  • joint_model – The joint_model

  • joint_limit – The joint_limit

static void checkVelocityBoundsThrowing(const moveit::core::JointModel *joint_model, const JointLimit &joint_limit)

Checks if the velocity limit from the given joint_limit are stricter than the limit of the joint_model. Throws AggregationBoundsViolationException on violation.

Parameters:
  • joint_model – The joint_model

  • joint_limit – The joint_limit