Commit 26611a19 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update Spline API.

parent 20c733d5
......@@ -23,6 +23,7 @@
# include <hpp/pinocchio/device.hh>
# include <hpp/core/fwd.hh>
# include <hpp/core/steering-method/fwd.hh>
# include <hpp/core/config.hh>
namespace hpp {
......@@ -109,7 +110,22 @@ namespace hpp {
/** Returns a vector \f$ (v_i) \f$ as
* \f[
* v_i = T^{-k} b_i^{(k)}(\frac{t - t_0}{T})
* v_i = b_i^{(k)}(u)
* \f]
**/
static void timeFreeBasisFunctionDerivative (const size_type order, const value_type& u, BasisFunctionVector_t& res);
static void timeFreeBasisFunctionDerivative (const size_type order, const value_type& u, vectorOut_t res)
{
assert (res.size() == NbCoeffs);
BasisFunctionVector_t tmp;
timeFreeBasisFunctionDerivative(order, u, tmp);
res = tmp;
}
/** Returns a vector \f$ (v_i) \f$ as
* \f[
* v_i = T^{-k} b_i^{(k)}(u)
* \f]
**/
void basisFunctionDerivative (const size_type order, const value_type& u, BasisFunctionVector_t& res) const;
......@@ -264,6 +280,8 @@ namespace hpp {
mutable vector_t velocity_;
mutable PowersOfT_t powersOfT_;
friend class steeringMethod::Spline<_PolynomeBasis, _Order>;
}; // class Spline
/// \}
} // namespace path
......
......@@ -84,18 +84,18 @@ namespace hpp {
// Compute the matrices
// TODO calls to basisFunctionDerivative could be cached as they do not
// depend on the inputs.
p->basisFunctionDerivative(0, 0, coeffs.row(0));
SplinePath::timeFreeBasisFunctionDerivative(0, 0, coeffs.row(0));
pinocchio::difference<hpp::pinocchio::LieGroupTpl>(device_.lock(), q1, p->base(), rhs.row(0));
for (std::size_t i = 0; i < order1.size(); ++i)
p->basisFunctionDerivative(order1[i], 0, coeffs.row(i+1));
SplinePath::timeFreeBasisFunctionDerivative(order1[i], 0, coeffs.row(i+1));
rhs.middleRows(1, order1.size()).transpose() = derivatives1;
size_type row = 1 + order1.size();
p->basisFunctionDerivative(0, 1, coeffs.row(row));
SplinePath::timeFreeBasisFunctionDerivative(0, 1, coeffs.row(row));
pinocchio::difference<hpp::pinocchio::LieGroupTpl>(device_.lock(), q2, p->base(), rhs.row(row));
++row;
for (std::size_t i = 0; i < order2.size(); ++i)
p->basisFunctionDerivative(order2[i], 1, coeffs.row(i+row));
SplinePath::timeFreeBasisFunctionDerivative(order2[i], 1, coeffs.row(i+row));
rhs.middleRows(row, order2.size()).transpose() = derivatives2;
// Solve the problem
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment