Skip to content

Commit

Permalink
update examples, PositionExtrema->Bound
Browse files Browse the repository at this point in the history
  • Loading branch information
pantor committed Dec 7, 2023
1 parent 3269c32 commit f7942a5
Show file tree
Hide file tree
Showing 20 changed files with 121 additions and 109 deletions.
7 changes: 3 additions & 4 deletions examples/01_position.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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);
}
Expand Down
6 changes: 3 additions & 3 deletions examples/02_position_offline.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<double, 3> 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<PositionExtrema, 3> position_extrema = trajectory.get_position_extrema();
std::array<Bound, 3> 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;
}
17 changes: 8 additions & 9 deletions examples/03_waypoints.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<DOFs> otg {control_cycle, max_number_of_waypoints};
Ruckig<DOFs> otg(control_cycle, max_number_of_waypoints);
InputParameter<DOFs> input;
OutputParameter<DOFs> output {max_number_of_waypoints};
OutputParameter<DOFs> output(max_number_of_waypoints);

// Set input parameters
input.current_position = {0.2, 0.0, -0.3};
Expand All @@ -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);

Expand Down
15 changes: 7 additions & 8 deletions examples/04_waypoints_online.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<DOFs> otg {control_cycle, max_number_of_waypoints};
Ruckig<DOFs> otg(control_cycle, max_number_of_waypoints);
InputParameter<DOFs> input;
OutputParameter<DOFs> output {max_number_of_waypoints};
OutputParameter<DOFs> output(max_number_of_waypoints);

// Set input parameters
input.current_position = {0.2, 0.0, -0.3};
Expand All @@ -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) {
Expand All @@ -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);
}
Expand Down
9 changes: 4 additions & 5 deletions examples/05_velocity.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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);
}
Expand Down
9 changes: 4 additions & 5 deletions examples/06_stop.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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) {
Expand Down
7 changes: 3 additions & 4 deletions examples/07_minimum_duration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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);
}
Expand Down
15 changes: 7 additions & 8 deletions examples/08_per_section_minimum_duration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<DOFs> otg {control_cycle, max_number_of_waypoints};
Ruckig<DOFs> otg(control_cycle, max_number_of_waypoints);
InputParameter<DOFs> input;
OutputParameter<DOFs> output {max_number_of_waypoints};
OutputParameter<DOFs> output(max_number_of_waypoints);

// Set input parameters
input.current_position = {0.8, 0.0, 0.5};
Expand All @@ -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);

Expand Down
11 changes: 5 additions & 6 deletions examples/09_dynamic_dofs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<DynamicDOFs> otg {degrees_of_freedom, 0.01};
InputParameter<DynamicDOFs> input {degrees_of_freedom};
OutputParameter<DynamicDOFs> output {degrees_of_freedom};
Ruckig<DynamicDOFs> otg(degrees_of_freedom, 0.01);
InputParameter<DynamicDOFs> input(degrees_of_freedom);
OutputParameter<DynamicDOFs> output(degrees_of_freedom);

// Set input parameters
input.current_position = {0.0, 0.0, 0.5};
Expand All @@ -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);
}
Expand Down
19 changes: 9 additions & 10 deletions examples/10_dynamic_dofs_waypoints.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<DynamicDOFs> otg {DOFs, control_cycle, max_number_of_waypoints};
InputParameter<DynamicDOFs> input {DOFs};
OutputParameter<DynamicDOFs> output {DOFs, max_number_of_waypoints};
Ruckig<DynamicDOFs> otg(DOFs, control_cycle, max_number_of_waypoints);
InputParameter<DynamicDOFs> input(DOFs);
OutputParameter<DynamicDOFs> output(DOFs, max_number_of_waypoints);

// Set input parameters
input.current_position = {0.2, 0.0, -0.3};
Expand All @@ -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);

Expand Down
7 changes: 3 additions & 4 deletions examples/11_eigen_vector_type.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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);
}
Expand Down
7 changes: 3 additions & 4 deletions examples/12_custom_vector_type.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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);
}
Expand Down
11 changes: 5 additions & 6 deletions examples/13_custom_vector_type_dynamic_dofs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,9 +46,9 @@ using namespace ruckig;

int main() {
// Create instances: the Ruckig OTG as well as input and output parameters
Ruckig<DynamicDOFs, MinimalDynamicDofsVector> otg {3, 0.01}; // control cycle
InputParameter<DynamicDOFs, MinimalDynamicDofsVector> input {3};
OutputParameter<DynamicDOFs, MinimalDynamicDofsVector> output {3};
Ruckig<DynamicDOFs, MinimalDynamicDofsVector> otg(3, 0.01); // control cycle
InputParameter<DynamicDOFs, MinimalDynamicDofsVector> input(3);
OutputParameter<DynamicDOFs, MinimalDynamicDofsVector> output(3);

// Set input parameters
input.current_position = {0.0, 0.0, 0.5};
Expand All @@ -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);
}
Expand Down
2 changes: 1 addition & 1 deletion examples/14_tracking.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
Loading

0 comments on commit f7942a5

Please sign in to comment.