Skip to content

Commit

Permalink
Merge 8c23309 into b3cc8e0
Browse files Browse the repository at this point in the history
  • Loading branch information
fdarricau committed May 14, 2015
2 parents b3cc8e0 + 8c23309 commit 41ca546
Show file tree
Hide file tree
Showing 13 changed files with 439 additions and 168 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ SET(PROJECT_URL "http://github.com/roboptim/roboptim-core-manifold")
SET(HEADERS
${CMAKE_SOURCE_DIR}/include/roboptim/core/manifold-map/decorator/descriptive-wrapper.hh
${CMAKE_SOURCE_DIR}/include/roboptim/core/manifold-map/decorator/descriptive-wrapper.hxx
${CMAKE_SOURCE_DIR}/include/roboptim/core/manifold-map/decorator/dispatcher.hh
${CMAKE_SOURCE_DIR}/include/roboptim/core/manifold-map/decorator/function-on-manifold.hh
${CMAKE_SOURCE_DIR}/include/roboptim/core/manifold-map/decorator/function-on-manifold.hxx
${CMAKE_SOURCE_DIR}/include/roboptim/core/manifold-map/decorator/manifold-desc.hh
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -100,15 +100,16 @@ namespace roboptim
operator<<(std::ostream& o, DescriptiveWrapper<U, V>& descWrap)
{
o <<
"Displaying info about function in DescriptiveWrapper :" << "\n" <<
"the function is " << descWrap.fct().getName() << "\n" <<
"the function input size is " << descWrap.fct().inputSize() << "\n" <<
"the function output size is " << descWrap.fct().outputSize() << "\n" <<
"Displaying info about the function manifold in DescriptiveWrapper :" << "\n" <<
"the manifold is " << descWrap.manifold().name() << "\n" <<
"Is the manifold elementary ? : " <<
(descWrap.manifold().isElementary() ? "yes" : "no" ) << "\n" <<
"the manifold dimension is " << descWrap.manifold().representationDim() << "\n";
"Displaying info about function in DescriptiveWrapper:" << incindent <<
iendl << "the function is " << descWrap.fct().getName() <<
iendl << "the function input size is " << descWrap.fct().inputSize() <<
iendl << "the function output size is " << descWrap.fct().outputSize() << decindent <<
iendl << "Displaying info about the function manifold in DescriptiveWrapper:" << incindent <<
iendl << "the manifold is " << descWrap.manifold().name() <<
iendl << "Is the manifold elementary? " <<
(descWrap.manifold().isElementary() ? "yes" : "no" ) <<
iendl << "the manifold dimension is " << descWrap.manifold().representationDim()
<< decindent;
return o;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,8 @@ namespace roboptim
template <typename U, typename V>
DescriptiveWrapper<U, V>::DescriptiveWrapper
(const U* fct, mnf::Manifold& manifold)
: fct_ (fct),
manifold_ (&manifold)
: fct_ (fct),
manifold_ (&manifold)
{
checkDimension();
}
Expand All @@ -56,14 +56,14 @@ namespace roboptim
{
long size = manifold_->representationDim();
if (fct_->inputSize() != size)
{
std::stringstream* error = new std::stringstream;
(*error) << "Representation dims mismatch on manifold "
<< manifold_->name() << " using function "
<< fct_->getName() << ". Expected dimension :" << size
<< ", actual one is " << fct_->inputSize();
throw std::runtime_error (error->str());
}
{
std::stringstream* error = new std::stringstream;
(*error) << "Representation dims mismatch on manifold "
<< manifold_->name() << " using function "
<< fct_->getName() << ". Expected dimension :" << size
<< ", actual one is " << fct_->inputSize();
throw std::runtime_error (error->str());
}
}

} // end of namespace roboptim.
Expand Down
100 changes: 100 additions & 0 deletions include/roboptim/core/manifold-map/decorator/dispatcher.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,100 @@
// Copyright (C) 2015 by Félix Darricau, AIST, CNRS, EPITA
// Grégoire Duchemin, AIST, CNRS, EPITA
//
// This file is part of the roboptim.
//
// roboptim is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// roboptim is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with roboptim. If not, see <http://www.gnu.org/licenses/>.

#ifndef ROBOPTIM_CORE_MANIFOLD_MAP_DECORATOR_DISPATCHER_HH
# define ROBOPTIM_CORE_MANIFOLD_MAP_DECORATOR_DISPATCHER_HH

# include <manifolds/Manifold.h>
# include <manifolds/RealSpace.h>

namespace roboptim
{
template <typename U> class FunctionOnManifold;

template <typename U, typename T>
struct Dispatcher;

template <typename U>
struct Dispatcher <U, roboptim::EigenMatrixDense>
{
static void unmapGradient(const FunctionOnManifold<U>* instance,
typename U::gradient_ref gradient)
{
for (long i = 0; i < instance->mappingFromFunctionSize_; ++i)
{
gradient(static_cast<long>(instance->mappingFromFunction_[i])) = instance->mappedGradient_(i);
}
}

static void jacobian(const FunctionOnManifold<U>* instance,
typename U::jacobian_ref jacobian)
{
for (long j = 0; j < jacobian.rows(); ++j)
{
instance->fct_->gradient(instance->mappedGradient_, instance->mappedInput_, j);
Dispatcher<U, roboptim::EigenMatrixDense>::unmapGradient(instance, jacobian.row(j));
}
}

static void applyDiff(const FunctionOnManifold<U>* instance)
{
instance->manifold_->applyDiffRetractation(instance->tangentMappedJacobian, instance->mappedJacobian_, instance->mappedInput_);
}
};

template <typename U>
struct Dispatcher <U, roboptim::EigenMatrixSparse>
{
static void unmapGradient(const FunctionOnManifold<U>* instance,
typename U::gradient_ref gradient)
{
assert(gradient.cols() == instance->inputSize());

for (int i = 0; i < instance->mappingFromFunctionSize_; ++i)
{
gradient.coeffRef(static_cast<int>(instance->mappingFromFunction_[i])) = instance->mappedGradient_.coeffRef(i);
}
}

static void jacobian(const FunctionOnManifold<U>* instance,
typename U::jacobian_ref jacobian)
{
assert(jacobian.cols() == instance->inputSize());
assert(jacobian.rows() == instance->outputSize());

for (int j = 0; j < jacobian.rows(); ++j)
{
instance->fct_->gradient(instance->mappedGradient_, instance->mappedInput_, j);
for (int i = 0; i < instance->mappingFromFunctionSize_; ++i)
{
jacobian.coeffRef(j,static_cast<int>(instance->mappingFromFunction_[i])) = instance->mappedGradient_.coeffRef(i);
}
}
}

static void applyDiff(const FunctionOnManifold<U>* instance)
{
if (instance->manifold_->getTypeId() != mnf::RealSpace(1).getTypeId())
throw std::runtime_error("No Sparse support for non RealSpaces manifolds");
else
instance->tangentMappedJacobian = instance->mappedJacobian_;
}
};
} // end of namespace roboptim

#endif //! ROBOPTIM_CORE_MANIFOLD_MAP_DECORATOR_DISPATCHER_HH
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
# include <roboptim/core/detail/autopromote.hh>
# include <roboptim/core/differentiable-function.hh>
# include <roboptim/core/manifold-map/decorator/descriptive-wrapper.hh>
# include <roboptim/core/manifold-map/decorator/dispatcher.hh>

# include <manifolds/Manifold.h>
# include <manifolds/RealSpace.h>
Expand Down Expand Up @@ -73,7 +74,7 @@ namespace roboptim
std::vector<const mnf::Manifold*> restrictedManifolds,
std::vector<std::pair<long, long>> restrictions)
: detail::AutopromoteTrait<U>::T_type
(problemManifold.representationDim(),
(static_cast<size_type>(problemManifold.representationDim()),
descWrap.fct().outputSize (),
(boost::format ("%1%")
% descWrap.fct().getName ()).str ()),
Expand Down Expand Up @@ -162,7 +163,7 @@ namespace roboptim
/// \brief new input mapped to the restricted problem
mutable vector_t mappedInput_;
/// \brief new gradient mapped to the restricted problem
mutable derivative_t mappedGradient_;
mutable gradient_t mappedGradient_;
/// \brief new jacobian mapped to the restricted problem
mutable jacobian_t mappedJacobian_;

Expand All @@ -175,13 +176,6 @@ namespace roboptim
void mapArgument(const_argument_ref argument)
const;

/// \brief gets the gradient from the restricted problem
///
/// \param gradient the output gradient
/// \param mappedGradient the gradient computed to unmap
void unmapGradient(gradient_ref gradient, const_gradient_ref mappedGradient)
const;

/// \brief unmap the jacobian from the restricted problem
///
/// \param jacobian the jacobian to unmap
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -333,9 +333,11 @@ namespace roboptim
onTangentSpace = false;

this->mappedInput_ = Eigen::VectorXd::Zero(this->mappingFromFunctionSize_);
this->mappedGradient_ = Eigen::VectorXd::Zero(this->mappingFromFunctionSize_);
this->mappedJacobian_ = Eigen::MatrixXd::Zero(descWrap.fct().outputSize(), this->mappingFromFunctionSize_);
this->mappedGradient_ = gradient_t(static_cast<int> (this->mappingFromFunctionSize_));
this->mappedJacobian_ = jacobian_t(descWrap.fct().outputSize(), static_cast<int> (this->mappingFromFunctionSize_));
this->tangentMappedJacobian = Eigen::MatrixXd::Zero(descWrap.fct().outputSize(), this->tangentMappingFromFunctionSize_);
this->mappedGradient_.setZero();
this->mappedJacobian_.setZero();

// This lambda computes the actual mapping between a manifold and the one
// in its place in the global manifold of the problem.
Expand Down Expand Up @@ -437,17 +439,6 @@ namespace roboptim
}
}

template <typename U>
void
FunctionOnManifold<U>::unmapGradient(gradient_ref gradient, const_gradient_ref mappedGradient)
const
{
for (long i = 0; i < this->mappingFromFunctionSize_; ++i)
{
gradient(static_cast<long>(this->mappingFromFunction_[i])) = mappedGradient(i);
}
}

template <typename U>
void
FunctionOnManifold<U>::unmapTangentJacobian(mnf::RefMat jacobian)
Expand Down Expand Up @@ -480,7 +471,7 @@ namespace roboptim
this->fct_->gradient(this->mappedGradient_, this->mappedInput_, functionId);

gradient.setZero();
this->unmapGradient(gradient, this->mappedGradient_);
roboptim::Dispatcher<U, typename U::traits_t>::unmapGradient(this, gradient);
}

template <typename U>
Expand All @@ -492,17 +483,13 @@ namespace roboptim
this->mapArgument(argument);
jacobian.setZero();

for (long j = 0; j < jacobian.rows(); ++j)
{
this->fct_->gradient(this->mappedGradient_, this->mappedInput_, j);
this->unmapGradient(jacobian.row(j), this->mappedGradient_);
}
roboptim::Dispatcher<U, typename U::traits_t>::jacobian(this, jacobian);
}

template <typename U>
void
FunctionOnManifold<U>::manifold_jacobian (mnf::RefMat jacobian,
const_argument_ref argument)\
const_argument_ref argument)
const
{
this->mapArgument(argument);
Expand All @@ -512,7 +499,7 @@ namespace roboptim

this->fct_->jacobian(this->mappedJacobian_, this->mappedInput_);

this->manifold_->applyDiffRetractation(this->tangentMappedJacobian, this->mappedJacobian_, this->mappedInput_);
roboptim::Dispatcher<U, typename U::traits_t>::applyDiff(this);

this->unmapTangentJacobian(jacobian);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,12 +45,12 @@ namespace roboptim
template<class U>
struct BoundsAndScalesSetter
{
void setBounds(typename Function::intervals_t& bounds);
void setBounds(typename U::function_t::intervals_t& bounds);
void setScales(typename U::scales_t& scales);

private:
std::pair<typename Function::intervals_t, typename U::scales_t>& bNSPair_;
BoundsAndScalesSetter(std::pair<typename Function::intervals_t, typename U::scales_t>& bNSPair);
std::pair<typename U::function_t::intervals_t, typename U::scales_t>& bNSPair_;
BoundsAndScalesSetter(std::pair<typename U::function_t::intervals_t, typename U::scales_t>& bNSPair);
friend ProblemFactory<U>;
};

Expand Down Expand Up @@ -90,7 +90,7 @@ namespace roboptim
/// \brief elementary manifolds composing the global manifold of the problem
std::map<long, const mnf::Manifold*> elementaryInstanceManifolds_;
/// \brief bounds and scales for each constraint
std::vector<std::pair<typename Function::intervals_t, typename U::scales_t>> boundsAndScales_;
std::vector<std::pair<typename U::function_t::intervals_t, typename U::scales_t>> boundsAndScales_;
/// \brief each std::function instantiate and add a constraint to the problem
std::vector<std::function<void(ProblemOnManifold<U>&, const mnf::Manifold&)>> lambdas_;
/// \brief instantiate and add the objective function to the problem
Expand Down
22 changes: 11 additions & 11 deletions include/roboptim/core/manifold-map/decorator/problem-factory.hxx
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ BoundsAndScalesSetter<U> ProblemFactory<U>::addConstraint(DescriptiveWrapper<V,
this->addElementaryManifolds(instanceManifold);
this->boundsAndScales_.push_back(std::make_pair(typename V::intervals_t(), typename U::scales_t()));

std::pair<typename Function::intervals_t, typename U::scales_t>* bNSPair = &(this->boundsAndScales_.back());
std::pair<typename U::function_t::intervals_t, typename U::scales_t>* bNSPair = &(this->boundsAndScales_.back());

// We need the type of the function to instantiate the FunctionOnManifold
// wrapper, hence why we capture the information we need here and wait
Expand All @@ -73,7 +73,7 @@ BoundsAndScalesSetter<U> ProblemFactory<U>::addConstraint(DescriptiveWrapper<V,
(ProblemOnManifold<U>& problem,
const mnf::Manifold& globMani)
{
std::pair<typename Function::intervals_t, typename U::scales_t>* bNSPair = &(this->boundsAndScales_[i]);
std::pair<typename U::function_t::intervals_t, typename U::scales_t>* bNSPair = &(this->boundsAndScales_[i]);

::boost::shared_ptr<FunctionOnManifold<typename V::parent_t>>
funcOnMani(new FunctionOnManifold<typename V::parent_t>
Expand All @@ -86,7 +86,7 @@ BoundsAndScalesSetter<U> ProblemFactory<U>::addConstraint(DescriptiveWrapper<V,
<< bNSPair->first.size() << " of constrained function "
<< funcOnMani->getName() << "." << std::endl
<< "Assuming no bounds..." << std::endl;
bNSPair->first.push_back(Function::makeInfiniteInterval());
bNSPair->first.push_back(U::function_t::makeInfiniteInterval());
}

while (bNSPair->second.size() < static_cast<size_t>(funcOnMani->outputSize()))
Expand Down Expand Up @@ -214,29 +214,29 @@ ProblemFactory<U>::ProblemFactory()
{
this->objLambda_ = [](mnf::CartesianProduct& globMani)
{
typename GenericConstantFunction<EigenMatrixDense>::vector_t offset (1);
typename GenericConstantFunction<typename U::function_t::traits_t>::vector_t offset (1);
offset.setZero();
GenericConstantFunction<EigenMatrixDense>* cst = new GenericConstantFunction<EigenMatrixDense>(globMani.representationDim(),offset);
GenericConstantFunction<typename U::function_t::traits_t>* cst = new GenericConstantFunction<typename U::function_t::traits_t>(static_cast<typename U::function_t::size_type>(globMani.representationDim()),offset);

// We make a mnf::Manifold& out of the CartesianProduct& to explicitly call
// the overloaded constructor instead of the variadic one
mnf::Manifold& globberMani = globMani;

DescriptiveWrapper<GenericConstantFunction<EigenMatrixDense>, ManiDesc<>>
DescriptiveWrapper<GenericConstantFunction<typename U::function_t::traits_t>, ManiDesc<>>
descWrap(cst, globberMani);

FunctionOnManifold<typename GenericConstantFunction<EigenMatrixDense>::parent_t>*
objOnMani = new FunctionOnManifold<typename GenericConstantFunction<EigenMatrixDense>::parent_t>
FunctionOnManifold<typename GenericConstantFunction<typename U::function_t::traits_t>::parent_t>*
objOnMani = new FunctionOnManifold<typename GenericConstantFunction<typename U::function_t::traits_t>::parent_t>
(descWrap, globMani, globMani);

return new ProblemOnManifold<U>(globMani, *objOnMani);
};
};
}

// ---- //

template<class U>
void BoundsAndScalesSetter<U>::setBounds(typename Function::intervals_t& bounds)
void BoundsAndScalesSetter<U>::setBounds(typename U::function_t::intervals_t& bounds)
{
this->bNSPair_.first.clear();
this->bNSPair_.first.insert(this->bNSPair_.first.end(), bounds.begin(), bounds.end());
Expand All @@ -250,7 +250,7 @@ void BoundsAndScalesSetter<U>::setScales(typename U::scales_t& scales)
}

template<class U>
BoundsAndScalesSetter<U>::BoundsAndScalesSetter(std::pair<typename Function::intervals_t, typename U::scales_t>& bNSPair)
BoundsAndScalesSetter<U>::BoundsAndScalesSetter(std::pair<typename U::function_t::intervals_t, typename U::scales_t>& bNSPair)
: bNSPair_(bNSPair)
{}

Expand Down
Loading

0 comments on commit 41ca546

Please sign in to comment.