1 |
|
|
// |
2 |
|
|
// Copyright (c) 2014 CNRS |
3 |
|
|
// Authors: Steve Tonneau (steve.tonneau@laas.fr) |
4 |
|
|
// |
5 |
|
|
// This file is part of hpp-rbprm. |
6 |
|
|
// hpp-rbprm is free software: you can redistribute it |
7 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
8 |
|
|
// License as published by the Free Software Foundation, either version |
9 |
|
|
// 3 of the License, or (at your option) any later version. |
10 |
|
|
// |
11 |
|
|
// hpp-rbprm is distributed in the hope that it will be |
12 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
13 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
14 |
|
|
// General Lesser Public License for more details. You should have |
15 |
|
|
// received a copy of the GNU Lesser General Public License along with |
16 |
|
|
// hpp-core If not, see |
17 |
|
|
// <http://www.gnu.org/licenses/>. |
18 |
|
|
|
19 |
|
|
#ifndef HPP_RBPRM_TIME_CONSTRAINT_UTILS_HH |
20 |
|
|
#define HPP_RBPRM_TIME_CONSTRAINT_UTILS_HH |
21 |
|
|
|
22 |
|
|
#include <hpp/core/config-projector.hh> |
23 |
|
|
#include <hpp/core/problem.hh> |
24 |
|
|
#include <hpp/rbprm/config.hh> |
25 |
|
|
#include <hpp/rbprm/interpolation/time-constraint-helper.hh> |
26 |
|
|
#include <hpp/rbprm/interpolation/time-constraint-path-validation.hh> |
27 |
|
|
#include <hpp/rbprm/interpolation/time-constraint-steering.hh> |
28 |
|
|
#include <hpp/rbprm/interpolation/time-dependant.hh> |
29 |
|
|
#include <hpp/rbprm/rbprm-device.hh> |
30 |
|
|
#include <hpp/rbprm/rbprm-fullbody.hh> |
31 |
|
|
#include <hpp/rbprm/rbprm-state.hh> |
32 |
|
|
#include <hpp/rbprm/tools.hh> |
33 |
|
|
#include <map> |
34 |
|
|
#include <string> |
35 |
|
|
#include <vector> |
36 |
|
|
|
37 |
|
|
namespace hpp { |
38 |
|
|
namespace rbprm { |
39 |
|
|
namespace interpolation { |
40 |
|
|
|
41 |
|
|
template <class Helper_T> |
42 |
|
|
void SetPathValidation(Helper_T& helper) { |
43 |
|
|
TimeConstraintPathValidationPtr_t pathVal = |
44 |
|
|
TimeConstraintPathValidation::create( |
45 |
|
|
helper.fullBodyDevice_, 0.05, |
46 |
|
|
helper.fullBodyDevice_->configSize() - 1); |
47 |
|
|
helper.rootProblem_->pathValidation(pathVal); |
48 |
|
|
} |
49 |
|
|
|
50 |
|
|
template <class Helper_T> |
51 |
|
|
core::ConfigurationPtr_t TimeConfigFromDevice(const Helper_T& helper, |
52 |
|
|
const State& state, |
53 |
|
|
const double time) { |
54 |
|
|
core::Configuration_t config(helper.fullBodyDevice_->currentConfiguration()); |
55 |
|
|
config.head(state.configuration_.rows()) = state.configuration_; |
56 |
|
|
config[config.rows() - 1] = time; |
57 |
|
|
return core::ConfigurationPtr_t(new core::Configuration_t(config)); |
58 |
|
|
} |
59 |
|
|
|
60 |
|
|
inline void UpdateConstraints(core::ConfigurationOut_t configuration, |
61 |
|
|
const T_TimeDependant& tds, |
62 |
|
|
const std::size_t pathDofRank) { |
63 |
|
|
const core::value_type y = configuration[pathDofRank]; |
64 |
|
|
for (CIT_TimeDependant cit = tds.begin(); cit != tds.end(); ++cit) { |
65 |
|
|
(*cit)(y, configuration); |
66 |
|
|
} |
67 |
|
|
} |
68 |
|
|
} // namespace interpolation |
69 |
|
|
} // namespace rbprm |
70 |
|
|
} // namespace hpp |
71 |
|
|
#endif // HPP_RBPRM_TIME_CONSTRAINT_UTILS_HH |