rate_limiting.h File Reference

Contains functions for limiting the rate of torques, Cartesian pose, Cartesian velocity, joint position and joint velocity. More...

#include <algorithm>
#include <array>
#include <cmath>
#include <limits>

Include dependency graph for rate_limiting.h:

Go to the source code of this file.

Functions
std::array< double, 7 >	franka::computeUpperLimitsJointVelocity (const std::array< double, 7 > &q)
	Computes the maximum joint velocity based on joint position.
std::array< double, 7 >	franka::computeLowerLimitsJointVelocity (const std::array< double, 7 > &q)
	Computes the minimum joint velocity based on joint position.
std::array< double, 7 >	franka::limitRate (const std::array< double, 7 > &max_derivatives, const std::array< double, 7 > &commanded_values, const std::array< double, 7 > &last_commanded_values)
	Limits the rate of an input vector of per-joint commands considering the maximum allowed time derivatives.
double	franka::limitRate (double upper_limits_velocity, double lower_limits_velocity, double max_acceleration, double max_jerk, double commanded_velocity, double last_commanded_velocity, double last_commanded_acceleration)
	Limits the rate of a desired joint velocity considering the limits provided.
double	franka::limitRate (double upper_limits_velocity, double lower_limits_velocity, double max_acceleration, double max_jerk, double commanded_position, double last_commanded_position, double last_commanded_velocity, double last_commanded_acceleration)
	Limits the rate of a desired joint position considering the limits provided.
std::array< double, 7 >	franka::limitRate (const std::array< double, 7 > &upper_limits_velocity, const std::array< double, 7 > &lower_limits_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)
	Limits the rate of a desired joint velocity considering the limits provided.
std::array< double, 7 >	franka::limitRate (const std::array< double, 7 > &upper_limits_velocity, const std::array< double, 7 > &lower_limits_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)
	Limits the rate of a desired joint position considering the limits provided.
std::array< double, 6 >	franka::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, const std::array< double, 6 > &last_O_dP_EE_c, const std::array< double, 6 > &last_O_ddP_EE_c)
	Limits the rate of a desired Cartesian velocity considering the limits provided.
std::array< double, 16 >	franka::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, const std::array< double, 16 > &last_O_T_EE_c, const std::array< double, 6 > &last_O_dP_EE_c, const std::array< double, 6 > &last_O_ddP_EE_c)
	Limits the rate of a desired Cartesian pose considering the limits provided.

Variables
constexpr double	franka::kDeltaT = 1e-3
	Sample time constant.
constexpr double	franka::kLimitEps = 1e-3
	Epsilon value for checking limits.
constexpr double	franka::kNormEps = std::numeric_limits<double>::epsilon()
	Epsilon value for limiting Cartesian accelerations/jerks or not.
constexpr double	franka::kTolNumberPacketsLost = 0.0
	Number of packets lost considered for the definition of velocity limits.
constexpr double	franka::kFactorCartesianRotationPoseInterface = 0.99
	Factor for the definition of rotational limits using the Cartesian Pose interface.
constexpr std::array< double, 7 >	franka::kMaxTorqueRate
	Maximum torque rate.
constexpr std::array< double, 7 >	franka::kMaxJointJerk
	Maximum joint jerk.
constexpr std::array< double, 7 >	franka::kMaxJointAcceleration
	Maximum joint acceleration.
constexpr std::array< double, 7 >	franka::kJointVelocityLimitsTolerance
	Tolerance value for joint velocity limits to deal with numerical errors and data losses.
constexpr double	franka::kMaxTranslationalJerk = 4500.0 - kLimitEps
	Maximum translational jerk.
constexpr double	franka::kMaxTranslationalAcceleration = 9.0000 - kLimitEps
	Maximum translational acceleration.
constexpr double	franka::kMaxTranslationalVelocity
	Maximum translational velocity.
constexpr double	franka::kMaxRotationalJerk = 8500.0 - kLimitEps
	Maximum rotational jerk.
constexpr double	franka::kMaxRotationalAcceleration = 17.0000 - kLimitEps
	Maximum rotational acceleration.
constexpr double	franka::kMaxRotationalVelocity
	Maximum rotational velocity.
constexpr double	franka::kMaxElbowJerk = 5000 - kLimitEps
	Maximum elbow jerk.
constexpr double	franka::kMaxElbowAcceleration = 10.0000 - kLimitEps
	Maximum elbow acceleration.
constexpr double	franka::kMaxElbowVelocity
	Maximum elbow velocity.

Detailed Description

Contains functions for limiting the rate of torques, Cartesian pose, Cartesian velocity, joint position and joint velocity.

Function Documentation

computeLowerLimitsJointVelocity()

std::array< double, 7 > franka::computeLowerLimitsJointVelocity ( const std::array< double, 7 > &  q )

inline

Computes the minimum joint velocity based on joint position.

Note

The implementation is based on https://frankaemika.github.io/docs/control_parameters.html#limits-for-franka-research-3.

Parameters

[in]

q

joint position.

Returns

Lower limits of joint velocity at the given joint position.

computeUpperLimitsJointVelocity()

std::array< double, 7 > franka::computeUpperLimitsJointVelocity ( const std::array< double, 7 > &  q )

inline

Computes the maximum joint velocity based on joint position.

Note

The implementation is based on https://frankaemika.github.io/docs/control_parameters.html#limits-for-franka-research-3.

Parameters

[in]

q

joint position.

Returns

Upper limits of joint velocity at the given joint position.

limitRate() [1/7]

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

Limits the rate of an input vector of per-joint commands considering the maximum allowed time derivatives.

Note

FCI filters must be deactivated to work properly.

Parameters

[in]	max_derivatives	Per-joint maximum allowed time derivative.
[in]	commanded_values	Commanded values of the current time step.
[in]	last_commanded_values	Commanded values of the previous time step.

Exceptions

std::invalid_argument

if commanded_values are infinite or NaN.

Returns

Rate-limited vector of desired values.

Examples

joint_impedance_control.cpp.

limitRate() [2/7]

std::array< double, 7 > franka::limitRate	(	const std::array< double, 7 > &	upper_limits_velocity,
		const std::array< double, 7 > &	lower_limits_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
	)

Limits the rate of a desired joint position considering the limits provided.

Note

FCI filters must be deactivated to work properly.

Parameters

[in]	upper_limits_velocity	Per-joint upper limits of allowed velocity.
[in]	lower_limits_velocity	Per-joint lower limits of allowed velocity.
[in]	max_acceleration	Per-joint maximum allowed acceleration.
[in]	max_jerk	Per-joint maximum allowed jerk.
[in]	commanded_positions	Commanded joint positions of the current time step.
[in]	last_commanded_positions	Commanded joint positions of the previous time step.
[in]	last_commanded_velocities	Commanded joint velocities of the previous time step.
[in]	last_commanded_accelerations	Commanded joint accelerations of the previous time step.

Exceptions

std::invalid_argument

if commanded_positions are infinite or NaN.

Returns

Rate-limited vector of desired joint positions.

limitRate() [3/7]

std::array< double, 7 > franka::limitRate	(	const std::array< double, 7 > &	upper_limits_velocity,
		const std::array< double, 7 > &	lower_limits_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
	)

Limits the rate of a desired joint velocity considering the limits provided.

Note

FCI filters must be deactivated to work properly.

Parameters

[in]	upper_limits_velocity	Per-joint upper limits of allowed velocity.
[in]	lower_limits_velocity	Per-joint lower limits of allowed velocity.
[in]	max_acceleration	Per-joint maximum allowed acceleration.
[in]	max_jerk	Per-joint maximum allowed jerk.
[in]	commanded_velocities	Commanded joint velocity of the current time step.
[in]	last_commanded_velocities	Commanded joint velocities of the previous time step.
[in]	last_commanded_accelerations	Commanded joint accelerations of the previous time step.

Exceptions

std::invalid_argument

if commanded_velocities are infinite or NaN.

Returns

Rate-limited vector of desired joint velocities.

limitRate() [4/7]

std::array< double, 16 > franka::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,
		const std::array< double, 16 > &	last_O_T_EE_c,
		const std::array< double, 6 > &	last_O_dP_EE_c,
		const std::array< double, 6 > &	last_O_ddP_EE_c
	)

Limits the rate of a desired Cartesian pose considering the limits provided.

Note

FCI filters must be deactivated to work properly.

Parameters

[in]	max_translational_velocity	Maximum translational velocity.
[in]	max_translational_acceleration	Maximum translational acceleration.
[in]	max_translational_jerk	Maximum translational jerk.
[in]	max_rotational_velocity	Maximum rotational velocity.
[in]	max_rotational_acceleration	Maximum rotational acceleration.
[in]	max_rotational_jerk	Maximum rotational jerk.
[in]	O_T_EE_c	Commanded pose of the current time step.
[in]	last_O_T_EE_c	Commanded pose of the previous time step.
[in]	last_O_dP_EE_c	Commanded end effector twist of the previous time step.
[in]	last_O_ddP_EE_c	Commanded end effector acceleration of the previous time step.

Exceptions

std::invalid_argument

if an element of O_T_EE_c is infinite or NaN.

Returns

Rate-limited desired pose.

limitRate() [5/7]

std::array< double, 6 > franka::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,
		const std::array< double, 6 > &	last_O_dP_EE_c,
		const std::array< double, 6 > &	last_O_ddP_EE_c
	)

Limits the rate of a desired Cartesian velocity considering the limits provided.

Note

FCI filters must be deactivated to work properly.

Parameters

[in]	max_translational_velocity	Maximum translational velocity.
[in]	max_translational_acceleration	Maximum translational acceleration.
[in]	max_translational_jerk	Maximum translational jerk.
[in]	max_rotational_velocity	Maximum rotational velocity.
[in]	max_rotational_acceleration	Maximum rotational acceleration.
[in]	max_rotational_jerk	Maximum rotational jerk.
[in]	O_dP_EE_c	Commanded end effector twist of the current time step.
[in]	last_O_dP_EE_c	Commanded end effector twist of the previous time step.
[in]	last_O_ddP_EE_c	Commanded end effector acceleration of the previous time step.

Exceptions

std::invalid_argument

if an element of O_dP_EE_c is infinite or NaN.

Returns

Rate-limited desired end effector twist.

limitRate() [6/7]

double franka::limitRate	(	double	upper_limits_velocity,
		double	lower_limits_velocity,
		double	max_acceleration,
		double	max_jerk,
		double	commanded_position,
		double	last_commanded_position,
		double	last_commanded_velocity,
		double	last_commanded_acceleration
	)

Limits the rate of a desired joint position considering the limits provided.

Note

FCI filters must be deactivated to work properly.

Parameters

[in]	upper_limits_velocity	Upper limits of allowed velocity.
[in]	lower_limits_velocity	Lower limits of allowed velocity.
[in]	max_acceleration	Maximum allowed acceleration.
[in]	max_jerk	Maximum allowed jerk.
[in]	commanded_position	Commanded joint position of the current time step.
[in]	last_commanded_position	Commanded joint position of the previous time step.
[in]	last_commanded_velocity	Commanded joint velocity of the previous time step.
[in]	last_commanded_acceleration	Commanded joint acceleration of the previous time step.

Exceptions

std::invalid_argument

if commanded_position is infinite or NaN.

Returns

Rate-limited desired joint position.

limitRate() [7/7]

double franka::limitRate	(	double	upper_limits_velocity,
		double	lower_limits_velocity,
		double	max_acceleration,
		double	max_jerk,
		double	commanded_velocity,
		double	last_commanded_velocity,
		double	last_commanded_acceleration
	)

Limits the rate of a desired joint velocity considering the limits provided.

Note

FCI filters must be deactivated to work properly.

Parameters

[in]	upper_limits_velocity	Upper limits of allowed velocity.
[in]	lower_limits_velocity	Lower limits of allowed velocity.
[in]	max_acceleration	Maximum allowed acceleration.
[in]	max_jerk	Maximum allowed jerk.
[in]	commanded_velocity	Commanded joint velocity of the current time step.
[in]	last_commanded_velocity	Commanded joint velocity of the previous time step.
[in]	last_commanded_acceleration	Commanded joint acceleration of the previous time step.

Exceptions

std::invalid_argument

if commanded_velocity is infinite or NaN.

Returns

Rate-limited desired joint velocity.

Variable Documentation

kJointVelocityLimitsTolerance

constexpr std::array<double, 7> franka::kJointVelocityLimitsTolerance

constexpr

Initial value:

{
    kLimitEps + kTolNumberPacketsLost * kDeltaT * kMaxJointAcceleration[0],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[1],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[2],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[3],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[4],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[5],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[6],
}

Tolerance value for joint velocity limits to deal with numerical errors and data losses.

kMaxElbowVelocity

constexpr double franka::kMaxElbowVelocity

constexpr

Initial value:

=
    1.5000 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxElbowAcceleration

Maximum elbow velocity.

kMaxJointAcceleration

constexpr std::array<double, 7> franka::kMaxJointAcceleration

constexpr

Initial value:

{
    {10.0000 - kLimitEps, 10.0000 - kLimitEps, 10.0000 - kLimitEps, 10.0000 - kLimitEps,
     10.0000 - kLimitEps, 10.0000 - kLimitEps, 10.0000 - kLimitEps}}

Maximum joint acceleration.

kMaxJointJerk

constexpr std::array<double, 7> franka::kMaxJointJerk

constexpr

Initial value:

{
    {5000.0 - kLimitEps, 5000.0 - kLimitEps, 5000.0 - kLimitEps, 5000.0 - kLimitEps,
     5000.0 - kLimitEps, 5000.0 - kLimitEps, 5000.0 - kLimitEps}}

Maximum joint jerk.

kMaxRotationalVelocity

constexpr double franka::kMaxRotationalVelocity

constexpr

Initial value:

=
    2.5000 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxRotationalAcceleration

Maximum rotational velocity.

kMaxTorqueRate

constexpr std::array<double, 7> franka::kMaxTorqueRate

constexpr

Initial value:

{
    {1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps,
     1000 - kLimitEps, 1000 - kLimitEps}}

Maximum torque rate.

Examples

joint_impedance_control.cpp.

kMaxTranslationalVelocity

constexpr double franka::kMaxTranslationalVelocity

constexpr

Initial value:

=
    3.0000 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxTranslationalAcceleration

Maximum translational velocity.

kTolNumberPacketsLost

constexpr double franka::kTolNumberPacketsLost = 0.0

constexpr

Number of packets lost considered for the definition of velocity limits.

When a packet is lost, FCI assumes a constant acceleration model. For FR3 there are no expected package loses. Therefore this number is set to 0. If you encounter package loses with your setup you can increase this number