1 |
|
|
// |
2 |
|
|
// Copyright (c) 2014 CNRS |
3 |
|
|
// Authors: Florent Lamiraux |
4 |
|
|
// |
5 |
|
|
// This file is part of hpp-core |
6 |
|
|
// hpp-core 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-core 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 |
|
|
#include <hpp/constraints/locked-joint.hh> |
20 |
|
|
#include <hpp/core/config-projector.hh> |
21 |
|
|
#include <hpp/pinocchio/configuration.hh> |
22 |
|
|
#include <hpp/pinocchio/device.hh> |
23 |
|
|
#include <hpp/rbprm/interpolation/polynom-trajectory.hh> |
24 |
|
|
#include <hpp/rbprm/interpolation/time-constraint-utils.hh> |
25 |
|
|
|
26 |
|
|
using namespace hpp::core; |
27 |
|
|
|
28 |
|
|
namespace hpp { |
29 |
|
|
namespace rbprm { |
30 |
|
|
namespace interpolation { |
31 |
|
|
PolynomTrajectory::PolynomTrajectory(PolynomPtr_t polynom, |
32 |
|
|
core::value_type subSetStart, |
33 |
|
|
core::value_type subSetEnd) |
34 |
|
|
: parent_t(interval_t(0, subSetEnd - subSetStart), 3, 3), |
35 |
|
|
polynom_(polynom), |
36 |
|
|
subSetStart_(subSetStart), |
37 |
|
|
subSetEnd_(subSetEnd), |
38 |
|
|
length_(subSetEnd - subSetStart) { |
39 |
|
|
timeRange(interval_t(subSetStart_, subSetEnd_)); |
40 |
|
|
assert(length_ >= 0); |
41 |
|
|
assert(!constraints()); |
42 |
|
|
} |
43 |
|
|
|
44 |
|
|
pinocchio::value_type normalize(const PolynomTrajectory& path, |
45 |
|
|
pinocchio::value_type param) { |
46 |
|
|
value_type u; |
47 |
|
|
if (path.timeRange().second == 0) |
48 |
|
|
u = 0; |
49 |
|
|
else |
50 |
|
|
u = (param - path.timeRange().first) / |
51 |
|
|
(path.timeRange().second - path.timeRange().first); |
52 |
|
|
return u; |
53 |
|
|
} |
54 |
|
|
|
55 |
|
|
PolynomTrajectory::PolynomTrajectory(const PolynomTrajectory& path) |
56 |
|
|
: parent_t(interval_t(0, path.length_), 3, 3), |
57 |
|
|
polynom_(path.polynom_), |
58 |
|
|
subSetStart_(path.subSetStart_), |
59 |
|
|
subSetEnd_(path.subSetEnd_), |
60 |
|
|
length_(path.length_) { |
61 |
|
|
timeRange(path.timeRange()); |
62 |
|
|
} |
63 |
|
|
|
64 |
|
|
bool PolynomTrajectory::impl_compute(ConfigurationOut_t result, |
65 |
|
|
value_type param) const { |
66 |
|
|
if (param == timeRange().first || timeRange().second == 0) { |
67 |
|
|
result = initial(); |
68 |
|
|
} else { |
69 |
|
|
result = polynom_->operator()(param); |
70 |
|
|
} |
71 |
|
|
return true; |
72 |
|
|
} |
73 |
|
|
|
74 |
|
|
PathPtr_t PolynomTrajectory::extract(const interval_t& subInterval) const { |
75 |
|
|
return PolynomTrajectory::create(polynom_, subInterval.first, |
76 |
|
|
subInterval.second); |
77 |
|
|
} |
78 |
|
|
} // namespace interpolation |
79 |
|
|
} // namespace rbprm |
80 |
|
|
} // namespace hpp |