Skip to content

Commit dfdf302

Browse files
pantorpantor
committed
initial commit
0 parents  commit dfdf302

18 files changed

+17677
-0
lines changed

‎.gitignore

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
.vscode
2+
3+
build
4+
reflexxes
5+
6+
local.sh

‎CMakeLists.txt

+68
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
cmake_minimum_required(VERSION 3.11)
2+
3+
4+
project(ruckig VERSION 0.1.0 LANGUAGES CXX)
5+
list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_SOURCE_DIR}/cmake)
6+
7+
8+
option(BUILD_PYTHON_MODULE "Build python module" ON)
9+
option(BUILD_TESTS "Build tests" ON)
10+
11+
12+
find_package(Eigen3 3.3.7 REQUIRED NO_MODULE)
13+
find_package(Reflexxes)
14+
15+
message("Found Eigen Version: ${Eigen3_VERSION}")
16+
17+
if(Reflexxes)
18+
set(REFLEXXES_TYPE "ReflexxesTypeII" CACHE STRING "Type of Reflexxes library") # or ReflexxesTypeIV
19+
20+
message("Found Reflexxes ${REFLEXXES_TYPE}")
21+
else()
22+
message("Did not found Reflexxes.")
23+
endif()
24+
25+
26+
add_library(ruckig SHARED
27+
src/ruckig.cpp
28+
)
29+
target_compile_features(ruckig PUBLIC cxx_std_17)
30+
target_include_directories(ruckig PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
31+
target_link_libraries(ruckig PUBLIC Eigen3::Eigen)
32+
33+
34+
if(Reflexxes)
35+
target_compile_definitions(ruckig PUBLIC WITH_REFLEXXES)
36+
target_link_libraries(ruckig PUBLIC Reflexxes::Reflexxes)
37+
endif()
38+
39+
40+
if(BUILD_PYTHON_MODULE)
41+
# Check if pybind11 exists as a subdirectory
42+
if(EXISTS pybind11)
43+
add_subdirectory(pybind11)
44+
else()
45+
find_package(Python COMPONENTS Interpreter Development)
46+
find_package(pybind11 2.6 REQUIRED)
47+
endif()
48+
49+
pybind11_add_module(_ruckig src/python.cpp)
50+
target_compile_features(_ruckig PUBLIC cxx_std_17)
51+
target_link_libraries(_ruckig PUBLIC ruckig)
52+
endif()
53+
54+
55+
if(BUILD_TESTS)
56+
enable_testing()
57+
58+
find_package(Catch2 REQUIRED)
59+
60+
foreach(test IN ITEMS otg-test)
61+
add_executable(${test} "test/${test}.cpp")
62+
if(Reflexxes)
63+
target_compile_definitions(${test} PUBLIC WITH_REFLEXXES)
64+
endif()
65+
target_link_libraries(${test} PRIVATE ruckig Catch2::Catch2)
66+
add_test(NAME ${test} COMMAND ${test})
67+
endforeach()
68+
endif()

‎README.md

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
# Ruckig
2+
3+
Online Trajectory Generation. Real-time. Jerk-constrained.

‎cmake/FindReflexxes.cmake

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
if(NOT TARGET Reflexxes::Reflexxes)
2+
if(NOT ${Reflexxes_ROOT_DIR} STREQUAL "")
3+
set(Reflexxes_LIB_DIR "${Reflexxes_ROOT_DIR}/MacOS/x64/release/lib/")
4+
set(Reflexxes_INCLUDE_DIR "${Reflexxes_ROOT_DIR}/include")
5+
endif()
6+
7+
find_library(Reflexxes ${REFLEXXES_TYPE} PATHS ${Reflexxes_LIB_DIR})
8+
9+
add_library(Reflexxes::Reflexxes INTERFACE IMPORTED)
10+
target_include_directories(Reflexxes::Reflexxes INTERFACE ${Reflexxes_INCLUDE_DIR})
11+
target_link_libraries(Reflexxes::Reflexxes INTERFACE ${Reflexxes})
12+
endif()

‎include/ruckig/parameter.hpp

+112
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
#pragma once
2+
3+
#include <optional>
4+
5+
#include <Eigen/Core>
6+
7+
8+
namespace ruckig {
9+
10+
enum class Result {
11+
Working,
12+
Finished,
13+
Error
14+
};
15+
16+
17+
template<size_t DOFs>
18+
struct InputParameter {
19+
using Vector = Eigen::Matrix<double, DOFs, 1, Eigen::ColMajor>;
20+
static constexpr size_t degrees_of_freedom {DOFs};
21+
22+
enum class Type {
23+
Position,
24+
Velocity,
25+
} type {Type::Position};
26+
27+
Vector current_position;
28+
Vector current_velocity {Vector::Zero()};
29+
Vector current_acceleration {Vector::Zero()};
30+
31+
Vector target_position;
32+
Vector target_velocity {Vector::Zero()};
33+
Vector target_acceleration {Vector::Zero()};
34+
35+
Vector max_velocity;
36+
Vector max_acceleration;
37+
Vector max_jerk;
38+
39+
std::array<bool, DOFs> enabled;
40+
std::optional<double> minimum_duration;
41+
42+
InputParameter() {
43+
enabled.fill(true);
44+
}
45+
46+
bool operator!=(const InputParameter<DOFs>& rhs) const {
47+
return (
48+
current_position != rhs.current_position
49+
|| current_velocity != rhs.current_velocity
50+
|| current_acceleration != rhs.current_acceleration
51+
|| target_position != rhs.target_position
52+
|| target_velocity != rhs.target_velocity
53+
|| target_acceleration != rhs.target_acceleration
54+
|| max_velocity != rhs.max_velocity
55+
|| max_acceleration != rhs.max_acceleration
56+
|| max_jerk != rhs.max_jerk
57+
|| enabled != rhs.enabled
58+
|| minimum_duration != rhs.minimum_duration
59+
|| type != rhs.type
60+
);
61+
}
62+
63+
std::string to_string(size_t dof) const {
64+
std::stringstream ss;
65+
ss << "p0: " << current_position[dof] << ", ";
66+
ss << "v0: " << current_velocity[dof] << ", ";
67+
ss << "a0: " << current_acceleration[dof] << ", ";
68+
ss << "pf: " << target_position[dof] << ", ";
69+
ss << "vf: " << target_velocity[dof] << ", ";
70+
ss << "vMax: " << max_velocity[dof] << ", ";
71+
ss << "aMax: " << max_acceleration[dof] << ", ";
72+
ss << "jMax: " << max_jerk[dof];
73+
return ss.str();
74+
}
75+
76+
std::string to_string() const {
77+
Eigen::IOFormat formatter(10, 0, ", ", "\n", "[", "]");
78+
79+
std::stringstream ss;
80+
ss << "\ninp.current_position = " << current_position.transpose().format(formatter) << "\n";
81+
ss << "inp.current_velocity = " << current_velocity.transpose().format(formatter) << "\n";
82+
ss << "inp.current_acceleration = " << current_acceleration.transpose().format(formatter) << "\n";
83+
ss << "inp.target_position = " << target_position.transpose().format(formatter) << "\n";
84+
ss << "inp.target_velocity = " << target_velocity.transpose().format(formatter) << "\n";
85+
ss << "inp.target_acceleration = " << target_acceleration.transpose().format(formatter) << "\n";
86+
ss << "inp.max_velocity = " << max_velocity.transpose().format(formatter) << "\n";
87+
ss << "inp.max_acceleration = " << max_acceleration.transpose().format(formatter) << "\n";
88+
ss << "inp.max_jerk = " << max_jerk.transpose().format(formatter) << "\n";
89+
return ss.str();
90+
}
91+
};
92+
93+
94+
template<size_t DOFs>
95+
struct OutputParameter {
96+
using Vector = Eigen::Matrix<double, DOFs, 1, Eigen::ColMajor>;
97+
static constexpr size_t degrees_of_freedom {DOFs};
98+
99+
Vector new_position;
100+
Vector new_velocity;
101+
Vector new_acceleration;
102+
103+
double duration; // [s]
104+
bool new_calculation {false};
105+
double calculation_duration; // [µs]
106+
107+
// Vector independent_min_durations;
108+
// Vector min_positions, max_positions;
109+
// Vector time_min_positions, time_max_positions;
110+
};
111+
112+
} // namespace ruckig

‎include/ruckig/quintic.hpp

+91
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
#pragma once
2+
3+
#include <Eigen/Core>
4+
5+
#include <ruckig/parameter.hpp>
6+
7+
8+
namespace ruckig {
9+
10+
template<size_t DOFs>
11+
class Quintic {
12+
using Vector = Eigen::Matrix<double, DOFs, 1, Eigen::ColMajor>;
13+
14+
// Trajectory
15+
Vector a, b, c, d, e, f;
16+
double t, tf;
17+
InputParameter<DOFs> current_input;
18+
19+
bool calculate(const InputParameter<DOFs>& input, OutputParameter<DOFs>& output) {
20+
current_input = input;
21+
22+
const Vector& x0 = input.current_position;
23+
const Vector& v0 = input.current_velocity;
24+
const Vector& a0 = input.current_acceleration;
25+
const Vector& xf = input.target_position;
26+
const Vector& vf = input.target_velocity;
27+
const Vector& af = input.target_acceleration;
28+
const Vector& v_max = input.max_velocity;
29+
const Vector& a_max = input.max_acceleration;
30+
const Vector& j_max = input.max_jerk;
31+
32+
// Check input
33+
if ((v_max.array() <= 0.0).any() || (a_max.array() <= 0.0).any() || (j_max.array() <= 0.0).any()) {
34+
return false;
35+
}
36+
37+
// Approximations for v0 == 0, vf == 0, a0 == 0, af == 0
38+
Vector v_max_tfs = (15 * (x0 - xf).array().abs()) / (8 * v_max).array();
39+
Vector a_max_tfs = (std::sqrt(10) * (x0.array().pow(2) - 2 * x0.array() * xf.array() + xf.array().pow(2)).pow(1./4)) / (std::pow(3, 1./4) * a_max.array().sqrt());
40+
Vector j_max_tfs = ((60 * (x0 - xf).array().abs()) / j_max.array()).pow(1./3); // Also solvable for v0 != 0
41+
42+
tf = std::max<double>({v_max_tfs.maxCoeff(), a_max_tfs.maxCoeff(), j_max_tfs.maxCoeff()});
43+
if (input.minimum_duration.has_value()) {
44+
tf = std::max<double>({tf, input.minimum_duration.value()});
45+
}
46+
47+
a = -((a0 - af) * std::pow(tf, 2) + 6 * tf * (v0 + vf) + 12 * (x0 - xf)) / (2 * std::pow(tf, 5));
48+
b = -((2 * af - 3 * a0) * std::pow(tf, 2) - 16 * tf * v0 - 14 * tf * vf - 30 * (x0 - xf)) / (2 * std::pow(tf, 4));
49+
c = -((3 * a0 - af) * std::pow(tf, 2) + 12 * tf * v0 + 8 * tf * vf + 20 * (x0 - xf)) / (2 * std::pow(tf, 3));
50+
d = a0 / 2;
51+
e = v0;
52+
f = x0;
53+
54+
t = 0.0;
55+
output.duration = tf;
56+
output.new_calculation = true;
57+
return true;
58+
}
59+
60+
public:
61+
double delta_time;
62+
63+
explicit Quintic(double delta_time): delta_time(delta_time) { }
64+
65+
Result update(const InputParameter<DOFs>& input, OutputParameter<DOFs>& output) {
66+
t += delta_time;
67+
output.new_calculation = false;
68+
69+
if (input != current_input && !calculate(input, output)) {
70+
return Result::Error;
71+
}
72+
73+
if (t >= tf) {
74+
output.new_position = input.target_position;
75+
output.new_velocity = input.target_velocity;
76+
output.new_acceleration = input.target_acceleration;
77+
return Result::Finished;
78+
}
79+
80+
output.new_position = f + t * (e + t * (d + t * (c + t * (b + a * t))));
81+
output.new_velocity = e + t * (2 * d + t * (3 * c + t * (4 * b + 5 * a * t)));
82+
output.new_acceleration = 2 * d + t * (6 * c + t * (12 * b + t * (20 * a)));
83+
84+
current_input.current_position = output.new_position;
85+
current_input.current_velocity = output.new_velocity;
86+
current_input.current_acceleration = output.new_acceleration;
87+
return Result::Working;
88+
}
89+
};
90+
91+
} // namespace ruckig

0 commit comments

Comments
 (0)