1 |
|
|
// |
2 |
|
|
// Copyright (c) 2017 CNRS |
3 |
|
|
// Authors: Pierre Fernbach |
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/pinocchio/configuration.hh> |
20 |
|
|
#include <hpp/pinocchio/device.hh> |
21 |
|
|
#include <hpp/rbprm/interpolation/spline/bezier-path.hh> |
22 |
|
|
#include <hpp/util/debug.hh> |
23 |
|
|
#include <hpp/util/exception.hh> |
24 |
|
|
|
25 |
|
|
namespace hpp { |
26 |
|
|
namespace rbprm { |
27 |
|
|
|
28 |
|
|
using core::ConfigurationIn_t; |
29 |
|
|
using core::ConfigurationOut_t; |
30 |
|
|
using core::DevicePtr_t; |
31 |
|
|
using core::interval_t; |
32 |
|
|
using core::Path; |
33 |
|
|
using pinocchio::value_type; |
34 |
|
|
|
35 |
|
|
BezierPath::BezierPath(const DevicePtr_t& robot, const bezier_Ptr& curve, |
36 |
|
|
core::ConfigurationIn_t init, |
37 |
|
|
core::ConfigurationIn_t end, interval_t timeRange) |
38 |
|
|
: parent_t(timeRange, robot->configSize(), robot->numberDof()), |
39 |
|
|
device_(robot), |
40 |
|
|
curve_(curve), |
41 |
|
|
initial_(init), |
42 |
|
|
end_(end) { |
43 |
|
|
hppDout(notice, "Create a bezier path, with init config : " |
44 |
|
|
<< pinocchio::displayConfig(initial_)); |
45 |
|
|
hppDout(notice, " end config : " |
46 |
|
|
<< pinocchio::displayConfig(end_)); |
47 |
|
|
assert(timeRange.first >= curve_->min() && |
48 |
|
|
"The time range is outside the curve definition"); |
49 |
|
|
assert(timeRange.second <= curve_->max() && |
50 |
|
|
"The time range is outside the curve definition"); |
51 |
|
|
} |
52 |
|
|
|
53 |
|
|
BezierPath::BezierPath(const core::DevicePtr_t& robot, |
54 |
|
|
std::vector<bezier_t::point_t>::const_iterator wpBegin, |
55 |
|
|
std::vector<bezier_t::point_t>::const_iterator wpEnd, |
56 |
|
|
core::ConfigurationIn_t init, |
57 |
|
|
core::ConfigurationIn_t end, core::interval_t timeRange) |
58 |
|
|
: parent_t(timeRange, robot->configSize(), robot->numberDof()), |
59 |
|
|
device_(robot), |
60 |
|
|
curve_(bezier_Ptr( |
61 |
|
|
new bezier_t(wpBegin, wpEnd, timeRange.second - timeRange.first))), |
62 |
|
|
initial_(init), |
63 |
|
|
end_(end) { |
64 |
|
|
hppDout(notice, "Create a bezier path, with init config : " |
65 |
|
|
<< pinocchio::displayConfig(initial_)); |
66 |
|
|
hppDout(notice, " end config : " |
67 |
|
|
<< pinocchio::displayConfig(end_)); |
68 |
|
|
assert(timeRange.first == 0 && |
69 |
|
|
"Bezier path cannot be created from waypoint with initiale time " |
70 |
|
|
"different from 0"); |
71 |
|
|
} |
72 |
|
|
|
73 |
|
|
BezierPath::BezierPath(const BezierPath& path) |
74 |
|
|
: parent_t(path), |
75 |
|
|
device_(path.device_), |
76 |
|
|
curve_(path.curve_), |
77 |
|
|
initial_(path.initial_), |
78 |
|
|
end_(path.end_) {} |
79 |
|
|
|
80 |
|
|
BezierPath::BezierPath(const BezierPath& path, |
81 |
|
|
const core::ConstraintSetPtr_t& constraints) |
82 |
|
|
: parent_t(path, constraints), |
83 |
|
|
device_(path.device_), |
84 |
|
|
curve_(path.curve_), |
85 |
|
|
initial_(path.initial_), |
86 |
|
|
end_(path.end_) |
87 |
|
|
|
88 |
|
|
{} |
89 |
|
|
|
90 |
|
|
/// Get the initial configuration |
91 |
|
|
core::Configuration_t BezierPath::initial() const { |
92 |
|
|
core::Configuration_t result(device_->configSize()); |
93 |
|
|
core::value_type u = |
94 |
|
|
curve_->min() + timeRange().first / (curve_->max() - curve_->min()); |
95 |
|
|
if (u < 0.) // may happen because of float precision |
96 |
|
|
u = 0.; |
97 |
|
|
pinocchio::interpolate(device_, initial_, end_, u, result); |
98 |
|
|
result.head<3>() = (*curve_)(timeRange().first); |
99 |
|
|
return result; |
100 |
|
|
} |
101 |
|
|
|
102 |
|
|
/// Get the final configuration |
103 |
|
|
core::Configuration_t BezierPath::end() const { |
104 |
|
|
core::Configuration_t result(device_->configSize()); |
105 |
|
|
core::value_type u = |
106 |
|
|
curve_->min() + timeRange().second / (curve_->max() - curve_->min()); |
107 |
|
|
if (u > 1.) // may happen because of float precision |
108 |
|
|
u = 1.; |
109 |
|
|
pinocchio::interpolate(device_, initial_, end_, u, result); |
110 |
|
|
result.head<3>() = (*curve_)(timeRange().second); |
111 |
|
|
return result; |
112 |
|
|
} |
113 |
|
|
|
114 |
|
|
bool BezierPath::impl_compute(ConfigurationOut_t result, value_type t) const { |
115 |
|
|
if (t < timeRange().first) { |
116 |
|
|
hppDout(warning, "Bezier path called with time outside time definition"); |
117 |
|
|
result = initial(); |
118 |
|
|
return true; |
119 |
|
|
} |
120 |
|
|
if (t > timeRange().second) { |
121 |
|
|
hppDout(warning, "Bezier path called with time outside time definition"); |
122 |
|
|
result = end(); |
123 |
|
|
return true; |
124 |
|
|
} |
125 |
|
|
value_type u = curve_->min() + t / (curve_->max() - curve_->min()); |
126 |
|
|
if (timeRange().second == 0) u = 0; |
127 |
|
|
pinocchio::interpolate(device_, initial_, end_, u, result); |
128 |
|
|
|
129 |
|
|
result.head<3>() = (*curve_)(t); |
130 |
|
|
return true; |
131 |
|
|
} |
132 |
|
|
|
133 |
|
|
} // namespace rbprm |
134 |
|
|
} // namespace hpp |