1 |
|
|
// |
2 |
|
|
// Copyright (c) 2017 CNRS |
3 |
|
|
// Authors: Fernbach Pierre |
4 |
|
|
// |
5 |
|
|
// This file is part of hpp-rbprm |
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/rbprm/dynamic/dynamic-validation.hh> |
21 |
|
|
#include <hpp/rbprm/planner/rbprm-node.hh> |
22 |
|
|
#include <hpp/rbprm/rbprm-device.hh> |
23 |
|
|
#include <hpp/util/debug.hh> |
24 |
|
|
#include <hpp/util/timer.hh> |
25 |
|
|
namespace hpp { |
26 |
|
|
namespace rbprm { |
27 |
|
|
|
28 |
|
|
DynamicValidationPtr_t DynamicValidation::create(bool rectangularContact, |
29 |
|
|
double sizeFootX, |
30 |
|
|
double sizeFootY, double mass, |
31 |
|
|
double mu, |
32 |
|
|
core::DevicePtr_t robot) { |
33 |
|
|
DynamicValidation* ptr = new DynamicValidation(rectangularContact, sizeFootX, |
34 |
|
|
sizeFootY, mass, mu, robot); |
35 |
|
|
return DynamicValidationPtr_t(ptr); |
36 |
|
|
} |
37 |
|
|
|
38 |
|
|
bool DynamicValidation::validate( |
39 |
|
|
const core::Configuration_t& config, |
40 |
|
|
core::ValidationReportPtr_t& validationReport) { |
41 |
|
|
hppDout(notice, "Begin dynamic validation"); |
42 |
|
|
// hppStartBenchmark(DYNAMIC_VALIDATION); |
43 |
|
|
// test if the same number of ROM are in collision : |
44 |
|
|
core::RbprmValidationReportPtr_t rbReport = |
45 |
|
|
dynamic_pointer_cast<core::RbprmValidationReport>(validationReport); |
46 |
|
|
if (!rbReport) { |
47 |
|
|
hppDout(error, "error while casting the report"); |
48 |
|
|
// hppStopBenchmark(DYNAMIC_VALIDATION); |
49 |
|
|
// hppDisplayBenchmark(DYNAMIC_VALIDATION); |
50 |
|
|
return false; |
51 |
|
|
} |
52 |
|
|
if (lastReport_->ROMReports.size() != rbReport->ROMReports.size()) { |
53 |
|
|
// hppDout(notice,"dynamic validation : rom report not the same size"); |
54 |
|
|
// hppStopBenchmark(DYNAMIC_VALIDATION); |
55 |
|
|
// hppDisplayBenchmark(DYNAMIC_VALIDATION); |
56 |
|
|
return false; |
57 |
|
|
} else { |
58 |
|
|
// hppDout(notice,"dynamic validation : rom report have the same size"); |
59 |
|
|
} |
60 |
|
|
bool sameContacts(true); |
61 |
|
|
|
62 |
|
|
for (std::map<std::string, |
63 |
|
|
core::CollisionValidationReportPtr_t>::const_iterator it = |
64 |
|
|
rbReport->ROMReports.begin(); |
65 |
|
|
it != rbReport->ROMReports.end(); ++it) { |
66 |
|
|
if (lastReport_->ROMReports.find(it->first) != |
67 |
|
|
lastReport_->ROMReports |
68 |
|
|
.end()) { // test if the same rom was in collision in init report |
69 |
|
|
// hppDout(notice,"rom "<<it->first<<" is in both |
70 |
|
|
// reports"); |
71 |
|
|
if (lastReport_->ROMReports.at(it->first)->object2 != |
72 |
|
|
it->second->object2) { |
73 |
|
|
// hppDout(notice,"detect contact change for rom : "<<it->first); |
74 |
|
|
sameContacts = false; |
75 |
|
|
break; |
76 |
|
|
} else { |
77 |
|
|
// hppDout(notice,"rom : "<<it->first<< " have the same contacts in both |
78 |
|
|
// report"); |
79 |
|
|
} |
80 |
|
|
} |
81 |
|
|
} |
82 |
|
|
// if !sameContact, compute new contacts infos and test acceleration |
83 |
|
|
if (sameContacts && initContacts_) { |
84 |
|
|
hppDout(notice, "initial contacts still active"); |
85 |
|
|
// hppStopBenchmark(DYNAMIC_VALIDATION); |
86 |
|
|
// hppDisplayBenchmark(DYNAMIC_VALIDATION); |
87 |
|
|
return true; |
88 |
|
|
} |
89 |
|
|
size_t configSize = config.size(); |
90 |
|
|
|
91 |
|
|
if (sameContacts) { // new contacts already computed |
92 |
|
|
if (config.segment<3>(configSize - 3) == lastAcc_) { |
93 |
|
|
hppDout(notice, "this acceleration is already verified"); |
94 |
|
|
// hppStopBenchmark(DYNAMIC_VALIDATION); |
95 |
|
|
// hppDisplayBenchmark(DYNAMIC_VALIDATION); |
96 |
|
|
return true; |
97 |
|
|
} else { // new acceleration, check if valid |
98 |
|
|
lastAcc_ = config.segment<3>(configSize - 3); |
99 |
|
|
bool aValid = sEq_->checkAdmissibleAcceleration(H_, h_, lastAcc_); |
100 |
|
|
hppDout(notice, "new acceleration : " << lastAcc_.transpose() |
101 |
|
|
<< ", valid = " << aValid); |
102 |
|
|
// hppStopBenchmark(DYNAMIC_VALIDATION); |
103 |
|
|
// hppDisplayBenchmark(DYNAMIC_VALIDATION); |
104 |
|
|
return aValid; |
105 |
|
|
} |
106 |
|
|
} else { // changes in contacts, recompute the matrices and check the |
107 |
|
|
// acceleration : |
108 |
|
|
initContacts_ = false; |
109 |
|
|
hppDout(notice, |
110 |
|
|
"new contacts ! for config = " << pinocchio::displayConfig(config)); |
111 |
|
|
lastAcc_ = config.segment<3>(configSize - 3); |
112 |
|
|
lastReport_ = rbReport; |
113 |
|
|
core::ConfigurationPtr_t q = |
114 |
|
|
core::ConfigurationPtr_t(new core::Configuration_t(config)); |
115 |
|
|
core::RbprmNode node(q); |
116 |
|
|
node.fillNodeMatrices(rbReport, rectangularContact_, sizeFootX_, sizeFootY_, |
117 |
|
|
mass_, mu_, robot_); |
118 |
|
|
sEq_->setG(node.getG()); |
119 |
|
|
h_ = node.geth(); |
120 |
|
|
H_ = node.getH(); |
121 |
|
|
// hppDout(notice,"fill matrices OK, test acceleration : "); |
122 |
|
|
|
123 |
|
|
// test the acceleration |
124 |
|
|
bool aValid = sEq_->checkAdmissibleAcceleration(H_, h_, lastAcc_); |
125 |
|
|
hppDout(notice, "new acceleration : " << lastAcc_.transpose() |
126 |
|
|
<< ", valid = " << aValid); |
127 |
|
|
// hppStopBenchmark(DYNAMIC_VALIDATION); |
128 |
|
|
// hppDisplayBenchmark(DYNAMIC_VALIDATION); |
129 |
|
|
return aValid; |
130 |
|
|
} |
131 |
|
|
} |
132 |
|
|
|
133 |
|
|
void DynamicValidation::setInitialReport( |
134 |
|
|
core::ValidationReportPtr_t initialReport) { |
135 |
|
|
core::RbprmValidationReportPtr_t rbReport = |
136 |
|
|
dynamic_pointer_cast<core::RbprmValidationReport>(initialReport); |
137 |
|
|
if (rbReport) { |
138 |
|
|
lastReport_ = rbReport; |
139 |
|
|
initContacts_ = true; |
140 |
|
|
} else |
141 |
|
|
hppDout(error, "Error while casting rbprmReport"); |
142 |
|
|
} |
143 |
|
|
|
144 |
|
|
DynamicValidation::DynamicValidation(bool rectangularContact, double sizeFootX, |
145 |
|
|
double sizeFootY, double mass, double mu, |
146 |
|
|
core::DevicePtr_t robot) |
147 |
|
|
: rectangularContact_(rectangularContact), |
148 |
|
|
sizeFootX_(sizeFootX), |
149 |
|
|
sizeFootY_(sizeFootY), |
150 |
|
|
mass_(mass), |
151 |
|
|
mu_(mu), |
152 |
|
|
robot_(dynamic_pointer_cast<pinocchio::RbPrmDevice>(robot)), |
153 |
|
|
sEq_(new centroidal_dynamics::Equilibrium( |
154 |
|
|
"dynamic_val", mass, 4, centroidal_dynamics::SOLVER_LP_QPOASES, true, |
155 |
|
|
10, false)) { |
156 |
|
|
assert(robot_ && "Error in dynamic cast of problem device to rbprmDevice"); |
157 |
|
|
hppDout(info, |
158 |
|
|
"Dynamic validation created with attribut : rectangular contact = " |
159 |
|
|
<< rectangularContact << " size foot : " << sizeFootX); |
160 |
|
|
hppDout(info, "mass = " << mass << " mu = " << mu); |
161 |
|
|
} |
162 |
|
|
|
163 |
|
|
} // namespace rbprm |
164 |
|
|
} // namespace hpp |