From f7942a5493436ceadc373fde0a7d55658c48b09e Mon Sep 17 00:00:00 2001 From: pantor Date: Thu, 7 Dec 2023 09:27:16 +0100 Subject: [PATCH] update examples, PositionExtrema->Bound --- examples/01_position.cpp | 7 ++- examples/02_position_offline.cpp | 6 +-- examples/03_waypoints.cpp | 17 ++++--- examples/04_waypoints_online.cpp | 15 +++--- examples/05_velocity.cpp | 9 ++-- examples/06_stop.cpp | 9 ++-- examples/07_minimum_duration.cpp | 7 ++- examples/08_per_section_minimum_duration.cpp | 15 +++--- examples/09_dynamic_dofs.cpp | 11 ++--- examples/10_dynamic_dofs_waypoints.cpp | 19 ++++---- examples/11_eigen_vector_type.cpp | 7 ++- examples/12_custom_vector_type.cpp | 7 ++- .../13_custom_vector_type_dynamic_dofs.cpp | 11 ++--- examples/14_tracking.cpp | 2 +- include/ruckig/input_parameter.hpp | 47 ++++++++++++++----- include/ruckig/output_parameter.hpp | 8 ++-- include/ruckig/profile.hpp | 10 ++-- include/ruckig/trajectory.hpp | 4 +- include/ruckig/utils.hpp | 7 +-- src/ruckig/python.cpp | 12 ++--- 20 files changed, 121 insertions(+), 109 deletions(-) diff --git a/examples/01_position.cpp b/examples/01_position.cpp index 10d3cafc..7a4ef6dd 100644 --- a/examples/01_position.cpp +++ b/examples/01_position.cpp @@ -7,7 +7,7 @@ using namespace ruckig; int main() { // Create instances: the Ruckig OTG as well as input and output parameters - Ruckig<3> otg {0.01}; // control cycle + Ruckig<3> otg(0.01); // control cycle InputParameter<3> input; OutputParameter<3> output; @@ -25,10 +25,9 @@ int main() { input.max_jerk = {4.0, 3.0, 2.0}; // Generate the trajectory within the control loop - std::cout << "t | p1 | p2 | p3" << std::endl; + std::cout << "t | position" << std::endl; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << " " << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); } diff --git a/examples/02_position_offline.cpp b/examples/02_position_offline.cpp index 2149c66c..936823a7 100644 --- a/examples/02_position_offline.cpp +++ b/examples/02_position_offline.cpp @@ -38,15 +38,15 @@ int main() { // Get duration of the trajectory std::cout << "Trajectory duration: " << trajectory.get_duration() << " [s]." << std::endl; - double new_time {1.0}; + double new_time = 1.0; // Then, we can calculate the kinematic state at a given time std::array new_position, new_velocity, new_acceleration; trajectory.at_time(new_time, new_position, new_velocity, new_acceleration); - std::cout << "Position at time " << new_time << " [s]: " << new_position[0] << ", " << new_position[1] << ", " << new_position[2] << std::endl; + std::cout << "Position at time " << new_time << " [s]: " << join(new_position) << std::endl; // Get some info about the position extrema of the trajectory - std::array position_extrema = trajectory.get_position_extrema(); + std::array position_extrema = trajectory.get_position_extrema(); std::cout << "Position extremas for DoF 4 are " << position_extrema[2].min << " (min) to " << position_extrema[2].max << " (max)" << std::endl; } diff --git a/examples/03_waypoints.cpp b/examples/03_waypoints.cpp index 603965a6..bfdd7672 100644 --- a/examples/03_waypoints.cpp +++ b/examples/03_waypoints.cpp @@ -8,14 +8,14 @@ using namespace ruckig; int main() { - const double control_cycle {0.01}; - const size_t DOFs {3}; - const size_t max_number_of_waypoints {10}; // for memory allocation + const double control_cycle = 0.01; + const size_t DOFs = 3; + const size_t max_number_of_waypoints = 10; // for memory allocation // Create instances: the Ruckig OTG as well as input and output parameters - Ruckig otg {control_cycle, max_number_of_waypoints}; + Ruckig otg(control_cycle, max_number_of_waypoints); InputParameter input; - OutputParameter output {max_number_of_waypoints}; + OutputParameter output(max_number_of_waypoints); // Set input parameters input.current_position = {0.2, 0.0, -0.3}; @@ -37,11 +37,10 @@ int main() { input.max_acceleration = {3.0, 2.0, 2.0}; input.max_jerk = {6.0, 10.0, 20.0}; - std::cout << "t | p1 | p2 | p3" << std::endl; - double calculation_duration {0.0}; + std::cout << "t | position" << std::endl; + double calculation_duration = 0.0; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); diff --git a/examples/04_waypoints_online.cpp b/examples/04_waypoints_online.cpp index 6881829f..3929ea41 100644 --- a/examples/04_waypoints_online.cpp +++ b/examples/04_waypoints_online.cpp @@ -8,14 +8,14 @@ using namespace ruckig; int main() { - const double control_cycle {0.01}; - const size_t DOFs {3}; - const size_t max_number_of_waypoints {10}; // for memory allocation + const double control_cycle = 0.01; + const size_t DOFs = 3; + const size_t max_number_of_waypoints = 10; // for memory allocation // Create instances: the Ruckig OTG as well as input and output parameters - Ruckig otg {control_cycle, max_number_of_waypoints}; + Ruckig otg(control_cycle, max_number_of_waypoints); InputParameter input; - OutputParameter output {max_number_of_waypoints}; + OutputParameter output(max_number_of_waypoints); // Set input parameters input.current_position = {0.2, 0.0, -0.3}; @@ -39,7 +39,7 @@ int main() { input.interrupt_calculation_duration = 500; // [µs] - std::cout << "t | p1 | p2 | p3" << std::endl; + std::cout << "t | position" << std::endl; double calculation_duration {0.0}; while (otg.update(input, output) == Result::Working) { if (output.new_calculation) { @@ -48,8 +48,7 @@ int main() { std::cout << " Calculation in " << output.calculation_duration << " [µs]." << std::endl; } - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); } diff --git a/examples/05_velocity.cpp b/examples/05_velocity.cpp index 98312b9b..84e86991 100644 --- a/examples/05_velocity.cpp +++ b/examples/05_velocity.cpp @@ -7,7 +7,7 @@ using namespace ruckig; int main() { // Create instances: the Ruckig OTG as well as input and output parameters - Ruckig<3> otg {0.01}; // control cycle + Ruckig<3> otg(0.01); // control cycle InputParameter<3> input; OutputParameter<3> output; @@ -20,15 +20,14 @@ int main() { input.target_velocity = {0.0, -0.5, -1.5}; input.target_acceleration = {0.0, 0.0, 0.5}; - + input.max_acceleration = {3.0, 2.0, 1.0}; input.max_jerk = {6.0, 6.0, 4.0}; // Generate the trajectory within the control loop - std::cout << "t | p1 | p2 | p3" << std::endl; + std::cout << "t | position" << std::endl; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); } diff --git a/examples/06_stop.cpp b/examples/06_stop.cpp index 5a2b8541..82892f3c 100644 --- a/examples/06_stop.cpp +++ b/examples/06_stop.cpp @@ -7,7 +7,7 @@ using namespace ruckig; int main() { // Create instances: the ruckig otg as well as input and output parameters - Ruckig<3> otg {0.01}; + Ruckig<3> otg(0.01); InputParameter<3> input; OutputParameter<3> output; @@ -25,11 +25,10 @@ int main() { input.max_jerk = {4.0, 3.0, 2.0}; // Generate the trajectory within the control loop - std::cout << "t | p1 | p2 | p3" << std::endl; - bool on_stop_trajectory {false}; + std::cout << "t | position" << std::endl; + bool on_stop_trajectory = false; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << " " << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; // Activate stop trajectory after 1s if (output.time >= 1.0 && !on_stop_trajectory) { diff --git a/examples/07_minimum_duration.cpp b/examples/07_minimum_duration.cpp index e3f30c2a..47b7b9fa 100644 --- a/examples/07_minimum_duration.cpp +++ b/examples/07_minimum_duration.cpp @@ -7,7 +7,7 @@ using namespace ruckig; int main() { // Create instances: the Ruckig OTG as well as input and output parameters - Ruckig<3> otg {0.01}; // control cycle + Ruckig<3> otg(0.01); // control cycle InputParameter<3> input; OutputParameter<3> output; @@ -28,10 +28,9 @@ int main() { input.minimum_duration = 5.0; // Generate the trajectory within the control loop - std::cout << "t | p1 | p2 | p3" << std::endl; + std::cout << "t | position" << std::endl; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << " " << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); } diff --git a/examples/08_per_section_minimum_duration.cpp b/examples/08_per_section_minimum_duration.cpp index b94136e9..64b889ce 100644 --- a/examples/08_per_section_minimum_duration.cpp +++ b/examples/08_per_section_minimum_duration.cpp @@ -8,14 +8,14 @@ using namespace ruckig; int main() { - const double control_cycle {0.01}; - const size_t DOFs {3}; - const size_t max_number_of_waypoints {10}; // for memory allocation + const double control_cycle = 0.01; + const size_t DOFs = 3; + const size_t max_number_of_waypoints = 10; // for memory allocation // Create instances: the Ruckig OTG as well as input and output parameters - Ruckig otg {control_cycle, max_number_of_waypoints}; + Ruckig otg(control_cycle, max_number_of_waypoints); InputParameter input; - OutputParameter output {max_number_of_waypoints}; + OutputParameter output(max_number_of_waypoints); // Set input parameters input.current_position = {0.8, 0.0, 0.5}; @@ -42,11 +42,10 @@ int main() { // Define a minimum duration per section of the trajectory (number waypoints + 1) input.per_section_minimum_duration = {0, 2.0, 0.0, 1.0, 0.0, 2.0, 0}; - std::cout << "t | p1 | p2 | p3" << std::endl; + std::cout << "t | position" << std::endl; double calculation_duration {0.0}; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); diff --git a/examples/09_dynamic_dofs.cpp b/examples/09_dynamic_dofs.cpp index d75c3fa0..f55ec667 100644 --- a/examples/09_dynamic_dofs.cpp +++ b/examples/09_dynamic_dofs.cpp @@ -9,9 +9,9 @@ int main() { // Create instances: the Ruckig OTG as well as input and output parameters size_t degrees_of_freedom = 3; - Ruckig otg {degrees_of_freedom, 0.01}; - InputParameter input {degrees_of_freedom}; - OutputParameter output {degrees_of_freedom}; + Ruckig otg(degrees_of_freedom, 0.01); + InputParameter input(degrees_of_freedom); + OutputParameter output(degrees_of_freedom); // Set input parameters input.current_position = {0.0, 0.0, 0.5}; @@ -27,10 +27,9 @@ int main() { input.max_jerk = {4.0, 3.0, 2.0}; // Generate the trajectory within the control loop - std::cout << "t | p1 | p2 | p3" << std::endl; + std::cout << "t | position" << std::endl; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << " " << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); } diff --git a/examples/10_dynamic_dofs_waypoints.cpp b/examples/10_dynamic_dofs_waypoints.cpp index 2a5ce006..11dcb518 100644 --- a/examples/10_dynamic_dofs_waypoints.cpp +++ b/examples/10_dynamic_dofs_waypoints.cpp @@ -8,14 +8,14 @@ using namespace ruckig; int main() { - double control_cycle {0.01}; - size_t DOFs {3}; - size_t max_number_of_waypoints {10}; // for memory allocation + double control_cycle = 0.01; + size_t DOFs = 3; + size_t max_number_of_waypoints = 10; // for memory allocation // Create instances: the Ruckig OTG as well as input and output parameters - Ruckig otg {DOFs, control_cycle, max_number_of_waypoints}; - InputParameter input {DOFs}; - OutputParameter output {DOFs, max_number_of_waypoints}; + Ruckig otg(DOFs, control_cycle, max_number_of_waypoints); + InputParameter input(DOFs); + OutputParameter output(DOFs, max_number_of_waypoints); // Set input parameters input.current_position = {0.2, 0.0, -0.3}; @@ -37,11 +37,10 @@ int main() { input.max_acceleration = {3.0, 2.0, 2.0}; input.max_jerk = {6.0, 10.0, 20.0}; - std::cout << "t | p1 | p2 | p3" << std::endl; - double calculation_duration {0.0}; + std::cout << "t | position" << std::endl; + double calculation_duration = 0.0; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); diff --git a/examples/11_eigen_vector_type.cpp b/examples/11_eigen_vector_type.cpp index e29aaa8e..a60f78fd 100644 --- a/examples/11_eigen_vector_type.cpp +++ b/examples/11_eigen_vector_type.cpp @@ -10,7 +10,7 @@ using namespace ruckig; int main() { // Create instances: the Ruckig OTG as well as input and output parameters - Ruckig<3, EigenVector> otg {0.01}; // control cycle + Ruckig<3, EigenVector> otg(0.01); // control cycle InputParameter<3, EigenVector> input; OutputParameter<3, EigenVector> output; @@ -34,10 +34,9 @@ int main() { input.max_jerk = {4.0, 3.0, 2.0}; // Generate the trajectory within the control loop - std::cout << "t | p1 | p2 | p3" << std::endl; + std::cout << "t | position" << std::endl; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << " " << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); } diff --git a/examples/12_custom_vector_type.cpp b/examples/12_custom_vector_type.cpp index 5af2444a..a2c0fbd0 100644 --- a/examples/12_custom_vector_type.cpp +++ b/examples/12_custom_vector_type.cpp @@ -40,7 +40,7 @@ using namespace ruckig; int main() { // Create instances: the Ruckig OTG as well as input and output parameters - Ruckig<3, MinimalVector> otg {0.01}; // control cycle + Ruckig<3, MinimalVector> otg(0.01); // control cycle InputParameter<3, MinimalVector> input; OutputParameter<3, MinimalVector> output; @@ -58,10 +58,9 @@ int main() { input.max_jerk = {4.0, 3.0, 2.0}; // Generate the trajectory within the control loop - std::cout << "t | p1 | p2 | p3" << std::endl; + std::cout << "t | position" << std::endl; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << " " << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); } diff --git a/examples/13_custom_vector_type_dynamic_dofs.cpp b/examples/13_custom_vector_type_dynamic_dofs.cpp index 6693e739..98bd99aa 100644 --- a/examples/13_custom_vector_type_dynamic_dofs.cpp +++ b/examples/13_custom_vector_type_dynamic_dofs.cpp @@ -46,9 +46,9 @@ using namespace ruckig; int main() { // Create instances: the Ruckig OTG as well as input and output parameters - Ruckig otg {3, 0.01}; // control cycle - InputParameter input {3}; - OutputParameter output {3}; + Ruckig otg(3, 0.01); // control cycle + InputParameter input(3); + OutputParameter output(3); // Set input parameters input.current_position = {0.0, 0.0, 0.5}; @@ -64,10 +64,9 @@ int main() { input.max_jerk = {4.0, 3.0, 2.0}; // Generate the trajectory within the control loop - std::cout << "t | p1 | p2 | p3" << std::endl; + std::cout << "t | position" << std::endl; while (otg.update(input, output) == Result::Working) { - auto& p = output.new_position; - std::cout << output.time << " " << p[0] << " " << p[1] << " " << p[2] << " " << std::endl; + std::cout << output.time << " | " << join(output.new_position) << std::endl; output.pass_to_input(input); } diff --git a/examples/14_tracking.cpp b/examples/14_tracking.cpp index 0a54da3b..8508d36e 100644 --- a/examples/14_tracking.cpp +++ b/examples/14_tracking.cpp @@ -37,7 +37,7 @@ TargetState<1> model_sinus(double t, double ramp_vel=0.4) { int main() { // Create instances: the Trackig OTG as well as input and output parameters - Trackig<1> otg {0.01}; // control cycle + Trackig<1> otg(0.01); // control cycle InputParameter<1> input; OutputParameter<1> output; diff --git a/include/ruckig/input_parameter.hpp b/include/ruckig/input_parameter.hpp index 67ef7229..079437aa 100644 --- a/include/ruckig/input_parameter.hpp +++ b/include/ruckig/input_parameter.hpp @@ -395,28 +395,51 @@ class InputParameter { std::string to_string() const { std::stringstream ss; - ss << "\ninp.current_position = [" << join(current_position, degrees_of_freedom) << "]\n"; - ss << "inp.current_velocity = [" << join(current_velocity, degrees_of_freedom) << "]\n"; - ss << "inp.current_acceleration = [" << join(current_acceleration, degrees_of_freedom) << "]\n"; - ss << "inp.target_position = [" << join(target_position, degrees_of_freedom) << "]\n"; - ss << "inp.target_velocity = [" << join(target_velocity, degrees_of_freedom) << "]\n"; - ss << "inp.target_acceleration = [" << join(target_acceleration, degrees_of_freedom) << "]\n"; - ss << "inp.max_velocity = [" << join(max_velocity, degrees_of_freedom) << "]\n"; - ss << "inp.max_acceleration = [" << join(max_acceleration, degrees_of_freedom) << "]\n"; - ss << "inp.max_jerk = [" << join(max_jerk, degrees_of_freedom) << "]\n"; + 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(), degrees_of_freedom) << "]\n"; + ss << "inp.min_velocity = [" << join(min_velocity.value(), true) << "]\n"; } if (min_acceleration) { - ss << "inp.min_acceleration = [" << join(min_acceleration.value(), degrees_of_freedom) << "]\n"; + 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, degrees_of_freedom) << "],\n"; + 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(); } }; diff --git a/include/ruckig/output_parameter.hpp b/include/ruckig/output_parameter.hpp index e4d76d49..4ca03f23 100644 --- a/include/ruckig/output_parameter.hpp +++ b/include/ruckig/output_parameter.hpp @@ -89,10 +89,10 @@ class OutputParameter { std::string to_string() const { std::stringstream ss; - ss << "\nout.new_position = [" << join(new_position, degrees_of_freedom) << "]\n"; - ss << "out.new_velocity = [" << join(new_velocity, degrees_of_freedom) << "]\n"; - ss << "out.new_acceleration = [" << join(new_acceleration, degrees_of_freedom) << "]\n"; - ss << "out.new_jerk = [" << join(new_jerk, degrees_of_freedom) << "]\n"; + ss << "\nout.new_position = [" << join(new_position, true) << "]\n"; + ss << "out.new_velocity = [" << join(new_velocity, true) << "]\n"; + ss << "out.new_acceleration = [" << join(new_acceleration, true) << "]\n"; + ss << "out.new_jerk = [" << join(new_jerk, true) << "]\n"; ss << "out.time = [" << std::setprecision(16) << time << "]\n"; ss << "out.calculation_duration = [" << std::setprecision(16) << calculation_duration << "]\n"; return ss.str(); diff --git a/include/ruckig/profile.hpp b/include/ruckig/profile.hpp index 51dce461..7c5ddceb 100644 --- a/include/ruckig/profile.hpp +++ b/include/ruckig/profile.hpp @@ -16,7 +16,7 @@ namespace ruckig { //! Information about the position extrema -struct PositionExtrema { +struct Bound { //! The extreme position double min, max; @@ -401,7 +401,7 @@ class Profile { // Secondary features - static void check_position_extremum(double t_ext, double t_sum, double t, double p, double v, double a, double j, PositionExtrema& ext) { + static void check_position_extremum(double t_ext, double t_sum, double t, double p, double v, double a, double j, Bound& ext) { if (0 < t_ext && t_ext < t) { double p_ext, a_ext; std::tie(p_ext, std::ignore, a_ext) = integrate(t_ext, p, v, a, j); @@ -415,7 +415,7 @@ class Profile { } } - static void check_step_for_position_extremum(double t_sum, double t, double p, double v, double a, double j, PositionExtrema& ext) { + static void check_step_for_position_extremum(double t_sum, double t, double p, double v, double a, double j, Bound& ext) { if (p < ext.min) { ext.min = p; ext.t_min = t_sum; @@ -438,8 +438,8 @@ class Profile { } } - PositionExtrema get_position_extrema() const { - PositionExtrema extrema; + Bound get_position_extrema() const { + Bound extrema; extrema.min = std::numeric_limits::infinity(); extrema.max = -std::numeric_limits::infinity(); diff --git a/include/ruckig/trajectory.hpp b/include/ruckig/trajectory.hpp index cb028fcd..3a307688 100644 --- a/include/ruckig/trajectory.hpp +++ b/include/ruckig/trajectory.hpp @@ -35,7 +35,7 @@ class Trajectory { Container cumulative_times; Vector independent_min_durations; - Vector position_extrema; + Vector position_extrema; size_t continue_calculation_counter {0}; @@ -266,7 +266,7 @@ class Trajectory { } //! Get the min/max values of the position for each DoF - Vector get_position_extrema() { + Vector get_position_extrema() { for (size_t dof = 0; dof < degrees_of_freedom; ++dof) { position_extrema[dof] = profiles[0][dof].get_position_extrema(); } diff --git a/include/ruckig/utils.hpp b/include/ruckig/utils.hpp index 23ad6f25..a648db61 100644 --- a/include/ruckig/utils.hpp +++ b/include/ruckig/utils.hpp @@ -29,11 +29,12 @@ template using StandardSizeVector = typename template -inline std::string join(const Vector& array, size_t size) { +inline std::string join(const Vector& array, bool high_precision = false) { std::ostringstream ss; - for (size_t i = 0; i < size; ++i) { + for (size_t i = 0; i < array.size(); ++i) { if (i) ss << ", "; - ss << std::setprecision(16) << array[i]; + if (high_precision) ss << std::setprecision(16); + ss << array[i]; } return ss.str(); } diff --git a/src/ruckig/python.cpp b/src/ruckig/python.cpp index 011ee2f5..200c433e 100644 --- a/src/ruckig/python.cpp +++ b/src/ruckig/python.cpp @@ -48,12 +48,12 @@ limited by velocity, acceleration, and jerk constraints."; py::register_exception(m, "RuckigError"); - py::class_(m, "PositionExtrema") - .def_readonly("min", &PositionExtrema::min) - .def_readonly("max", &PositionExtrema::max) - .def_readonly("t_min", &PositionExtrema::t_min) - .def_readonly("t_max", &PositionExtrema::t_max) - .def("__repr__", [](const PositionExtrema& ext) { + py::class_(m, "Bound") + .def_readonly("min", &Bound::min) + .def_readonly("max", &Bound::max) + .def_readonly("t_min", &Bound::t_min) + .def_readonly("t_max", &Bound::t_max) + .def("__repr__", [](const Bound& ext) { return "[" + std::to_string(ext.min) + ", " + std::to_string(ext.max) + "]"; });