joint_velocity_limits.h

Go to the documentation of this file.

// Copyright (c) 2025 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once
 
#include <array>
#include <string>
 
namespace franka {
 
struct PositionBasedJointVelocityLimitConstants {
  double max_velocity;
  double velocity_offset;
  double deceleration_limit;
  double upper_position_limit;
  double lower_position_limit;
 
  PositionBasedJointVelocityLimitConstants()
      : max_velocity(0),
        velocity_offset(0),
        deceleration_limit(0),
        upper_position_limit(0),
        lower_position_limit(0) {}
 
  PositionBasedJointVelocityLimitConstants(double max_velocity,
                                           double velocity_offset,
                                           double deceleration_limit,
                                           double upper_position_limit,
                                           double lower_position_limit)
      : max_velocity(max_velocity),
        velocity_offset(velocity_offset),
        deceleration_limit(deceleration_limit),
        upper_position_limit(upper_position_limit),
        lower_position_limit(lower_position_limit) {}
};
 
class JointVelocityLimitsConfig {
 public:
  // Robot configuration constants
  static constexpr int kNumJoints = 7;
 
  JointVelocityLimitsConfig() = default;
 
  explicit JointVelocityLimitsConfig(const std::string& urdf_string) { parseFromURDF(urdf_string); }
 
  const PositionBasedJointVelocityLimitConstants& operator[](size_t joint_index) const {
    return joint_params_[joint_index];
  }
 
  const std::array<PositionBasedJointVelocityLimitConstants, kNumJoints>& getJointParams() const {
    return joint_params_;
  }
 
  std::array<double, kNumJoints> getUpperJointVelocityLimits(
      const std::array<double, kNumJoints>& joint_positions) const;
 
  std::array<double, kNumJoints> getLowerJointVelocityLimits(
      const std::array<double, kNumJoints>& joint_positions) const;
 
 private:
  // XML element and attribute name constants
  static constexpr const char* kRobotElementName = "robot";
  static constexpr const char* kJointElementName = "joint";
  static constexpr const char* kPositionBasedVelocityLimitsElementName =
      "position_based_velocity_limits";
  static constexpr const char* kLimitElementName = "limit";
  static constexpr const char* kNameAttributeName = "name";
  static constexpr const char* kVelocityAttributeName = "velocity";
  static constexpr const char* kUpperAttributeName = "upper";
  static constexpr const char* kLowerAttributeName = "lower";
  static constexpr const char* kVelocityOffsetAttributeName = "velocity_offset";
  static constexpr const char* kDecelerationLimitAttributeName = "deceleration_limit";
 
  // Joint name pattern constants
  static constexpr const char* kJoint1Name = "joint1";
  static constexpr const char* kJoint2Name = "joint2";
  static constexpr const char* kJoint3Name = "joint3";
  static constexpr const char* kJoint4Name = "joint4";
  static constexpr const char* kJoint5Name = "joint5";
  static constexpr const char* kJoint6Name = "joint6";
  static constexpr const char* kJoint7Name = "joint7";
 
  void parseFromURDF(const std::string& urdf_string);
 
  static int getJointIndex(const std::string& joint_name);
 
  std::array<PositionBasedJointVelocityLimitConstants, kNumJoints> joint_params_;
};
 
}  // namespace franka