...
 
Commits (8)
......@@ -21,7 +21,7 @@
#include <vector>
#include <utility>
namespace curves {
// REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
/// \brief An inverse kinematics architecture enforcing an arbitrary number of strict priority levels (Reference : Boulic et Al. 2003)
template <typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat) {
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
......
......@@ -31,7 +31,7 @@ inline unsigned int bin(const unsigned int n, const unsigned int k) {
}
/// \class Bernstein.
/// \brief Computes a Bernstein polynome.
/// \brief Computes a Bernstein polynomial.
///
template <typename Numeric = double>
struct Bern {
......@@ -40,6 +40,9 @@ struct Bern {
~Bern() {}
/// \brief Evaluation of Bernstein polynomial at value u.
/// \param u : value between 0 and 1.
/// \return Evaluation corresponding at value u.
Numeric operator()(const Numeric u) const {
if (!(u >= 0. && u <= 1.)) {
throw std::invalid_argument("u needs to be betwen 0 and 1.");
......@@ -47,11 +50,17 @@ struct Bern {
return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
}
/// \brief Check if actual Bernstein polynomial and other are approximately equal.
/// \param other : the other Bernstein polynomial to check.
/// \return true if the two Bernstein polynomials are approximately equals.
virtual bool operator==(const Bern& other) const {
return curves::isApprox<Numeric>(m_minus_i, other.m_minus_i) && curves::isApprox<Numeric>(i_, other.i_) &&
curves::isApprox<Numeric>(bin_m_i_, other.bin_m_i_);
}
/// \brief Check if actual Bernstein polynomial and other are different.
/// \param other : the other Bernstein polynomial to check.
/// \return true if the two Bernstein polynomials are different.
virtual bool operator!=(const Bern& other) const { return !(*this == other); }
/* Attributes */
......
......@@ -54,7 +54,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
/// Given the first and last point of a control points set, create the bezier curve.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param T : upper bound of time which is between \f$[0;T]\f$ (default \f$[0;1]\f$).
/// \param T_min : lower bound of time, curve will be defined for time in [T_min, T_max].
/// \param T_max : upper bound of time, curve will be defined for time in [T_min, T_max].
/// \param mult_T : ... (default value is 1.0).
///
template <typename In>
......@@ -66,7 +67,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
mult_T_(mult_T),
size_(std::distance(PointsBegin, PointsEnd)),
degree_(size_ - 1),
bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_)) {
bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
{
if (bernstein_.size() != size_) {
throw std::invalid_argument("Invalid size of polynomial");
}
......@@ -81,12 +83,15 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
}
/// \brief Constructor
/// \brief Constructor with constraints.
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
/// to ensure that velocity and acceleration constraints are respected.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param constraints : constraints applying on start / end velocities and acceleration.
/// \param T_min : lower bound of time, curve will be defined for time in [T_min, T_max].
/// \param T_max : upper bound of time, curve will be defined for time in [T_min, T_max].
/// \param mult_T : ... (default value is 1.0).
///
template <typename In>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T_min = 0.,
......@@ -97,7 +102,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
mult_T_(mult_T),
size_(std::distance(PointsBegin, PointsEnd) + 4),
degree_(size_ - 1),
bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_)) {
bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
{
if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
throw std::invalid_argument("can't create bezier min bound is higher than max bound");
}
......@@ -146,7 +152,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
* isEquivalent
* @param other the other curve to check
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
* @return true if the two curves are approximately equals
*/
bool isApprox(const bezier_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
bool equal = curves::isApprox<num_t>(T_min_, other.min()) && curves::isApprox<num_t>(T_max_, other.max()) &&
......@@ -337,7 +343,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
/// \brief Split the bezier curve in 2 at time t.
/// \param t : list of points.
/// \param u : unNormalized time.
/// \return pair containing the first element of both bezier curve obtained.
///
std::pair<bezier_curve_t, bezier_curve_t> split(const Numeric t) const {
......@@ -411,6 +416,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
private:
/// \brief Ensure constraints of bezier curve.
/// Add 4 points (2 after the first one, 2 before the last one) to biezer curve
/// to ensure that velocity and acceleration constraints are respected.
///
template <typename In>
t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints) {
t_point_t res;
......
......@@ -52,8 +52,8 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
cubic_hermite_spline() : dim_(0), T_min_(0), T_max_(0) {}
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
/// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
/// \param PairsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
/// \param PairsEnd : an iterator pointing to the last element of a pair(position, derivative) container.
/// \param time_control_points : vector containing time for each waypoint.
///
template <typename In>
......@@ -215,7 +215,6 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
///
std::size_t numIntervals() const { return size() - 1; }
private:
/// \brief Get index of the interval (subspline) corresponding to time t for the interpolation.
/// \param t : time where to look for interval.
......@@ -266,6 +265,8 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
return bezier_t(control_points.begin(), control_points.end(), t0, t1);
}
/// \brief Check if control points list is not empty and dimension of point superior to zero.
///
void check_conditions() const {
if (control_points_.size() == 0) {
throw std::runtime_error(
......
......@@ -72,7 +72,7 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
* @param other the other curve to check
* @param order the order up to which the derivatives of the curves are checked for equality
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
* @return true if the two curves are approximately equal
*/
bool isEquivalent(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision(),
const size_t order = 5) const {
......@@ -90,7 +90,7 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
}
t += inc;
}
// check if the derivatives are equals
// check if the derivatives are equal
for (size_t n = 1; n <= order; ++n) {
t = min();
while (t <= max()) {
......@@ -104,12 +104,12 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
}
/**
* @brief isApprox check if other and *this are approximately equals given a precision treshold
* Only two curves of the same class can be approximately equals,
* @brief isApprox check if other and *this are approximately equal given a precision treshold
* Only two curves of the same class can be approximately equal,
* for comparison between different type of curves see isEquivalent.
* @param other the other curve to check
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
* @return true if the two curves are approximately equal
*/
virtual bool isApprox(const curve_t* other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const = 0;
......
......@@ -119,8 +119,14 @@ class rotation_spline : public curve_abc_quat_t {
return exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end());
}
/// \brief Get dimension of curve.
/// \return dimension of curve.
virtual std::size_t dim() const { return dim_; }
/// \brief Get the minimum time for which the curve is defined.
/// \return \f$t_{min}\f$, lower bound of time range.
virtual time_t min() const { return min_; }
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
virtual time_t max() const { return max_; }
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
......
......@@ -32,7 +32,10 @@ struct linear_variable : public serialization::Serializable {
linear_variable(const vector_x_t& c) : B_(matrix_x_t::Zero(c.size(), c.size())), c_(c), zero(false) {} // constant
linear_variable(const matrix_x_t& B, const vector_x_t& c) : B_(B), c_(c), zero(false) {} // mixed
// linear evaluation
/// \brief Linear evaluation for vector x.
/// \param val : vector to evaluate the linear variable.
/// \return Evaluation of linear variable for vector x.
///
vector_x_t operator()(const Eigen::Ref<const vector_x_t>& val) const {
if (isZero()) return c();
if (Safe && B().cols() != val.rows())
......@@ -40,6 +43,10 @@ struct linear_variable : public serialization::Serializable {
return B() * val + c();
}
/// \brief Add another linear variable.
/// \param w1 : linear variable to add.
/// \return Linear variable after operation.
///
linear_variable_t& operator+=(const linear_variable_t& w1) {
if (w1.isZero()) return *this;
if (isZero()) {
......@@ -51,6 +58,11 @@ struct linear_variable : public serialization::Serializable {
this->c_ += w1.c_;
return *this;
}
/// \brief Substract another linear variable.
/// \param w1 : linear variable to substract.
/// \return Linear variable after operation.
///
linear_variable_t& operator-=(const linear_variable_t& w1) {
if (w1.isZero()) return *this;
if (isZero()) {
......@@ -62,25 +74,48 @@ struct linear_variable : public serialization::Serializable {
this->c_ -= w1.c_;
return *this;
}
/// \brief Divide by a constant : p_i / d = B_i*x/d + c_i/d.
/// \param d : constant.
/// \return Linear variable after operation.
///
linear_variable_t& operator/=(const double d) {
B_ /= d;
c_ /= d;
return *this;
}
/// \brief Multiply by a constant : p_i / d = B_i*x*d + c_i*d.
/// \param d : constant.
/// \return Linear variable after operation.
///
linear_variable_t& operator*=(const double d) {
B_ *= d;
c_ *= d;
return *this;
}
/// \brief Get a linear variable equal to zero.
/// \param dim : Dimension of linear variable.
/// \return Linear variable equal to zero.
///
static linear_variable_t Zero(size_t dim = 0) {
return linear_variable_t(matrix_x_t::Identity(dim, dim), vector_x_t::Zero(dim));
}
/// \brief Get dimension of linear variable.
/// \return Dimension of linear variable.
///
std::size_t size() const { return zero ? 0 : std::max(B_.cols(), c_.size()); }
/// \brief Get norm of linear variable (Norm of B plus norm of C).
/// \return Norm of linear variable.
Numeric norm() const { return isZero() ? 0 : (B_.norm() + c_.norm()); }
/// \brief Check if actual linear variable and other are approximately equal given a precision treshold.
/// Only two curves of the same class can be approximately equal,
/// \param prec : the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
/// \return true if the two linear variables are approximately equal.
bool isApprox(const linear_variable_t& other,
const double prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
return (*this - other).norm() < prec;
......
......@@ -215,10 +215,20 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
return isContinuous;
}
/// \brief Get number of curves in piecewise curve.
/// \return Number of curves in piecewise curve.
std::size_t num_curves() const { return curves_.size(); }
/// \brief Get curve corresponding to time t in piecewise curve.
/// Example : A piecewise curve PC made of two curves : c1 for t in [0,1] and c2 for t in ]1,2].
/// PC.curve_at_time(0.5) will return c1.
/// \param t : time to select curve.
/// \return Curve corresponding to time t in piecewise curve.
curve_ptr_t curve_at_time(const time_t t) const { return curves_[find_interval(t)]; }
/// \brief Get curve at specified index in piecewise curve.
/// \param idx : Index of curve to return, from 0 to num_curves-1.
/// \return curve corresonding to index in piecewise curve.
curve_ptr_t curve_at_index(const std::size_t idx) const {
if (Safe && idx >= num_curves()) {
throw std::length_error(
......@@ -227,6 +237,9 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
return curves_[idx];
}
/// \brief Convert all curves in piecewise curve into bezier curves.
/// \return piecewise bezier curve.
///
template <typename Bezier>
piecewise_curve_t convert_piecewise_curve_to_bezier() {
check_if_not_empty();
......@@ -242,6 +255,10 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
return pc_res;
}
/// \brief Convert all curves in piecewise curve into cubic hermite curves.
/// Curves need to be of degree inferior or equal to three.
/// \return piecewise cubic hermite curve.
///
template <typename Hermite>
piecewise_curve_t convert_piecewise_curve_to_cubic_hermite() {
check_if_not_empty();
......@@ -257,6 +274,9 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
return pc_res;
}
/// \brief Convert all curves in piecewise curve into polynomial curves.
/// \return piecewise polynomial curve.
///
template <typename Polynomial>
piecewise_curve_t convert_piecewise_curve_to_polynomial() {
check_if_not_empty();
......@@ -272,6 +292,11 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
return pc_res;
}
/// \brief Convert discrete points into piecewise polynomial curve with C0 continuity.
/// \param points : discrete points to convert.
/// \param time_points : time corresponding to each point in piecewise curve.
/// \return piecewise polynomial curve of C0 continuity.
///
template <typename Polynomial>
static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_time_t time_points) {
if (Safe & !(points.size() > 1)) {
......@@ -295,6 +320,12 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
return piecewise_res;
}
/// \brief Convert discrete points into piecewise polynomial curve with C1 continuity.
/// \param points : discrete points to convert.
/// \param points_derivative : derivative of order 1 corresponding to each point in piecewise curve.
/// \param time_points : time corresponding to each point in piecewise curve.
/// \return piecewise polynomial curve of C1 continuity.
///
template <typename Polynomial>
static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points,
t_point_derivate_t points_derivative,
......@@ -326,6 +357,13 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
return piecewise_res;
}
/// \brief Convert discrete points into piecewise polynomial curve with C2 continuity.
/// \param points : discrete points to convert.
/// \param points_derivative : derivative of order 1 corresponding to each point in piecewise curve.
/// \param points_second_derivative : derivative of order 2 corresponding to each point in piecewise curve.
/// \param time_points : time corresponding to each point in piecewise curve.
/// \return piecewise polynomial curve of C2 continuity.
///
template <typename Polynomial>
static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points,
t_point_derivate_t points_derivative,
......