rate_limiting.h

Go to the documentation of this file.

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once

#include <array>
#include <limits>

namespace franka {
constexpr double kDeltaT = 1e-3;
constexpr double kLimitEps = 1e-3;
constexpr double kNormEps = std::numeric_limits<double>::epsilon();
constexpr double kTolNumberPacketsLost = 3.0;
constexpr double kFactorCartesianRotationPoseInterface = 0.99;
constexpr std::array<double, 7> kMaxTorqueRate{
    {1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps,
     1000 - kLimitEps, 1000 - kLimitEps}};
constexpr std::array<double, 7> kMaxJointJerk{
    {7500.0 - kLimitEps, 3750.0 - kLimitEps, 5000.0 - kLimitEps, 6250.0 - kLimitEps,
     7500.0 - kLimitEps, 10000.0 - kLimitEps, 10000.0 - kLimitEps}};
constexpr std::array<double, 7> kMaxJointAcceleration{
    {15.0000 - kLimitEps, 7.500 - kLimitEps, 10.0000 - kLimitEps, 12.5000 - kLimitEps,
     15.0000 - kLimitEps, 20.0000 - kLimitEps, 20.0000 - kLimitEps}};
constexpr std::array<double, 7> kMaxJointVelocity{
    {2.1750 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxJointAcceleration[0],
     2.1750 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[1],
     2.1750 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[2],
     2.1750 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[3],
     2.6100 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[4],
     2.6100 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[5],
     2.6100 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[6]}};
constexpr double kMaxTranslationalJerk = 6500.0 - kLimitEps;
constexpr double kMaxTranslationalAcceleration = 13.0000 - kLimitEps;
constexpr double kMaxTranslationalVelocity =
    1.7000 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxTranslationalAcceleration;
constexpr double kMaxRotationalJerk = 12500.0 - kLimitEps;
constexpr double kMaxRotationalAcceleration = 25.0000 - kLimitEps;
constexpr double kMaxRotationalVelocity =
    2.5000 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxRotationalAcceleration;
constexpr double kMaxElbowJerk = 5000 - kLimitEps;
constexpr double kMaxElbowAcceleration = 10.0000 - kLimitEps;
constexpr double kMaxElbowVelocity =
    2.1750 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxElbowAcceleration;

std::array<double, 7> limitRate(const std::array<double, 7>& max_derivatives,
                                const std::array<double, 7>& commanded_values,
                                const std::array<double, 7>& last_commanded_values);

double limitRate(double max_velocity,
                 double max_acceleration,
                 double max_jerk,
                 double commanded_velocity,
                 double last_commanded_velocity,
                 double last_commanded_acceleration);

double limitRate(double max_velocity,
                 double max_acceleration,
                 double max_jerk,
                 double commanded_position,
                 double last_commanded_position,
                 double last_commanded_velocity,
                 double last_commanded_acceleration);

std::array<double, 7> limitRate(const std::array<double, 7>& max_velocity,
                                const std::array<double, 7>& max_acceleration,
                                const std::array<double, 7>& max_jerk,
                                const std::array<double, 7>& commanded_velocities,
                                const std::array<double, 7>& last_commanded_velocities,
                                const std::array<double, 7>& last_commanded_accelerations);

std::array<double, 7> limitRate(const std::array<double, 7>& max_velocity,
                                const std::array<double, 7>& max_acceleration,
                                const std::array<double, 7>& max_jerk,
                                const std::array<double, 7>& commanded_positions,
                                const std::array<double, 7>& last_commanded_positions,
                                const std::array<double, 7>& last_commanded_velocities,
                                const std::array<double, 7>& last_commanded_accelerations);

std::array<double, 6> limitRate(
    double max_translational_velocity,
    double max_translational_acceleration,
    double max_translational_jerk,
    double max_rotational_velocity,
    double max_rotational_acceleration,
    double max_rotational_jerk,
    const std::array<double, 6>& O_dP_EE_c,         // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_dP_EE_c,    // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_ddP_EE_c);  // NOLINT(readability-identifier-naming)

std::array<double, 16> limitRate(
    double max_translational_velocity,
    double max_translational_acceleration,
    double max_translational_jerk,
    double max_rotational_velocity,
    double max_rotational_acceleration,
    double max_rotational_jerk,
    const std::array<double, 16>& O_T_EE_c,         // NOLINT(readability-identifier-naming)
    const std::array<double, 16>& last_O_T_EE_c,    // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_dP_EE_c,    // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_ddP_EE_c);  // NOLINT(readability-identifier-naming)

}  // namespace franka