rafaelrojasmiliani/gsplines_cpp

Generalized Splines for Motion Optimization in C++ and python3

General Splines: A Library for Algebraically and Analytically Consistent Representation of Motions

Library to represent and formulate motion and trajectory planning problems with generalized splines and piece-wise polynomials.

• Generalized splines as a GSpline class. They can represent
  • Piecewise polynomial curves representation
  • Piece-wise Lagrange polynomials at (interpolation at Gauss-Lobatto points already implemented).
• Analitical consistenc GSpline provide a derivate method which returns the its derivative as a new GSpline instance. This library provides automatic exact (and fast) differentiation of the generalized splines implemented.
• Algebraic consistence: This library implement basic operations between GSplines inner product, norms, addition, multiplication, composition and concatenation of curves (allows only when it has mathematical sense).
• Optimization with waypoint (via-point) constraints: minimum jerk, snap, crank, etc.
• ROS implementation here
• MoveIt implementation here
• Contact: Rafael A. Rojas [email protected]
• Docker containers with this library installed
  • vim awesome plugins for development and moveit rafa606/moveit-gsplines-vim-dev:noetic
  • vim awesome plugins for development and awesome ros packages rafa606/ros-gsplines-vim-dev:noetic

Examples

Get your minimum jerk trajectory passing through random waypoints

import numpy as np
from gsplines.optimization import minimum_jerk_path
dim = 7  # number of joint of the robot
waypoint_number = 4 # number of waypoints
waypoints = np.random.rand(waypoint_number, dim) # random waypoints

# get the minimum jerk path inn [0, 1]
path = minimum_jerk_path(waypoints)

#get the minimum jerk trajectory with execution time of 10.0 seconds
trajectory  = path.linear_scaling_new_execution_time(10.0)

# Evaluate your trajectory
trajectory_points = trajectory([0.0, 5.0, 10.0]) # matrix, rows are points

# Get the derivative
trajectory_derivative = trajectory.deriv()
# Get the jerk
trajectory_jerk = trajectory.deriv(3)
# Evaluate the jerk at points
trajectory_jerk_at_instants = trajectory_jerk([0.0, 5.0, 10.0])


# Algebraic operations
expression = trajectory + trajectory_jerk + trajectory_derivative

Installation

In Ubuntu using deb packages and ROS

To install using debian packages it is needed to have access to the ROS repos (read here). The reason to use ros packages is that this library depends on ifopt, and its deb package is available with ros.

1. Install the requirements

sudo apt-get install  python3-matplotlib ros-noetic-ifopt

2. Download the package a install

wget https://github.com/rafaelrojasmiliani/gsplines_cpp/releases/download/master/gsplines-0.0.1-amd64.deb
sudo dpkg -i gsplines-0.0.1-amd64.deb

From source without ros

1. Install the requirements

sudo apt-get install  python3-matplotlib libgtest-dev cmake libeigen3-dev coinor-libipopt-dev

2. Install ifopt

   git clone https://github.com/ethz-adrl/ifopt.git
   cd ifopt
   mkdir build
   cd build
   cmake .. -DCMAKE_INSTALL_PREFIX=/usr
   make
   make install

3. Download the repo with recursive mode and compile

apt-get update
DEBIAN_FRONTEND=noninteractive apt-get install -y --no-install-recommends -o Dpkg::Options::="--force-confnew"  git ros-noetic-ifopt libgtest-dev
git clone --recursive https://github.com/rafaelrojasmiliani/gsplines_cpp.git
cd gsplines_cpp
mkdir build
cd build
cmake ..
make
make install

Definition

• Definition A generalized spline is a piece-wise defined curve such that in each interval it is the linear combination of certain linearly independent functions $B_1, B_2, ... ,B_k$
• Formal Definition
  1. Let $J=[0, T]$ and consider the partition of $J$ given by $N + 1$ points $t_i\in J$, i.e. $I_1, I_2, ... ,I_N$ with $I_i=[t_i, t_{i + 1})$.
  2. Let $I_0=[-1,1]$ and $B_1, B_2, ... ,B_k$ be $k$ linearly independent functions $B_i:I_0\longrightarrow \mathbb{R}$.
  3. Let $s_i:I_i\longrightarrow I_0$ given by

$$ s_i(t)= 2\frac{t-t_i}{t_{i + 1}-t_i} - 1 $$

          iv. Let $\mathbf{y}_i^j \in\mathbb{R}^n$.

          v. A generalized spline from $J$ into $\mathbb{R}^n$ is a curve given by

$$ f_r= (y_{i}^j)^\top \mathbf{B} \circ s_j(t) \text{ if } t\in I_j\ \ \ \ \ \ \ \ \ \ \ \ \ (\star) $$

where $\mathbf{B}$ is the vector constructed by stacking the basis in a colunm vector

Motivation

Generalized splines appear naturally in problems of trajectory optimization when waypoint constraints are added. In other words, if we desire to optimize a motion which pass trough a sequence of positions we will meet with generalized splines.

Generalized splines trajectories arising in such kind of optimization problems are uniquely characterized by the waypoints that they attain, the time intervals between waypoints and the speed, and possible higher order derivatives at the boundaries. Moreover, such a relation is synthesised in the expression of the type

$$ \mathbf{A}(\boldsymbol\tau)\mathbf{y} = \mathbf{b}(\mathbf{w})\ \ \ \ \ \ \ \ \ \ \ \ \ \ (0) $$

where $\mathbf{A}(\boldsymbol\tau)$ is a matrix which depends on the time intervals $\boldsymbol\tau$, $\mathbf{b}(\mathbf{w})$ is a column vector which depends on the waypoints $\mathbf{w}$, the speed, and possible higher order derivatives at the boundaries, and $\mathbf{y}$ is a column vector which represents uniquely the curve at each interval.

The main challenge to build into a computer a trajectory optimization problems with waypoint constraints is to compute the derivatives of $\mathbf{y}$ with respect to $\mathbf{\tau}$.

This library provides a uniform and simple interface to formulate gradient-based optimization problems for waypoint-constrained trajectory planning. The library leverage on the representation (0) to compute the "derivatives of the splines" with respect to the time intervals (and possibly the waypoints) as the corresponding derivatives of $\mathbf{y}$

Background

This library is aimed to find a trajectory passing trough a sequence of waypoints $\mathbf{w}_0, ...,\mathbf{w}_{N + 1}$ such that the following integral is minized

$$ \Large I=\int_0^T \alpha_1\left|\frac{\mathsf{d}\mathbf{q}}{\mathsf{d} t }\right|^2 + \alpha_2 \left|\frac{\mathsf{d}^2\mathbf{q}}{\mathsf{d} t^2 }\right|^2 + \alpha_3\left|\frac{\mathsf{d}^3\mathbf{q}}{\mathsf{d} t^3 }\right|^2 + \alpha_4\left|\frac{\mathsf{d}^4\mathbf{q}}{\mathsf{d} t^4 }\right|^2 \mathsf{d} t \ \ \ \ \ (1) $$

It may be proven that such a problem can be subdivided in two steps

1. Find the family of optimal curves that joint waypoints
2. Compute time instants ${t_0, t_1, ...,t_N, t_{N + 1}}$ where the optimal curves must be attached

The step 1. is done by solving a linear ordinary differential equation. One method to achieve 2. is to formulate an optimization problem (e.g. a gradient based one).

Optimal curves

We underline that this library leverages on the general theory of linear ODEs. It may be proven that any optimal of (1) solves the following linear ODE, which turn out to be the Euler-Lagrange equations at each interval $[t_i, t_{i + 1}]$

$$ -\alpha_1\frac{\mathsf{d}^2\mathbf{q}}{\mathsf{d} t^2 } + \alpha_2 \frac{\mathsf{d}^4\mathbf{q}}{\mathsf{d} t^4 } - \alpha_3\frac{\mathsf{d}^6\mathbf{q}}{\mathsf{d} t^6 } + \alpha_4 \frac{\mathsf{d}^8\mathbf{q}}{\mathsf{d} t^8 } = 0\ \ \ \ \ (2) $$

with the following boundary conditions

$$ \mathbf{q}(t_i) = \mathbf{w}_i\ \ \ \ \ \ \ \mathbf{q}(t_{i+1}) = \mathbf{w}_{i+1}\ \ \ \ \ \ \ \ \ \ \ \ \ \ (3) $$

Because the ODE (2) is linear, we can compute its general suction depending on the value of the coefficients $\alpha_i$.

In fact, the general solution of (2) may be written as a piecewise function defined at each interval as

$$ \mathbf{q} = \sum_{i=1}^{n_b} \mathbf{y}_i^j B_i(t) \ \ \ \ \text{if}\ \ \ \ t \in [t_{j}, t_{j + 1}]\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (4) $$

where $n_b$, $B_i(t)$ depend on the coefficients $\alpha_i$ and $\mathbf{y}_i^j$ are column vectors in which represents the curve uniquely at the interval $[t_j, t_{j + 1}]$.

If we stack the column vectors $\mathbf{y}_i^j$ in a suitable way we obtain the column vector $\mathbf{y}$ used in (0). In fact, the equation (0) is obtained after applying to (4) the waypoint constrains and the boundary conditions.

After substituting (4) in (1) we obtain the following expression

$$ I=\mathbf{y}^\top \mathbf{Q}(\boldsymbol{\tau}) \mathbf{y}\ \ \ \ \ \ \ \ \ \ \ \ \ (5) $$

Finally we can substitute (0) in (5) to obtain the representation of (1) subject to the waypoint constrains as a function of $N$ real variables:

$$ I=I(\boldsymbol{\tau})=\mathbf{b}^{\top}\mathbf{A}^{-\top}(\boldsymbol{\tau})\mathbf{Q}(\boldsymbol{\tau}) \mathbf{A}^{-1}(\boldsymbol{\tau})\mathbf{b}\ \ \ \ \ \ \ \ \ \ \ \ \ (6) $$

Software architecture

From the formalization of the optimization problem, we derive that a flexible and uniform methodology for the construction of the problem of optimizing (1) consists in designing an abstract representation of the basis $B_i(t)$ in (4) capable of building in an automatic fashion the constraint (0), the cost function (5) and their derivatives.

In fact, note that the input of any gradient-based optimizer is the expression (6) and its derivatives. This library provides a template class to represent the basis $B_i(t)$ and a series of procedures which utilizes these basis as an input and then generate (0), (6) and their derivatives.

This library provides the class gsplines::basis::Basis which represent an arbitrary set of linearly independent functions and the class gsplines::GSpline which implements the definition $\star$. In addition this library provides the class gsplines::functions::FunctionBase and gsplines::functions::FunctionExpression that allows to define arbitrary curves in $\mathbb{R}^n$ with the possibility of performing algebraic operations and derivation.

Arbitrary function definition and algebraic operations

The abstract class gsplines::functions::FunctionBase represents objects that can be evaluated at points of some interval of $\mathbb{R}$. To inherit from this class the auxiliary template class gsplines::functions::FunctionInheritanceHelper allows to define a custom function which $k$-derivative is implemented as another class using the Curiously recurring template pattern. For example, the exponential and other elementary functions are declared here.

The class gsplines::functions::FunctionExpression inherits from gsplines::functions::FunctionBase and contain an array of pointers to gsplines::functions::FunctionBase called function_array_. The evaluation operator of gsplines::functions::FunctionExpression evaluate the desired algebraic operation between the functions in function_array_. This architecture allow to implement complex algebraic expressions by recursively calling the evaluation of each function in function_array_.

Requirements

• numpy
• scipy
• matplotlib
• coinor-libipopt-dev
• ros-noetic-ifopt
• libeigen3-dev

Publications

• Rojas, Rafael A., et al. "A variational approach to minimum-jerk trajectories for psychological safety in collaborative assembly stations." IEEE Robotics and Automation Letters 4.2 (2019): 823-829.

@article{rojas2019variational,
  title={A variational approach to minimum-jerk trajectories for psychological safety in collaborative assembly stations},
  author={Rojas, Rafael A and Garcia, Manuel A Ruiz and Wehrle, Erich and Vidoni, Renato},
  journal={IEEE Robotics and Automation Letters},
  volume={4},
  number={2},
  pages={823--829},
  year={2019},
  publisher={IEEE}
}

• Rojas, Rafael A., et al. "Combining safety and speed in collaborative assembly systems–An approach to time optimal trajectories for collaborative robots." Procedia CIRP 97 (2021): 308-312.

@article{rojas2021combining,
  title={Combining safety and speed in collaborative assembly systems--An approach to time optimal trajectories for collaborative robots},
  author={Rojas, Rafael A and Garcia, Manuel A Ruiz and Gualtieri, Luca and Rauch, Erwin},
  journal={Procedia CIRP},
  volume={97},
  pages={308--312},
  year={2021},
  publisher={Elsevier}
}

• Rojas, Rafael A., and Renato Vidoni. "Designing fast and smooth trajectories in collaborative workstations." IEEE Robotics and Automation Letters 6.2 (2021): 1700-1706.

@article{rojas2021designing,
  title={Designing fast and smooth trajectories in collaborative workstations},
  author={Rojas, Rafael A and Vidoni, Renato},
  journal={IEEE Robotics and Automation Letters},
  volume={6},
  number={2},
  pages={1700--1706},
  year={2021},
  publisher={IEEE}
}

• Rojas, Rafael A., Erich Wehrle, and Renato Vidoni. "A multicriteria motion planning approach for combining smoothness and speed in collaborative assembly systems." Applied Sciences 10.15 (2020): 5086.

@article{rojas2020multicriteria,
  title={A multicriteria motion planning approach for combining smoothness and speed in collaborative assembly systems},
  author={Rojas, Rafael A and Wehrle, Erich and Vidoni, Renato},
  journal={Applied Sciences},
  volume={10},
  number={15},
  pages={5086},
  year={2020},
  publisher={Multidisciplinary Digital Publishing Institute}
}

• Rojas, Rafael A., Andrea Giusti, and Renato Vidoni. "Online Computation of Time-Optimization-Based, Smooth and Path-Consistent Stop Trajectories for Robots." Robotics 11.4 (2022): 70.

@article{rojas2022online,
  title={Online Computation of Time-Optimization-Based, Smooth and Path-Consistent Stop Trajectories for Robots},
  author={Rojas, Rafael A and Giusti, Andrea and Vidoni, Renato},
  journal={Robotics},
  volume={11},
  number={4},
  pages={70},
  year={2022},
  publisher={MDPI}
}

gsplines_cpp's Issues

Implement CI/CD docker building

Create two docker images

• Docker with vim and gsplines installed
• Docker with vim, ros and gsplines inalled

Implement Gsplines as Functions

Take the current implementation of PiecewiseFunction and rename it as Gspline which inherits from Function.

This overrides some method as as

PiecewiseFunction operator*(double); // multiply coefficients
PiecewiseFunction operator*=(double);
PiecewiseFunction operator+(const PiecewiseFunction&); // assert same domain, same partition, same basis, add vectors
PiecewiseFunction operator+=(const PiecewiseFunction&);

Function that returns GL spline evaluated ad GL points

Make Basis singleton

Motivation

The Basis represent an abstract mathematical object that is unique, in the sense that it only provide the evaluations of objects that do not change. In the same sense, they must not have a state. Basis are stateless. I have used mutable private variables to improve performance and avoid creating new objects.

Because its abstract and unique nature, Basis must exists in a unique non writable instance.

Proposed solution

class Base {};

template <typename Current> class Helper {
public:
  template <typename... Ts> static std::shared_ptr<Current> get(Ts &&... args) {
    auto const values{std::make_tuple(args...)};

    static std::map<std::tuple<Ts...>, std::shared_ptr<Current>> database;

    if (database.find(values) == database.end()) {
      database[values] = std::make_shared<Current>(args...);
    }

    return database[values];
  }
};
int main() { return 0; }

Implement basis factory

• remove gsplines::basis::string_to_basis
• Implement

std::shared_ptr<Basis> get_basis(const std::string& _name, std::size_t _dim, const std::vector<double> &_params){
  if(_name == 'lagrange'){
    return BasisLagrange::get(_params);
  }else
  if(_name== 'legendre'){
     return BasisLegendre::get(_dim);
  }
}

• Implement BasisLegendre::get(std::size_t _dim)
• Implement BasisLagrange::get(const std::vector<double> &_points)
• Implement BasisLagrange::get(const Eigen::VectorXd &_points)

pybind11: set class holder to shared pointers

By defaults pybind handle just unique ptr see here. We need to expose to python functions that returns shared pointers.

see here

Compare with other libraries

• Compare with ruckig paper
• Compare with Reflexxes paper
• ecl_geometry
• pathfinder
• Topico

Change the name of PiecewiseFunction to GSpline

In fact, our implementation of a PiecewiseFunction is the exact definition of a Gspline

Implement specific sobolev semi-norm minimizers

• Minimum speed (broken lines)
• Minimum Acceleration
• Minimum Jerk
• Minimum Snap
• Minumim Crackle

Add Positive Polynomial support

Add support to compute polynomial gsplines based on Positive Polynomials.

• Implement representation using PSD matrices. (for hankel matrix handling see here)
• Implement the differential of the exponential see here
• Implement an oder solver boost/numeric/odeint

Implement Sobolev norm for `FunctionBase`

Implement linear dilation of GSplines

Implement a member method of GSpline that linearly scales the domain.

Identify how to define complex functions of GSplines and its derivatives

Motivation, Approximation with GSplines

Let be an interval, be a function and let be the set of piecewise polynomials of degree with pieces.

Theorem For every there exists an and map such that

Theorem For every there exists an and map such that if

such that

Definitions

• We call the polynomial approximation of
• We call the finite-dimensional approximation of

Remark Let . Then we can define

• The polynomial approximation of the restriction of to the polynomials
• The finite-dimensional approximation of the restriction of to the polynomials

Requirement: We need a class that can represent arbitrary functions an its derivative

Derivative

Integral

## Newton-Euler

Here the finite dimensional approximation using the Lagrange interpolation at some points can be computed with

Direct kinematics

Here we need to represent a manifold (quaternions, euler angles. etc)

Arc lent of the direct kinematics

How we want to use this representation: We want to build new functionals from old ones

• Gspline to Gspline functional a simple function that we cal call as

GSpline torque = NewtonEuler::evaluate(q);
GSplineFunctional diff = NewtonEuler::evaluate(q);

GSpline pose = DirectKinematic::evaluate(q);

class ArcLen: public Functinal{
public:
ArcLen(GSplineSet);/*Domain, codom dim, number of intervals, interval width*/
ReturnType_1 value(const GSpline &_in) const{
return _in | gspline_set_.derivative | gspline_set_.norm | gspline_set_.integral;
}
ReturnType_2 diff(const GSpline &_in){
return gspline_set_.integral / gspline_set.norm(_in | derivative) * derivative
}
};

Remarks

• In the above example we desire to have a ReturnType_1 which can behave as both, a vector and a GSpline.
  • Cast from GSpline to Vector: This is not simple, because a gspline is constituted by two vectors. We can return the vector that represent a GSpline as an element of a vector space GSpline::get_coefficients.
  • Cast Vector to GSpline, from where we take the intervals?: This is impossible without adding more data structures.
• In the above example we desire to have a ReturnType_2 which can behave as both, a matrix and a Functional.
  • this is done here
  • This case is simpler, because ReturnType_2 does not need explicitly the interval lenghts because we don't have to cast it into a GSpline

Implement a converion from `FunctionBase` to `FunctionExpression` and derivative in `FunctionBase`

Change Architecture

Evaluate if we need to change the inheritance architecture to this

Implement interpolator

Implement interpolator

Implement a class that given

• a sequence of waypoints in R^n
• a sequence of instances or distance between instants
• a basis
  generates a gspline of the given bases that interpolates the waypoints at the instances.

Procedure

• Implement the interpolation matrix and vector builder
• Implement the derivative of the interpolation matrix with respect to the length of the intervals

Optimize Compiler Flags

See here and here

Make Domain linear dilation, constfunction and identity inherit from CanonicPolynomial

Implement Google Test

Change the test from CTest to google test

Implement specific class for Gauss-Lobatto Lagrange polynomial

• with sum operations with other GaussLobatoLagrange GSpline.
  • This require to test
    1. If the basis are the same
    2. If they have both the same number of intervals
    3. If they have both the sames dimension of co-domain
• #45
• multiplication by real scalar
• multiplication by scalar valued GSpline
• division of a real number by a scalar valued GSpline
• assignment multiplication by real scalar (consider write this for splines)
• Matrix multiplication, left (transpose) and right
• Multiplication by a history of matrices (Different matrix for a different instances) left (transpose) and right
• euclidean norm, takes a spline and returns a scalar valued spline.
• dot product
• #49

Implement `forward_list` in `FunctionExpression` as `Function` Container

Motivation

Now FunctionExpression stores the generic functions to evaluate in a std::vector here.

Given the actual use that we have defined to evaluate function expressions we require to implement an array of basic functions in such a way that we

• Optimize insertions at the end and at the beginning.
• Optimized iteration from the beginning to the end.

These properties may be obtained with std::list.

Procedurea

As the repo is at the commit 6420cba we require to change the vector implentation of FunctionExpression::function_array_ to FunctionExpression::functions_list_.
This require the following changes

• All calls to push_back remain.
• The insertions at the end may optimized in function multiplication by scalar
• The implementations of operation evaluation and derivative
  • Evaluation composition here
  • Evaluation of concatenation here
  • Computation of the firs derivative of composition here
  • Computation of the derivative of composition here
  • Computation of the derivative of the concatenation here

Automatize tests by running python scripts

Documentation

• here

Consider new architecture to introduce covariant clones and deriv

Many times the derivative of a function returns a pointer to an specific type of function a we don't want to handle it a a FunctionExpression.
Read this, this and this

increment/decrement operation by a GSpline.

Test sum and multiplications

Add method to retrive waypoints from GSpline

1. Introduce new member

Eigen::MatrixXd waypoints_;

2. Initialize waypoints

waypoints_(_n_intervals+1, _codom_dim)

3. Initialize waypoints in the copy constructor and the move constructor
4. Call the evaluation function in the constructure

GSpline::value(domain_break_points_, waypoints)

5. Create method to get the waypoints

Eigen::Ref<const Eigen::MatrixXd> get_waypoints() const{return waypoints_;}

Implement Elementary fuction

Constant Function

class ConstantFunction:public Function{
private:
    eigen::VectorXd values_;
public:
    ConstantFunction(std::pair _domain, eigen::Ref<eigen::VectorXd> values_):Function(_domain, values_.size()), values_(_values){
    }

  ConstantFunction(std::pair _domain, std::size_t _codom_dim, double value_):Function(_domain, _codom_dim), values_(_values, _codom_dim){
    }
Eigen::MatrixXd
  operator()(const Eigen::Ref<const Eigen::VectorXd> _domain_points) override {
    return values_;
  };

  std::unique_ptr<ConstFunction> clone() const override {
    return std::make_unique<ConstFunction>(*this);
  }
  std::unique_ptr<ConstFunction> deriv(int _deg) override {
    return std::make_unique<ConstFunction>(get_domain(), get_codom_dim(), 0.0);
  }
};

Sine

class Sine:public Function{

public:
    Sine(std::pair _domain):Function(_domain, 1){
           return _values.sine()
    }
Eigen::MatrixXd
  operator()(const Eigen::Ref<const Eigen::VectorXd> _domain_points) override {
   
      return _values.sine()
  };

  std::unique_ptr<ConstFunction> clone() const override {
    return std::make_unique<ConstFunction>(*this);
  }
  std::unique_ptr<Cos> deriv(int _deg) override {
    return std::make_unique<Cos>(Cos(get_domain));
  }
};

Cosine

class Cos:public Function{

public:
    Cos(std::pair _domain):Function(_domain, 1){
           return _values.sine()
    }
Eigen::MatrixXd
  operator()(const Eigen::Ref<const Eigen::VectorXd> _domain_points) override {
   
      return _values.cos()
  };

  std::unique_ptr<Cos> clone() const override {
    return std::make_unique<Cos>(*this);
  }
  std::unique_ptr<FunctionExpression> deriv(int _deg) override {
    return std::make_unique<FunctionExpression>(ConstFunction(get_domain, 1, -1.0)*Sin(get_domain));
  }
};

Exponential

class Cos:public Function{

public:
    Cos(std::pair _domain):Function(_domain, 1){
           return _values.sine()
    }
Eigen::MatrixXd
  operator()(const Eigen::Ref<const Eigen::VectorXd> _domain_points) override {
   
      return _values.cos()
  };

  std::unique_ptr<Cos> clone() const override {
    return std::make_unique<Cos>(*this);
  }
  std::unique_ptr<FunctionExpression> deriv(int _deg) override {
    return std::make_unique<FunctionExpression>(ConstFunction(get_domain, 1, -1.0)*Sin(get_domain));
  }
};

Evaluate if we should implement new features to concatenate functions

Description

Implment

• concat_at_end
• concat_at_the_beginig
• Implement shift_domain translates "rigidly" the domain of the function
  • Implement shift_domain in Function as virtual
  • Implement set_domain in FunctionExpression

Both functions do the following

1. Update the domain of the Function expression
2. Append to the array of "elementary" functions on the corrects lace.
3. Update the domain of the "elementary" functions

Re-implement functions and functions form R^n into R^m

At the commit 8b53d24, functions are only intended to represent curves. However, in order to reresent maps between manifolds we can expand this type into maps from R^n into R^m.

• Define an abstract class called domain with the following pure virtual methods
  • With equal operator
  • with "contain" method which accept a point
  • with "contain" method which accepts another domain
  • with dimension method "dim"
  • make is inheritance helper
  • clone, move_clone
• redefine the base function class
  • it has a domain as a member
  • Modify its value member. Now value should accept an array of elements of its domain (Vector<double, N, 1>)
  • [ ]

Fix implementation of binary operations

For each operation implemnet

FunctionExpression FunctionExpression::binary_operation(const Function& );
FunctionExpression FunctionExpression::binary_operation(const FunctionExpression& );
FunctionExpression FunctionExpression::binary_operation(FunctionExpression&& );

FunctionExpression &Function::assignment_operation( const Function& );
FunctionExpression &Function::assignment_operation(const FunctionExpression& );
FunctionExpression &Function::assignment_operation( FunctionExpression&& );

FunctionExpression Function::binary_operations( const Function& );
FunctionExpression Function::binary_operations( const FunctionExpression& );
FunctionExpression Function::binary_operations( FunctionExpression&& );

Example: if binary operation is operator*() the assignment operation is operator*=

Introduce a class which represent a set of GSplines

Motivation

We are working with two different sets of GSpline

• Set 1: (Is a Manifold) The set of GSplines that passes through a series of waypoints. Is defined by
  • Sequence of waypoints
  • Domain
  • Basis
• Set 2: (Is a Vector Space) The set of Gsplines with unique domain interval lengths. Is defined by
  • Interval lengths
  • Domain
  • Basis
• Remark On space of functions and domain information Note that the Set 2 is a vector space for a given domain. Function with different domain cannot be added. In the case of Set 1, we have that all problems that involve it have a fixed domain.
• Remark on different domains We underline that the GSplines defined in different domains are isomorphic. We need to apply a linear dilation if the proportion between the lengths of the intervals rest the same.

A GSpline set can be a GSpline factory

Introduce Manifold gspline support

Motivation

Many times we desire to compute a motion in an arbitrary manifold. For example, a motion in SO(3), a motion in SE(3), in a sphere or a plane.

Scope

This issue ask for the implementation of generalized splines in Manifolds. For example

• Simple computation of gpline on a surface given a sequence of waypoints on such a surface.
• Simple computation of gsplines on SO(3).
• Simple computation of gsplines on SE(3).

What is a GSpline on a Manifold

GSplines are naturally defined in because polynomials an other spaces of functions as as co-domain. This makes a Gspline on depend on the particular basis or chart used to represent the coordinates of the co-domain.

Definition (Gspline on a Manifold Given a Chart) Let be a smooth manifold, be an open set and be a chart. We define a Gspline on given as a map such that there exists a unique Gspline such that .

Remark Note that a Gspline on given may not be a gspline given another chart.

Make basis buffer mutable

Currently in the evaluation of a GSpline here we use a static variable to store the values of the basis.

This is a bug, because this static variable will be common to all instances of the class GSpline (very bad).

We have to declare a mutable member of the GSpline in order to avoid this.

Implement dot product

Implement the dot product between vector-valued functions.

Add to Basis move_clone

Implement function composition

Task

• Implement compose on FunctionExpression

This function does the following

1. inserts the elementary function and the beginning of the array of elementary functions.
2. Update the domain of the FunctionExpression

Basis parameters and equality

In order to implement basis a singletons (#40) we should be ale to compare basis with a set of parameters that that identify each basis uniquely. This would allow to compare basis as.

class Basis {
protected:
  Eigen::VectorXd parameters_;
  Eigen::VectorXi parameters_int_;
...
public:
bool operator=(const Basis& _that){
     return get_name() == _that.get_name() and 
                gsplines::tools::approximate_equal(paramters_, _that.parameters, 1.0e-4) and
                (parameters_int_.array() == paramters_int.array()).all();
}

This will require to force the user to provide the basis parameters at the constructor.

Implement continuity and derivative matrices into Basis

Implement Function evaluation with result as reference

The method value of Function Expression calls the method operator() which is the pure virtual method of Function.

This architecture does not allow the user of this API to have the possibility of saving the computational effort for the result vector allocation.

For this reasong we have to modifiy this architecture.

1. Implement in FunctionExpression

void value(Eigen::Red<const Eigen::VectorXd> _domain_points, Eigen::Ref<Eigen::MatrixXd> _result)

• Redefine the definitions of Eval_Function_Type and Deriv_Function_Type
• rewrite eval_unique_functions
• eval_single_functions
• eval_sum_functions
• eval_mul_functions
• eval_compose_functions
• eval_concat_functions

2. Rewrite the following methods of FunctionExpression in such a way that they call the function defined in 1.
   • value
   • operator()
3. Rewrite the definition of Function in such a way that void value(...) (the function in 1.) is pure virutal. Remove operator()
4. Rewrite the definition of elementary functions, substitute operator() for value
5. Rewrite the definition of PiecewiseFunction

Implement addition and multilication assigment operations

In function expression, we have to implement the addition and multiplication assignement operation

• This will be the unique operations that change the operation type of the function expression.
• Them return void

Re-implemnet derivatives and operations of `FunctionExpression`

Now FunctionExpression contains an array of unique pointers to FunctionBase. As a consequence the call to the element of this arraywill calle FunctionBase::deriv which is not virtual. For this reason we have to implement the generation of derivatives as old-pointers in FunctionExpression::deriv_impl. This also require to change the definitions of the derivative operations

• Change the type of FunctionExpression::Deriv_Function_Type to

  typedef FunctionExpression* (Deriv_Function_Type)(
      const std::list<std::unique_ptr<FunctionBase>> &, std::size_t);

• Change the signature of all functions that derivative expressions in the declaration and definition
  • deriv_unique_functions
  • deriv_sum_functions
  • deriv_mul_functions
  • deriv_compose_functions
  • deriv_concat_functions
• Change the return type in the definition
  • deriv_unique_functions
  • deriv_sum_functions
  • deriv_mul_functions
  • deriv_compose_functions
  • deriv_concat_functions
• Change the implementation of the algebraic operations in order to implement the FunctionBase lists
  • Unique Function Operations
  • Sum operations
  • Multiplication Operations
  • Composition Operations
  • Concatenation Operations

Implement CI/CD with github actions and create .deb packages and upload them into releases

• use releases
• install apt-get from github releases

Implemente simple GSpline linear operations with SFINAE

template <typename T, typename std::enable_if_t<
                          std::is_base_of_v<GSplineBase, T>> * = nullptr>
T operator*(double _a, const T &_that);

template <typename T, typename std::enable_if_t<
                          std::is_base_of_v<GSplineBase, T>> * = nullptr>
T operator*(double _a, T &&_that);

Implemtn r-value method modifier

Read this.
We can specify in C++ when a method has to be called from an lvalue or a rvalue.

Write the basis as templates

Description

The basis take as constructor argument its dimension. We have to change this and make that basis are templates of its dimension.

Consider implementing a "automated" downcasting tecniche from FunctionExpressions with single functions into their specific function

Use

gsplines::GSpline curve(...);
gsplines::functions::FunctionExpression curve_dot = curve.derivate()
gsplines::GSpline curve_2 = curve_dot;

See here

Implement move constructor of PiecewiseFunction

Fix Basis implementation and GSpline derivative

The implementation of the basis must be fixed in order to reduce the code in their derived classes.
We require

• Fast implementation of n-th derivative matrix (returned as a referece)
• Fast implementation of the n-th matrix of the sobolev norm

This may be related to #2 as efficiently would also require to define derivative matrices as static members, i.e. common to all instances of a given basis.

read this

read this

this

this

Simplify the logic of `FunctionExpression` operations

Introduction

Binary Operations

Each binary operation has 4 implementations:

  FunctionExpression operator+(const FunctionExpression &that) const &;
  FunctionExpression operator+(FunctionExpression &&that) const &;
  FunctionExpression operator+(const FunctionExpression &that) &&;
  FunctionExpression operator+(FunctionExpression &&that) &&;

The reason of this is to reduce the copy operations and move objects efficienty in complex operations like f1*f2*(f1.compose(f2)+f3)+ f4.compose(f5). In this kind of cases we can exploit the move semantics to avoid copying temporal objects. Instead, we move then and use their memory.

The procedures are:

• FunctionExpression operator+(const FunctionExpression &that) const &; Copy the list of FunctionBase from both and build the new array of functions properly
• FunctionExpression operator+(FunctionExpression &&that) const &; Copy the list of FunctionBase of the lhs and move the own list into the new array of functions, which is constructed properly
• FunctionExpression operator+(const FunctionExpression &that) &&; Copy the lhs list of FunctionBase and move the own into the new array of functions, which is constructed properly
• FunctionExpression operator+(FunctionExpression &&that) const &&; move the lhs list of FunctionBase and move the own into the new array of functions, which is constructed properly

Possible binary operations depending on the type of the arguments

Each expression has a type, and it evaluates and derivatives it self depending on this type. In order to avoid excessive nesting of functions expressions, we require to identify in each operations what is the type of the this and the lhs Expressions.
For each operations the cases are:

• this and _lhs are not of the operation type: In this case we have to nest functions expressions inside the function array of the resulting function.
• this and _lhs are of the operation type: In this case we can copy/move the elements if both lists of FunctionBase and place these items directly on the list of the result.
• this is of the operation type and _lhs is not of the operation type: In this case we can copy/move into the result list the elements of the list of this and nest the _lhs.
• this is not of the operation type and _lhs is of the operation type: In this case we nest into the result list this and place into the resulting list the elements of the list of _lhs.

Issue: We need and efficienty way to handle this 16 cases

In fact, writing the code for this is error-prone and exhausting. Moreover, the order in which the elements are placed on the result array list depends on the operation and the dimension of the right and left hand side of the operation. So, in the case of the multiplication, we have to handle 32 cases.

Implement Lagrange Basis and associated tools

1. Implement the basis of Lagrange polynomials for a given set of points.

   • Implement computation of barycentric weights with algorithm 30 of David A. Kopriva Implementing Spectral Methods for Partial Differential Equations
   • Compute Lagrange Polynomials Implement computation of lagrange polinomialas with algorithm 34 of David A. Kopriva Implementing Spectral Methods for Partial Differential Equations
   • Compute Lagrange Polynomials derivative Algorithm 36 of Kopriva
   • Compute derivative matrix Algorithm 37 of Kopriva.

2. Implement auxiliar tools to compute Gauss-Lobatto points to be used with Lagrange Polynomials