1 |
|
|
// Copyright (c) 2014, LAAS-CNRS |
2 |
|
|
// Authors: Steve Tonneau (steve.tonneau@laas.fr) |
3 |
|
|
// |
4 |
|
|
// This file is part of hpp-rbprm. |
5 |
|
|
// hpp-rbprm is free software: you can redistribute it |
6 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
7 |
|
|
// License as published by the Free Software Foundation, either version |
8 |
|
|
// 3 of the License, or (at your option) any later version. |
9 |
|
|
// |
10 |
|
|
// hpp-rbprm is distributed in the hope that it will be |
11 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
12 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
13 |
|
|
// General Lesser Public License for more details. You should have |
14 |
|
|
// received a copy of the GNU Lesser General Public License along with |
15 |
|
|
// hpp-rbprm. If not, see <http://www.gnu.org/licenses/>. |
16 |
|
|
|
17 |
|
|
#include <hpp/core/bi-rrt-planner.hh> |
18 |
|
|
#include <hpp/rbprm/interpolation/interpolation-constraints.hh> |
19 |
|
|
#include <hpp/rbprm/interpolation/limb-rrt.hh> |
20 |
|
|
#include <hpp/rbprm/interpolation/time-constraint-utils.hh> |
21 |
|
|
|
22 |
|
|
namespace hpp { |
23 |
|
|
using namespace core; |
24 |
|
|
namespace rbprm { |
25 |
|
|
namespace interpolation { |
26 |
|
|
void SetLimbRRTConstraints::operator()(LimbRRTHelper &helper, const State &from, |
27 |
|
|
const State &to) const { |
28 |
|
|
CreateContactConstraints<LimbRRTHelper>(helper, from, to); |
29 |
|
|
} |
30 |
|
|
|
31 |
|
|
core::PathPtr_t limbRRT(RbPrmFullBodyPtr_t fullbody, |
32 |
|
|
core::ProblemPtr_t referenceProblem, |
33 |
|
|
const rbprm::CIT_State &startState, |
34 |
|
|
const rbprm::CIT_State &endState, |
35 |
|
|
const std::size_t numOptimizations, |
36 |
|
|
const std::size_t maxIteration) { |
37 |
|
|
LimbRRTShooterFactory shooterFactory; |
38 |
|
|
SetLimbRRTConstraints constraintFactory; |
39 |
|
|
return interpolateStates<LimbRRTHelper, LimbRRTShooterFactory, |
40 |
|
|
SetLimbRRTConstraints, CIT_State>( |
41 |
|
|
fullbody, referenceProblem, shooterFactory, constraintFactory, startState, |
42 |
|
|
endState, numOptimizations, false, 0.001, maxIteration); |
43 |
|
|
} |
44 |
|
|
|
45 |
|
|
core::PathPtr_t limbRRTFromPath(RbPrmFullBodyPtr_t fullbody, |
46 |
|
|
core::ProblemPtr_t referenceProblem, |
47 |
|
|
const PathPtr_t refPath, |
48 |
|
|
const CIT_StateFrame &startState, |
49 |
|
|
const CIT_StateFrame &endState, |
50 |
|
|
const std::size_t numOptimizations) { |
51 |
|
|
LimbRRTShooterFactory shooterFactory; |
52 |
|
|
SetLimbRRTConstraints constraintFactory; |
53 |
|
|
return interpolateStatesFromPath<LimbRRTHelper, LimbRRTShooterFactory, |
54 |
|
|
SetLimbRRTConstraints>( |
55 |
|
|
fullbody, referenceProblem, shooterFactory, constraintFactory, refPath, |
56 |
|
|
startState, endState, numOptimizations); |
57 |
|
|
} |
58 |
|
|
|
59 |
|
|
} // namespace interpolation |
60 |
|
|
} // namespace rbprm |
61 |
|
|
} // namespace hpp |