Commit 52915b1c authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add SimpleTimeParameterization

parent 8a7bceff
......@@ -92,6 +92,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/core/path-optimization/gradient-based.hh
include/hpp/core/path-optimization/partial-shortcut.hh
include/hpp/core/path-optimization/config-optimization.hh
include/hpp/core/path-optimization/simple-time-parameterization.hh
include/hpp/core/path-optimization/spline-gradient-based.hh
include/hpp/core/path-optimization/spline-gradient-based/linear-constraint.hh
include/hpp/core/path-optimizer.hh
......
......@@ -276,6 +276,8 @@ namespace hpp {
typedef boost::shared_ptr <PathLength> PathLengthPtr_t;
HPP_PREDEF_CLASS (PartialShortcut);
typedef boost::shared_ptr <PartialShortcut> PartialShortcutPtr_t;
HPP_PREDEF_CLASS (SimpleTimeParameterization);
typedef boost::shared_ptr <SimpleTimeParameterization> SimpleTimeParameterizationPtr_t;
HPP_PREDEF_CLASS (ConfigOptimization);
typedef boost::shared_ptr <ConfigOptimization>
ConfigOptimizationPtr_t;
......
// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core 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.
//
// hpp-core 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
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.
#ifndef HPP_CORE_PATH_OPTIMIZATION_SIMPLE_TIME_PARAMETERIZATION_HH
# define HPP_CORE_PATH_OPTIMIZATION_SIMPLE_TIME_PARAMETERIZATION_HH
# include <hpp/core/path-optimizer.hh>
namespace hpp {
namespace core {
namespace pathOptimization {
/// \addtogroup path_optimization
/// \{
/// Change the time of straight paths so that the velocity does not
/// exceeds the velocity limits.
///
/// Parameter SimpleTimeParameterization/Safety (value_type) defines
/// is used to rescale the velocity limit.
class HPP_CORE_DLLAPI SimpleTimeParameterization : public PathOptimizer
{
public:
/// Return shared pointer to new object.
static SimpleTimeParameterizationPtr_t create (const Problem& problem);
/// Optimize path
virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
protected:
SimpleTimeParameterization (const Problem& problem);
}; // class SimpleTimeParameterization
/// \}
} // namespace pathOptimization
} // namespace core
} // namespace hpp
#endif // HPP_CORE_PATH_OPTIMIZATION_SIMPLE_TIME_PARAMETERIZATION_HH
......@@ -62,6 +62,7 @@ locked-joint.cc
path-optimization/spline-gradient-based.cc #
path-optimization/partial-shortcut.cc #
path-optimization/config-optimization.cc#
path-optimization/simple-time-parameterization.cc#
path-planner.cc #
path-vector.cc #
path/spline.cc
......
// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core 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.
//
// hpp-core 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
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.
#include <hpp/core/path-optimization/simple-time-parameterization.hh>
#include <pinocchio/multibody/model.hpp>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/liegroup.hh>
#include <hpp/core/interpolated-path.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/straight-path.hh>
namespace hpp {
namespace core {
namespace pathOptimization {
SimpleTimeParameterizationPtr_t SimpleTimeParameterization::create (const Problem& problem)
{
SimpleTimeParameterizationPtr_t ptr (new SimpleTimeParameterization(problem));
return ptr;
}
PathVectorPtr_t SimpleTimeParameterization::optimize (const PathVectorPtr_t& path)
{
const value_type safety = problem().getParameter("SimpleTimeParameterization/safety", (value_type)1);
// Retrieve velocity limits
const DevicePtr_t& robot = problem().robot();
size_type d = robot->numberDof() - robot->extraConfigSpace().dimension();
vector_t ub ( robot->model().velocityLimit),
lb (-robot->model().velocityLimit),
cb ((ub + lb) / 2);
assert (cb.size() == d);
// The velocity must be in [lb, ub]
ub = cb + safety * (ub - cb);
lb = cb + safety * (lb - cb);
hppDout (info, "Lower velocity bound :" << lb.transpose());
hppDout (info, "Upper velocity bound :" << ub.transpose());
if ( ( ub.array() <= 0 ).any()
&& ( lb.array() >= 0 ).any())
throw std::invalid_argument ("The case where zero is not an admissible velocity is not implemented.");
vector_t ub_inv (ub.cwiseInverse());
vector_t lb_inv (lb.cwiseInverse());
// When ub or lb are NaN, set them to infinity, or, equivalently,
// set ub_inv or lb_inv to zero
ub_inv = (ub.array() == ub.array()).select(ub_inv, 0);
lb_inv = (lb.array() == lb.array()).select(lb_inv, 0);
hppDout (info, "Inverse of lower velocity bound :" << lb_inv.transpose());
hppDout (info, "Inverse of upper velocity bound :" << ub_inv.transpose());
PathVectorPtr_t input = PathVector::create(
path->outputSize(), path->outputDerivativeSize());
PathVectorPtr_t output = PathVector::create(
path->outputSize(), path->outputDerivativeSize());
path->flatten(input);
vector_t delta (robot->numberDof());
for (std::size_t i = 0; i < input->numberPaths(); ++i) {
PathPtr_t p = input->pathAtRank(i);
StraightPathPtr_t sp = HPP_DYNAMIC_PTR_CAST(StraightPath, p);
InterpolatedPathPtr_t ip = HPP_DYNAMIC_PTR_CAST(InterpolatedPath, p);
if (sp) {
pinocchio::difference <hpp::pinocchio::LieGroupTpl>
(robot, sp->end(), sp->initial(), delta);
// Shortest length
value_type l = std::max (
(delta.head(d).array() * ub_inv.array()).maxCoeff(),
(delta.head(d).array() * lb_inv.array()).maxCoeff());
output->appendPath (
StraightPath::create (robot, sp->initial(), sp->end(), l, sp->constraints())
);
} else if (ip) {
typedef InterpolatedPath::InterpolationPoints_t IPs_t;
const IPs_t& ips = ip->interpolationPoints();
value_type l = 0;
std::vector<value_type> ts; ts.reserve(ips.size());
for (IPs_t::const_iterator _ip2 = ips.begin(), _ip1 = _ip2++;
_ip2 != ips.end(); ++_ip1, ++_ip2) {
pinocchio::difference <hpp::pinocchio::LieGroupTpl>
(robot, _ip2->second, _ip1->second, delta);
// Shortest length
ts.push_back(l);
l += std::max (
(delta.head(d).array() * ub_inv.array()).maxCoeff(),
(delta.head(d).array() * lb_inv.array()).maxCoeff());
}
InterpolatedPathPtr_t nip = InterpolatedPath::create (robot, ip->initial(), ip->end(), l, ip->constraints());
std::size_t T = 1;
for (IPs_t::const_iterator _ip = (++ips.begin()); _ip != (--ips.end()); ++_ip)
nip->insert(ts[T], _ip->second);
output->appendPath(nip);
} else {
throw std::invalid_argument ("unknown type of paths.");
}
}
return output;
}
SimpleTimeParameterization::SimpleTimeParameterization (const Problem& problem):
PathOptimizer(problem) {}
} // namespace pathOptimization
} // namespace core
} // namespace hpp
......@@ -53,6 +53,7 @@
#include <hpp/core/path-optimization/spline-gradient-based.hh>
#include <hpp/core/path-optimization/partial-shortcut.hh>
#include <hpp/core/path-optimization/config-optimization.hh>
#include <hpp/core/path-optimization/simple-time-parameterization.hh>
#include <hpp/core/path-validation-report.hh>
// #include <hpp/core/problem-target/task-target.hh>
#include <hpp/core/problem-target/goal-configurations.hh>
......@@ -190,6 +191,7 @@ namespace hpp {
add <PathOptimizerBuilder_t> ("GradientBased", pathOptimization::GradientBased::create);
add <PathOptimizerBuilder_t> ("PartialShortcut", pathOptimization::PartialShortcut::create);
add <PathOptimizerBuilder_t> ("ConfigOptimization", pathOptimization::ConfigOptimization::create);
add <PathOptimizerBuilder_t> ("SimpleTimeParameterization", pathOptimization::SimpleTimeParameterization::create);
add <PathOptimizerBuilder_t> ("None", NoneOptimizer::create); // TODO: Delete me
// add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<path::CanonicalPolynomeBasis, 1>::create);
......
Markdown is supported
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