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/time-constraint-path.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 |
|
|
TimeConstraintPath::TimeConstraintPath(const DevicePtr_t& device, |
32 |
|
|
ConfigurationIn_t init, |
33 |
|
|
ConfigurationIn_t end, value_type length, |
34 |
|
|
const std::size_t pathDofRank, |
35 |
|
|
const T_TimeDependant& tds) |
36 |
|
|
: parent_t(interval_t(0, length), device->configSize(), |
37 |
|
|
device->numberDof()), |
38 |
|
|
device_(device), |
39 |
|
|
initial_(init), |
40 |
|
|
end_(end), |
41 |
|
|
pathDofRank_(pathDofRank), |
42 |
|
|
tds_(tds) { |
43 |
|
|
assert(device); |
44 |
|
|
assert(length >= 0); |
45 |
|
|
assert(!constraints()); |
46 |
|
|
} |
47 |
|
|
|
48 |
|
|
TimeConstraintPath::TimeConstraintPath(const DevicePtr_t& device, |
49 |
|
|
ConfigurationIn_t init, |
50 |
|
|
ConfigurationIn_t end, value_type length, |
51 |
|
|
ConstraintSetPtr_t constraints, |
52 |
|
|
const std::size_t pathDofRank, |
53 |
|
|
const T_TimeDependant& tds) |
54 |
|
|
: parent_t(interval_t(0, length), device->configSize(), device->numberDof(), |
55 |
|
|
constraints), |
56 |
|
|
device_(device), |
57 |
|
|
initial_(init), |
58 |
|
|
end_(end), |
59 |
|
|
pathDofRank_(pathDofRank), |
60 |
|
|
tds_(tds) { |
61 |
|
|
assert(device); |
62 |
|
|
assert(length >= 0); |
63 |
|
|
} |
64 |
|
|
|
65 |
|
|
TimeConstraintPath::TimeConstraintPath(const TimeConstraintPath& path) |
66 |
|
|
: parent_t(path), |
67 |
|
|
device_(path.device_), |
68 |
|
|
initial_(path.initial_), |
69 |
|
|
end_(path.end_), |
70 |
|
|
pathDofRank_(path.pathDofRank_), |
71 |
|
|
tds_(path.tds_) {} |
72 |
|
|
|
73 |
|
|
TimeConstraintPath::TimeConstraintPath(const TimeConstraintPath& path, |
74 |
|
|
const ConstraintSetPtr_t& constraints) |
75 |
|
|
: parent_t(path, constraints), |
76 |
|
|
device_(path.device_), |
77 |
|
|
initial_(path.initial_), |
78 |
|
|
end_(path.end_), |
79 |
|
|
pathDofRank_(path.pathDofRank_), |
80 |
|
|
tds_(path.tds_) {} |
81 |
|
|
|
82 |
|
|
pinocchio::value_type ComputeExtraDofValue( |
83 |
|
|
const std::size_t dofRank, const Configuration_t init, |
84 |
|
|
const Configuration_t end, const pinocchio::value_type normalizedValue) { |
85 |
|
|
double a = init[dofRank]; |
86 |
|
|
double b = end[dofRank]; |
87 |
|
|
return (b - a) * normalizedValue + a; |
88 |
|
|
} |
89 |
|
|
|
90 |
|
|
void TimeConstraintPath::updateConstraints( |
91 |
|
|
core::ConfigurationOut_t configuration) const { |
92 |
|
|
if (constraints() && constraints()->configProjector()) { |
93 |
|
|
UpdateConstraints(configuration, tds_, pathDofRank_); |
94 |
|
|
} |
95 |
|
|
} |
96 |
|
|
|
97 |
|
|
bool TimeConstraintPath::impl_compute(ConfigurationOut_t result, |
98 |
|
|
value_type param) const { |
99 |
|
|
if (param == timeRange().first || timeRange().second == 0) { |
100 |
|
|
result = initial(); |
101 |
|
|
} else if (param == timeRange().second) { |
102 |
|
|
result = end(); |
103 |
|
|
} else { |
104 |
|
|
value_type u; |
105 |
|
|
if (timeRange().second == 0) |
106 |
|
|
u = 0; |
107 |
|
|
else |
108 |
|
|
u = (param - timeRange().first) / |
109 |
|
|
(timeRange().second - timeRange().first); |
110 |
|
|
device_->RnxSOnConfigSpace()->interpolate(initial_, end_, u, result); |
111 |
|
|
pinocchio::value_type dof = |
112 |
|
|
ComputeExtraDofValue(pathDofRank_, initial_, end_, u); |
113 |
|
|
result[pathDofRank_] = dof; |
114 |
|
|
} |
115 |
|
|
updateConstraints(result); |
116 |
|
|
return true; |
117 |
|
|
} |
118 |
|
|
|
119 |
|
|
PathPtr_t TimeConstraintPath::extract(const interval_t& subInterval) const { |
120 |
|
|
// Length is assumed to be proportional to interval range |
121 |
|
|
value_type l = fabs(subInterval.second - subInterval.first); |
122 |
|
|
|
123 |
|
|
bool success; |
124 |
|
|
Configuration_t q1((*this)(subInterval.first, success)); |
125 |
|
|
if (!success) |
126 |
|
|
throw projection_error( |
127 |
|
|
"Failed to apply constraints in StraightPath::extract"); |
128 |
|
|
q1[pathDofRank_] = |
129 |
|
|
ComputeExtraDofValue(pathDofRank_, initial_, end_, |
130 |
|
|
(subInterval.first - timeRange().first) / |
131 |
|
|
(timeRange().second - timeRange().first)); |
132 |
|
|
Configuration_t q2((*this)(subInterval.second, success)); |
133 |
|
|
if (!success) |
134 |
|
|
throw projection_error( |
135 |
|
|
"Failed to apply constraints in StraightPath::extract"); |
136 |
|
|
q2[pathDofRank_] = |
137 |
|
|
ComputeExtraDofValue(pathDofRank_, initial_, end_, |
138 |
|
|
(subInterval.second - timeRange().first) / |
139 |
|
|
(timeRange().second - timeRange().first)); |
140 |
|
|
PathPtr_t result = TimeConstraintPath::create( |
141 |
|
|
device_, q1, q2, l, constraints(), pathDofRank_, tds_); |
142 |
|
|
return result; |
143 |
|
|
} |
144 |
|
|
|
145 |
|
|
DevicePtr_t TimeConstraintPath::device() const { return device_; } |
146 |
|
|
|
147 |
|
|
void TimeConstraintPath::checkPath() const { |
148 |
|
|
Configuration_t initc = initial(); |
149 |
|
|
Configuration_t endc = end(); |
150 |
|
|
vector_t errr; |
151 |
|
|
updateConstraints(initc); |
152 |
|
|
hppDout(notice, "Check path, init = " << pinocchio::displayConfig(initc)); |
153 |
|
|
hppDout(notice, "Check path, end = " << pinocchio::displayConfig(endc)); |
154 |
|
|
|
155 |
|
|
if (constraints()) { |
156 |
|
|
if (!constraints()->isSatisfied(initial(), errr)) { |
157 |
|
|
device_->currentConfiguration(initc); |
158 |
|
|
device_->computeForwardKinematics(); |
159 |
|
|
hppDout(notice, "Ini com : " << device_->positionCenterOfMass() |
160 |
|
|
<< " error : " << errr); |
161 |
|
|
// std::cout << "init conf " << device_->positionCenterOfMass() << "\n |
162 |
|
|
// error \n" << errr << std::endl; |
163 |
|
|
/*device_->currentConfiguration(initc); |
164 |
|
|
device_->computeForwardKinematics(); |
165 |
|
|
std::cout << "rf_foot_joint " << std::endl; |
166 |
|
|
std::cout << |
167 |
|
|
device_->getJointByName("rf_foot_joint")->currentTransformation().getTranslation() |
168 |
|
|
<< std::endl; |
169 |
|
|
|
170 |
|
|
std::cout << "lf_foot_joint " << std::endl; |
171 |
|
|
std::cout << |
172 |
|
|
device_->getJointByName("lf_foot_joint")->currentTransformation().getTranslation() |
173 |
|
|
<< std::endl; |
174 |
|
|
|
175 |
|
|
std::cout << "rh_foot_joint " << std::endl; |
176 |
|
|
std::cout << |
177 |
|
|
device_->getJointByName("rh_foot_joint")->currentTransformation().getTranslation() |
178 |
|
|
<< std::endl; |
179 |
|
|
|
180 |
|
|
std::cout << "lh_foot_joint " << std::endl; |
181 |
|
|
std::cout << |
182 |
|
|
device_->getJointByName("lh_foot_joint")->currentTransformation().getTranslation() |
183 |
|
|
<< std::endl;*/ |
184 |
|
|
hppDout(error, |
185 |
|
|
"Initial configuration of path does not satisfy the constraints" |
186 |
|
|
<< pinocchio::displayConfig(initial())); |
187 |
|
|
throw projection_error( |
188 |
|
|
"Initial configuration of path does not satisfy " |
189 |
|
|
"the constraints"); |
190 |
|
|
} |
191 |
|
|
updateConstraints(endc); |
192 |
|
|
if (constraints() && !constraints()->isSatisfied(end(), errr)) { |
193 |
|
|
// std::cout << "end conf " << initc << std::endl; |
194 |
|
|
device_->currentConfiguration(endc); |
195 |
|
|
device_->computeForwardKinematics(); |
196 |
|
|
hppDout(notice, "End com : " << device_->positionCenterOfMass() |
197 |
|
|
<< " error : " << errr); |
198 |
|
|
hppDout(error, |
199 |
|
|
"End configuration of path does not satisfy the constraints" |
200 |
|
|
<< pinocchio::displayConfig(end())); |
201 |
|
|
throw projection_error( |
202 |
|
|
"End configuration of path does not satisfy " |
203 |
|
|
"the constraints"); |
204 |
|
|
} |
205 |
|
|
} |
206 |
|
|
} |
207 |
|
|
} // namespace interpolation |
208 |
|
|
} // namespace rbprm |
209 |
|
|
} // namespace hpp |