Commit 27a00993 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Move convinience function to SplineGradientBasedAbstract

parent 65a65b89
......@@ -132,6 +132,23 @@ namespace hpp {
PathVectorPtr_t buildPathVector (const Splines_t& splines) const;
/// \name Spline convinience functions
/// \{
/// Copy a vector of Spline
static void copy (const Splines_t& in, Splines_t& out);
/// Sets the parameters each spline.
/// \todo make this function static (currently, it only needs the
/// robot number dof.
void updateSplines (Splines_t& spline, const vector_t& param) const;
/// Returns res = (1 - alpha) * a + alpha * b
static void interpolate (const Splines_t& a, const Splines_t& b,
const value_type& alpha, Splines_t& res);
/// \}
DevicePtr_t robot_;
private:
......
......@@ -124,19 +124,6 @@ namespace hpp {
template <typename Cost_t> bool checkHessian (const Cost_t& cost, const matrix_t& H, const Splines_t& splines) const;
/// \todo static
void copy (const Splines_t& in, Splines_t& out) const;
/// Returns res = (1 - alpha) * a + alpha * b
void updateSplines (Splines_t& spline, const vector_t& param) const;
/// Returns res = (1 - alpha) * a + alpha * b
/// \todo static
void interpolate (const Splines_t& a, const Splines_t& b,
const value_type& alpha, Splines_t& res) const;
void copyParam (const Splines_t& in, Splines_t& out) const;
// Continuity constraints
// matrix_t Jcontinuity_;
// vector_t rhsContinuity_;
......
......@@ -402,6 +402,39 @@ namespace hpp {
}
}
template <int _PB, int _SO>
void SplineGradientBasedAbstract<_PB, _SO>::copy
(const Splines_t& in, Splines_t& out)
{
out.resize(in.size());
for (std::size_t i = 0; i < in.size(); ++i)
out[i] = HPP_STATIC_PTR_CAST(Spline, in[i]->copy());
}
template <int _PB, int _SO>
void SplineGradientBasedAbstract<_PB, _SO>::updateSplines
(Splines_t& splines, const vector_t& param) const
{
size_type row = 0, size = robot_->numberDof() * Spline::NbCoeffs;
for (std::size_t i = 0; i < splines.size(); ++i) {
splines[i]->rowParameters(param.segment(row, size));
row += size;
}
}
template <int _PB, int _SO>
void SplineGradientBasedAbstract<_PB, _SO>::interpolate
(const Splines_t& a, const Splines_t& b, const value_type& alpha, Splines_t& res)
{
assert (a.size() == b.size() && b.size() == res.size());
assert (alpha >= 0 && alpha <= 1);
for (std::size_t i = 0; i < a.size(); ++i)
res[i]->rowParameters(
(1 - alpha) * a[i]->rowParameters()
+ alpha * b[i]->rowParameters());
}
// ----------- Instanciate -------------------------------------------- //
// template class SplineGradientBased<path::CanonicalPolynomeBasis, 1>; // equivalent to StraightPath
......
......@@ -538,7 +538,7 @@ namespace hpp {
Splines_t alphaSplines, collSplines;
Splines_t* currentSplines;
copy(splines, alphaSplines); copy(splines, collSplines);
Base::copy(splines, alphaSplines); Base::copy(splines, collSplines);
Reports_t reports;
QuadraticProblem QP(cost.inputDerivativeSize_);
......@@ -559,7 +559,7 @@ namespace hpp {
if (computeOptimum) {
// 6.2
constraint.computeSolution(QPc.xStar);
updateSplines(collSplines, constraint.xSol);
Base::updateSplines(collSplines, constraint.xSol);
cost.value(costLowerBound, collSplines);
hppDout (info, "Cost interval: [" << optimalCost << ", " << costLowerBound << "]");
currentSplines = &collSplines;
......@@ -567,7 +567,7 @@ namespace hpp {
computeOptimum = false;
}
if (computeInterpolatedSpline) {
interpolate(splines, collSplines, alpha, alphaSplines);
Base::interpolate(splines, collSplines, alpha, alphaSplines);
currentSplines = &alphaSplines;
minimumReached = false;
computeInterpolatedSpline = false;
......@@ -690,39 +690,6 @@ namespace hpp {
return ret;
}
template <int _PB, int _SO>
void SplineGradientBased<_PB, _SO>::copy
(const Splines_t& in, Splines_t& out) const
{
out.resize(in.size());
for (std::size_t i = 0; i < in.size(); ++i)
out[i] = HPP_STATIC_PTR_CAST(Spline, in[i]->copy());
}
template <int _PB, int _SO>
void SplineGradientBased<_PB, _SO>::updateSplines
(Splines_t& splines, const vector_t& param) const
{
size_type row = 0, size = robot_->numberDof() * Spline::NbCoeffs;
for (std::size_t i = 0; i < splines.size(); ++i) {
splines[i]->rowParameters(param.segment(row, size));
row += size;
}
}
template <int _PB, int _SO>
void SplineGradientBased<_PB, _SO>::interpolate
(const Splines_t& a, const Splines_t& b, const value_type& alpha, Splines_t& res) const
{
assert (a.size() == b.size() && b.size() == res.size());
assert (alpha >= 0 && alpha <= 1);
for (std::size_t i = 0; i < a.size(); ++i)
res[i]->rowParameters(
(1 - alpha) * a[i]->rowParameters()
+ alpha * b[i]->rowParameters());
}
// ----------- Instanciate -------------------------------------------- //
// template class SplineGradientBased<path::CanonicalPolynomeBasis, 1>; // equivalent to StraightPath
......
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