Program Listing for File input_parameter.hpp
↰ Return to documentation for file (include/ruckig/input_parameter.hpp)
#pragma once
#include <array>
#include <iomanip>
#include <limits>
#include <optional>
#include <sstream>
#include <type_traits>
#include <vector>
#include <ruckig/error.hpp>
#include <ruckig/result.hpp>
#include <ruckig/utils.hpp>
namespace ruckig {
enum class ControlInterface {
Position,
Velocity,
};
enum class Synchronization {
Time,
TimeIfNecessary,
Phase,
// PhaseOnly, ///< Always phase synchronize the DoFs (even when this is not time-optimal), else returns "ErrorNoPhaseSynchronization". Ruckig will then guarantee a straight-line trajectory
None,
};
enum class DurationDiscretization {
Continuous,
Discrete,
};
template<size_t DOFs, template<class, size_t> class CustomVector = StandardVector>
class InputParameter {
template<class T> using Vector = CustomVector<T, DOFs>;
inline static double v_at_a_zero(double v0, double a0, double j) {
return v0 + (a0 * a0) / (2 * j);
}
void initialize() {
for (size_t dof = 0; dof < degrees_of_freedom; ++dof) {
current_velocity[dof] = 0.0;
current_acceleration[dof] = 0.0;
target_velocity[dof] = 0.0;
target_acceleration[dof] = 0.0;
max_acceleration[dof] = std::numeric_limits<double>::infinity();
max_jerk[dof] = std::numeric_limits<double>::infinity();
enabled[dof] = true;
}
}
void resize(size_t dofs) {
current_position.resize(dofs);
current_velocity.resize(dofs);
current_acceleration.resize(dofs);
target_position.resize(dofs);
target_velocity.resize(dofs);
target_acceleration.resize(dofs);
max_velocity.resize(dofs);
max_acceleration.resize(dofs);
max_jerk.resize(dofs);
enabled.resize(dofs);
}
#if defined WITH_CLOUD_CLIENT
void reserve(size_t max_number_of_waypoints) {
intermediate_positions.reserve(max_number_of_waypoints);
}
#endif
public:
size_t degrees_of_freedom;
ControlInterface control_interface {ControlInterface::Position};
Synchronization synchronization {Synchronization::Time};
DurationDiscretization duration_discretization {DurationDiscretization::Continuous};
Vector<double> current_position, current_velocity, current_acceleration;
Vector<double> target_position, target_velocity, target_acceleration;
Vector<double> max_velocity, max_acceleration, max_jerk;
std::optional<Vector<double>> min_velocity, min_acceleration;
std::vector<Vector<double>> intermediate_positions;
std::optional<std::vector<Vector<double>>> per_section_max_velocity, per_section_max_acceleration, per_section_max_jerk;
std::optional<std::vector<Vector<double>>> per_section_min_velocity, per_section_min_acceleration;
std::optional<std::vector<Vector<double>>> per_section_max_position, per_section_min_position;
std::optional<Vector<double>> max_position, min_position;
Vector<bool> enabled;
std::optional<Vector<ControlInterface>> per_dof_control_interface;
std::optional<Vector<Synchronization>> per_dof_synchronization;
std::optional<double> minimum_duration;
std::optional<std::vector<double>> per_section_minimum_duration;
std::optional<double> interrupt_calculation_duration;
template<size_t D = DOFs, typename std::enable_if<(D >= 1), int>::type = 0>
InputParameter(): degrees_of_freedom(DOFs) {
initialize();
}
template<size_t D = DOFs, typename std::enable_if<(D == 0), int>::type = 0>
InputParameter(size_t dofs): degrees_of_freedom(dofs) {
resize(dofs);
initialize();
}
#if defined WITH_CLOUD_CLIENT
template<size_t D = DOFs, typename std::enable_if<(D >= 1), int>::type = 0>
InputParameter(size_t max_number_of_waypoints): degrees_of_freedom(DOFs) {
reserve(max_number_of_waypoints);
initialize();
}
template<size_t D = DOFs, typename std::enable_if<(D == 0), int>::type = 0>
InputParameter(size_t dofs, size_t max_number_of_waypoints): degrees_of_freedom(dofs) {
reserve(max_number_of_waypoints);
resize(dofs);
initialize();
}
#endif
template<bool throw_validation_error = true>
bool validate(bool check_current_state_within_limits = false, bool check_target_state_within_limits = true) const {
for (size_t dof = 0; dof < degrees_of_freedom; ++dof) {
const double jMax = max_jerk[dof];
if (std::isnan(jMax) || jMax < 0.0) {
if constexpr (throw_validation_error) {
throw RuckigError("maximum jerk limit " + std::to_string(jMax) + " of DoF " + std::to_string(dof) + " should be larger than or equal to zero.");
}
return false;
}
const double aMax = max_acceleration[dof];
if (std::isnan(aMax) || aMax < 0.0) {
if constexpr (throw_validation_error) {
throw RuckigError("maximum acceleration limit " + std::to_string(aMax) + " of DoF " + std::to_string(dof) + " should be larger than or equal to zero.");
}
return false;
}
const double aMin = min_acceleration ? min_acceleration.value()[dof] : -max_acceleration[dof];
if (std::isnan(aMin) || aMin > 0.0) {
if constexpr (throw_validation_error) {
throw RuckigError("minimum acceleration limit " + std::to_string(aMin) + " of DoF " + std::to_string(dof) + " should be smaller than or equal to zero.");
}
return false;
}
const double a0 = current_acceleration[dof];
if (std::isnan(a0)) {
if constexpr (throw_validation_error) {
throw RuckigError("current acceleration " + std::to_string(a0) + " of DoF " + std::to_string(dof) + " should be a valid number.");
}
return false;
}
const double af = target_acceleration[dof];
if (std::isnan(af)) {
if constexpr (throw_validation_error) {
throw RuckigError("target acceleration " + std::to_string(af) + " of DoF " + std::to_string(dof) + " should be a valid number.");
}
return false;
}
if (check_current_state_within_limits) {
if (a0 > aMax) {
if constexpr (throw_validation_error) {
throw RuckigError("current acceleration " + std::to_string(a0) + " of DoF " + std::to_string(dof) + " exceeds its maximum acceleration limit " + std::to_string(aMax) + ".");
}
return false;
}
if (a0 < aMin) {
if constexpr (throw_validation_error) {
throw RuckigError("current acceleration " + std::to_string(a0) + " of DoF " + std::to_string(dof) + " undercuts its minimum acceleration limit " + std::to_string(aMin) + ".");
}
return false;
}
}
if (check_target_state_within_limits) {
if (af > aMax) {
if constexpr (throw_validation_error) {
throw RuckigError("target acceleration " + std::to_string(af) + " of DoF " + std::to_string(dof) + " exceeds its maximum acceleration limit " + std::to_string(aMax) + ".");
}
return false;
}
if (af < aMin) {
if constexpr (throw_validation_error) {
throw RuckigError("target acceleration " + std::to_string(af) + " of DoF " + std::to_string(dof) + " undercuts its minimum acceleration limit " + std::to_string(aMin) + ".");
}
return false;
}
}
const double v0 = current_velocity[dof];
if (std::isnan(v0)) {
if constexpr (throw_validation_error) {
throw RuckigError("current velocity " + std::to_string(v0) + " of DoF " + std::to_string(dof) + " should be a valid number.");
}
return false;
}
const double vf = target_velocity[dof];
if (std::isnan(vf)) {
if constexpr (throw_validation_error) {
throw RuckigError("target velocity " + std::to_string(vf) + " of DoF " + std::to_string(dof) + " should be a valid number.");
}
return false;
}
auto control_interface_ = per_dof_control_interface ? per_dof_control_interface.value()[dof] : control_interface;
if (control_interface_ == ControlInterface::Position) {
const double p0 = current_position[dof];
if (std::isnan(p0)) {
if constexpr (throw_validation_error) {
throw RuckigError("current position " + std::to_string(p0) + " of DoF " + std::to_string(dof) + " should be a valid number.");
}
return false;
}
const double pf = target_position[dof];
if (std::isnan(pf)) {
if constexpr (throw_validation_error) {
throw RuckigError("target position " + std::to_string(pf) + " of DoF " + std::to_string(dof) + " should be a valid number.");
}
return false;
}
const double vMax = max_velocity[dof];
if (std::isnan(vMax) || vMax < 0.0) {
if constexpr (throw_validation_error) {
throw RuckigError("maximum velocity limit " + std::to_string(vMax) + " of DoF " + std::to_string(dof) + " should be larger than or equal to zero.");
}
return false;
}
const double vMin = min_velocity ? min_velocity.value()[dof] : -max_velocity[dof];
if (std::isnan(vMin) || vMin > 0.0) {
if constexpr (throw_validation_error) {
throw RuckigError("minimum velocity limit " + std::to_string(vMin) + " of DoF " + std::to_string(dof) + " should be smaller than or equal to zero.");
}
return false;
}
if (check_current_state_within_limits) {
if (v0 > vMax) {
if constexpr (throw_validation_error) {
throw RuckigError("current velocity " + std::to_string(v0) + " of DoF " + std::to_string(dof) + " exceeds its maximum velocity limit " + std::to_string(vMax) + ".");
}
return false;
}
if (v0 < vMin) {
if constexpr (throw_validation_error) {
throw RuckigError("current velocity " + std::to_string(v0) + " of DoF " + std::to_string(dof) + " undercuts its minimum velocity limit " + std::to_string(vMin) + ".");
}
return false;
}
}
if (check_target_state_within_limits) {
if (vf > vMax) {
if constexpr (throw_validation_error) {
throw RuckigError("target velocity " + std::to_string(vf) + " of DoF " + std::to_string(dof) + " exceeds its maximum velocity limit " + std::to_string(vMax) + ".");
}
return false;
}
if (vf < vMin) {
if constexpr (throw_validation_error) {
throw RuckigError("target velocity " + std::to_string(vf) + " of DoF " + std::to_string(dof) + " undercuts its minimum velocity limit " + std::to_string(vMin) + ".");
}
return false;
}
}
if (check_current_state_within_limits) {
if (a0 > 0 && jMax > 0 && v_at_a_zero(v0, a0, jMax) > vMax) {
if constexpr (throw_validation_error) {
throw RuckigError("DoF " + std::to_string(dof) + " will inevitably reach a velocity " + std::to_string(v_at_a_zero(v0, a0, jMax)) + " from the current kinematic state that will exceed its maximum velocity limit " + std::to_string(vMax) + ".");
}
return false;
}
if (a0 < 0 && jMax > 0 && v_at_a_zero(v0, a0, -jMax) < vMin) {
if constexpr (throw_validation_error) {
throw RuckigError("DoF " + std::to_string(dof) + " will inevitably reach a velocity " + std::to_string(v_at_a_zero(v0, a0, -jMax)) + " from the current kinematic state that will undercut its minimum velocity limit " + std::to_string(vMin) + ".");
}
return false;
}
}
if (check_target_state_within_limits) {
if (af < 0 && jMax > 0 && v_at_a_zero(vf, af, jMax) > vMax) {
if constexpr (throw_validation_error) {
throw RuckigError("DoF " + std::to_string(dof) + " will inevitably have reached a velocity " + std::to_string(v_at_a_zero(vf, af, jMax)) + " from the target kinematic state that will exceed its maximum velocity limit " + std::to_string(vMax) + ".");
}
return false;
}
if (af > 0 && jMax > 0 && v_at_a_zero(vf, af, -jMax) < vMin) {
if constexpr (throw_validation_error) {
throw RuckigError("DoF " + std::to_string(dof) + " will inevitably have reached a velocity " + std::to_string(v_at_a_zero(vf, af, -jMax)) + " from the target kinematic state that will undercut its minimum velocity limit " + std::to_string(vMin) + ".");
}
return false;
}
}
}
}
if (!intermediate_positions.empty() && control_interface == ControlInterface::Position) {
if (minimum_duration || duration_discretization != DurationDiscretization::Continuous) {
if constexpr (throw_validation_error) {
throw RuckigError("Intermediate position can not be used together with a global minimum or discrete duration.");
}
return false;
}
if (per_dof_control_interface || per_dof_synchronization) {
if constexpr (throw_validation_error) {
throw RuckigError("Intermediate positions can only be used together with the position control interface and a global synchronization.");
}
return false;
}
for (size_t dof = 0; dof < degrees_of_freedom; ++dof) {
const double jMax = max_jerk[dof];
if (std::isinf(jMax)) {
if constexpr (throw_validation_error) {
throw RuckigError("infinite jerk limit of DoF " + std::to_string(dof) + " is currently not supported with intermediate positions.");
}
return false;
}
}
}
return true;
}
bool operator!=(const InputParameter<DOFs, CustomVector>& rhs) const {
return !(
current_position == rhs.current_position
&& current_velocity == rhs.current_velocity
&& current_acceleration == rhs.current_acceleration
&& target_position == rhs.target_position
&& target_velocity == rhs.target_velocity
&& target_acceleration == rhs.target_acceleration
&& max_velocity == rhs.max_velocity
&& max_acceleration == rhs.max_acceleration
&& max_jerk == rhs.max_jerk
&& intermediate_positions == rhs.intermediate_positions
&& per_section_max_velocity == rhs.per_section_max_velocity
&& per_section_max_acceleration == rhs.per_section_max_acceleration
&& per_section_max_jerk == rhs.per_section_max_jerk
&& per_section_min_velocity == rhs.per_section_min_velocity
&& per_section_min_acceleration == rhs.per_section_min_acceleration
&& per_section_max_position == rhs.per_section_max_position
&& per_section_min_position == rhs.per_section_min_position
&& max_position == rhs.max_position
&& min_position == rhs.min_position
&& enabled == rhs.enabled
&& minimum_duration == rhs.minimum_duration
&& per_section_minimum_duration == rhs.per_section_minimum_duration
&& min_velocity == rhs.min_velocity
&& min_acceleration == rhs.min_acceleration
&& control_interface == rhs.control_interface
&& synchronization == rhs.synchronization
&& duration_discretization == rhs.duration_discretization
&& per_dof_control_interface == rhs.per_dof_control_interface
&& per_dof_synchronization == rhs.per_dof_synchronization
);
}
std::string to_string() const {
std::stringstream ss;
ss << "\n";
if (control_interface == ControlInterface::Velocity) {
ss << "inp.control_interface = ControlInterface.Velocity\n";
}
if (synchronization == Synchronization::Phase) {
ss << "inp.synchronization = Synchronization.Phase\n";
} else if (synchronization == Synchronization::None) {
ss << "inp.synchronization = Synchronization.No\n";
}
if (duration_discretization == DurationDiscretization::Discrete) {
ss << "inp.duration_discretization = DurationDiscretization.Discrete\n";
}
ss << "inp.current_position = [" << join(current_position, true) << "]\n";
ss << "inp.current_velocity = [" << join(current_velocity, true) << "]\n";
ss << "inp.current_acceleration = [" << join(current_acceleration, true) << "]\n";
ss << "inp.target_position = [" << join(target_position, true) << "]\n";
ss << "inp.target_velocity = [" << join(target_velocity, true) << "]\n";
ss << "inp.target_acceleration = [" << join(target_acceleration, true) << "]\n";
ss << "inp.max_velocity = [" << join(max_velocity, true) << "]\n";
ss << "inp.max_acceleration = [" << join(max_acceleration, true) << "]\n";
ss << "inp.max_jerk = [" << join(max_jerk, true) << "]\n";
if (min_velocity) {
ss << "inp.min_velocity = [" << join(min_velocity.value(), true) << "]\n";
}
if (min_acceleration) {
ss << "inp.min_acceleration = [" << join(min_acceleration.value(), true) << "]\n";
}
if (minimum_duration) {
ss << "inp.minimum_duration = " << minimum_duration.value() << "\n";
}
if (!intermediate_positions.empty()) {
ss << "inp.intermediate_positions = [\n";
for (auto p: intermediate_positions) {
ss << " [" << join(p, true) << "],\n";
}
ss << "]\n";
}
if (min_position) {
ss << "inp.min_position = [" << join(min_position.value(), true) << "]\n";
}
if (max_position) {
ss << "inp.max_position = [" << join(max_position.value(), true) << "]\n";
}
return ss.str();
}
};
} // namespace ruckig