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/com-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 |
|
|
ComTrajectory::ComTrajectory(pinocchio::vector3_t init, |
32 |
|
|
pinocchio::vector3_t end, |
33 |
|
|
pinocchio::vector3_t initSpeed, |
34 |
|
|
pinocchio::vector3_t acceleration, |
35 |
|
|
core::value_type length) |
36 |
|
|
: parent_t(interval_t(0, length), 3, 3), |
37 |
|
|
initial_(init), |
38 |
|
|
end_(end), |
39 |
|
|
initSpeed_(initSpeed), |
40 |
|
|
half_acceleration_(acceleration / 2), |
41 |
|
|
acceleration_(acceleration), |
42 |
|
|
length_(length) { |
43 |
|
|
assert(length >= 0); |
44 |
|
|
assert(!constraints()); |
45 |
|
|
} |
46 |
|
|
|
47 |
|
|
pinocchio::value_type normalize(const ComTrajectory& path, |
48 |
|
|
pinocchio::value_type param) { |
49 |
|
|
value_type u; |
50 |
|
|
if (path.timeRange().second == 0) |
51 |
|
|
u = 0; |
52 |
|
|
else |
53 |
|
|
u = (param - path.timeRange().first) / |
54 |
|
|
(path.timeRange().second - path.timeRange().first); |
55 |
|
|
return u; |
56 |
|
|
} |
57 |
|
|
|
58 |
|
|
ComTrajectory::ComTrajectory(const ComTrajectory& path) |
59 |
|
|
: parent_t(interval_t(0, path.length_), 3, 3), |
60 |
|
|
initial_(path.initial_), |
61 |
|
|
end_(path.end_), |
62 |
|
|
initSpeed_(path.initSpeed_), |
63 |
|
|
half_acceleration_(path.half_acceleration_), |
64 |
|
|
acceleration_(path.acceleration_), |
65 |
|
|
length_(path.length_) {} |
66 |
|
|
|
67 |
|
|
bool ComTrajectory::impl_compute(ConfigurationOut_t result, |
68 |
|
|
value_type param) const { |
69 |
|
|
if (param == timeRange().first || timeRange().second == 0) { |
70 |
|
|
result = initial(); |
71 |
|
|
} else if (param == timeRange().second) { |
72 |
|
|
result = end(); |
73 |
|
|
} else { |
74 |
|
|
value_type u = normalize(*this, param) * length_; |
75 |
|
|
result = acceleration_ * u * u / 2. + initSpeed_ * u + initial_; |
76 |
|
|
} |
77 |
|
|
return true; |
78 |
|
|
} |
79 |
|
|
|
80 |
|
|
PathPtr_t ComTrajectory::extract(const interval_t& subInterval) const { |
81 |
|
|
// Length is assumed to be proportional to interval range |
82 |
|
|
value_type l = |
83 |
|
|
std::min(fabs(subInterval.second - subInterval.first), length_); |
84 |
|
|
|
85 |
|
|
bool success; |
86 |
|
|
Configuration_t q1((*this)(subInterval.first, success)); |
87 |
|
|
Configuration_t q2((*this)(subInterval.second, success)); |
88 |
|
|
value_type u = normalize(*this, subInterval.first) * length_; |
89 |
|
|
pinocchio::vector3_t speedAtQ1 = acceleration_ * u + initSpeed_; |
90 |
|
|
PathPtr_t result = ComTrajectory::create(q1, q2, speedAtQ1, acceleration_, l); |
91 |
|
|
return result; |
92 |
|
|
} |
93 |
|
|
} // namespace interpolation |
94 |
|
|
} // namespace rbprm |
95 |
|
|
} // namespace hpp |